Embedded Motion Control 2019 Group 1

Group members

1. Toos van Gool - 0885992
2. Paul Janssen - 1273507
3. Jochem Manders - 0858988
4. Max van Meer - 0951669
5. Raoul Surie - 0810262

Design Document

An initial design report was created to assist in the design of the software, which can be found here: Initial Design Report Group 1. The parts from the report can also be found here on this wiki.

Introduction

The information structure proposed in this document is used to design the software of an autonomous robot, named PICO. PICO has to complete an escape room challenge and a hospital room challenge. To ensure good performance in these challenges, the requirements and specifications are defined initially. Afterwards, the hardware and software components are identified and the functions of the software components are defined. Finally, the interfaces between the components and functions are explained.

Requirements

Table 1: Requirements of PICO

Requirements	Specifications
PICO should execute all tasks autonomously.	Once PICO is started, no interaction is allowed.
PICO should perform all tasks without bumping into a wall.	The forward distance of PICO with respect to an object should always be at least 15 cm, sideways contact is not allowed.
Minimize oscilation of PICO to ensure correct sensor data.	PICO’s acceleration profile should be smooth.
PICO cannot exceed speed limitations.	PICO’s maximum translational velocity is 0.5 m/s.
	PICO’s maximum rotational velocity is 1.2 rad/s.
PICO should be aware of its surroundings.	PICO should create and update a (local) map of its surroundings.
PICO should minimize its stationary time.	PICO should not stand still for longer than 30 seconds.
PICO should terminate when the objective is reached.	ERC: PICO should stop when its rear wheels have crossed the finish line.
PICO should fulfill its objective as fast as possible.	ERC: PICO should exit the room within 5 minutes.

Components

Hardware

The hardware of PICO consists of the following components:

• Sensors: Laser Range Finder (LRF), and wheel encoders (odometry)

• Actuators: Holonomic base (omni-wheels)

• Computer: Intel i7 running Ubuntu 16.04

Information

The information architecture of PICO consists of the following components

• World model

• Perceptor

• Planner

• Controller

• Monitor

The world model contains the state of all activities on which the other components base their actions. The perceptor functions as a data processor that creates a perception of the world around PICO by interpreting the sensor data. The planner contains a state machine in which the strategy of a process is implemented and plans actions based on this state machine. The controller ensures that PICO executes tasks in a correct manner and the monitor ensures that problematic situations, such as encountering static or dynamic obstacles, are resolved. The manner in which these components communicate among each other is described in the Section on Interfaces.

Functions

Table 2: Functions of PICO: Controllers & Monitors

Function	Description
Controller
OrientInRoom	Determines whether towards move to an exit or a wall.
DriveToWall	Drives PICO towards a predefined wall.
DriveAlongWall	Drives PICO within a certain distance along a wall.
MoveToExit	Drives to a point allocated in front of an exit.
MoveThroughCorridor	Manoeuvers PICO through the middle of a corridor.
AvoidStationaryObstacle	Adjusts trajectory to avoid stationary obstacle.
AvoidDynamicObstacle	Adjusts trajectory to avoid dynamic obstacle.
TerminateActivity	Shuts down PICO when objective is reached.
MoveToTrajectory	Determines a feedforward trajectory to end up at desired trajectory.
Monitor
FindProximityToWall	Updates world model state to avoid objects.
FindConcaveCorner	Adds new concave corner to the world model.
FindConvexCorner	Adds new convex corner to the world model.
TrackTrajectory	Updates world model that actual trajectory deviates too much from desired trajectory.
FindStationaryObstacle	Updates world model with new stationary obstacle.
FindMovingObstacle	Updates world model with new dynamic obstacle.
FindExit	Updates world model state when two concave corners are found in close proximity.
FindCorridor	Upadtes world model state when an exit with extending walls is found in close proximity.
FindCabinet	Updates world model if cabinet is found.
LostExit	Updates world model state if an exit is lost when moving towards one.
LostWall	Updates world model state if a wall is lost when tracking one.

Specifications

A couple of important specifications of PICO are:

• The maximum speed of the robot is limited to ±0.5 m/s translationwise and ±1.2 rad/s rotationwise.

• PICO is 41 cm wide and 35 cm deep.

• The LaserRangeFinder has a maximum measurable distance of 10 m and a scope of ±2 rad, which is divided into 1000 equal parts.

Interfaces

The interfaces between the different components is illustrated below.

The Information Architecture for the escape room challenge.

All components communicate with the world model to ensure that they operate using the same perception of surroundings and from the same tasks. This is realized by allowing the components to perform certain tasks based on the state of the world model. For example, if the planner notices that the exit is found in the escape room challenge, the state is changed to ExitRoom. The change of this state causes the controller, monitor and perceptor to only use the functions relevant to leaving the room. In this case, the functions of the controller could be MoveToExit,MoveToTrajectory and AvoidStationaryObstacle.

The world model thus contains the state on which the components base their events, current location with respect to a certain reference (e.g. the wall besides him or the position within aroom), landmarks currently visible and past landmarks to determine the trajectory, a desired and actual trajectory and the incoming sensor data.

The planner contains both strategies for the challenges. The strategy to be used in the escaperoom challenge is illustrated below. PICO starts in the Orient state to immediately find the exit if the exit is in sight, and otherwise a wall to follow. If a wall is found, the monitor indicates this by updating the world model. The planner notices this event and sets the state of the world model to FollowWall. This process continues until the state ObjectiveComplete is reached.

The state machine for the escape room challenge.

Software Architecture

The information architecture shown in the Initial Design Document is made to a modular software architecture, which can be used for both the Escape Room Challenge and the Hospital Challenge. In this section, this software architecture will be elaborated.

Summary

A modular software architecture was created in which the event loop always remains the same. A plan is seen as a state machine of controllers, which are in turn state machines. These state machines can be constructed in a user friendly manner, using function pointers to prevent code duplication and to keep a clear overview of the robot's strategy. Monitors can be created in an equally modular way. Additionally, all relevant information passes through a central WorldModel. Monitors and controllers that are not relevant to the current state of the Plan state machine are not executed to spare resources. Adding semantic information such as Corners or Checkpoints to the WorldModel is easy and scalable.

User manual

To let the robot do anything, whether it is escaping a room or navigating through a hospital, the following steps should be taken.

Creating the concept

1. Draw the plan on paper as a state machine. Give all states an ID (int), starting from zero. Give all connections (events) an ID as well, starting from zero.
2. Every state of the plan corresponds to a controller. Draw each controller on paper as a state machine. Again give an ID (int) to each state. *Note that the numbering is shared by all controllers, but that it is separate from the plan.* For example, the plan can have states with IDs 0, 1, 2. These three controllers could have states with IDs (0, 1, 2, 3), (4, 5), (6, 7, 8). The same numbering rule holds for connections (events).
3. Every connection (event) of the plan corresponds to a monitor. Write down in pseudo-code what exactly this monitor needs to see from the WorldModel before it triggers an event.

Programming the plan, controllers and monitors

1. Label all the state IDs and connection IDs as macros (all caps) in config.h.
2. Create the plan in plan.cpp. To do this, create a function such as StateMachine getEscapeRoomPlan() that is called by main.cpp. The plan is created as follows.
   1. Initialize a StateMachine object plan with the initial state ID, the amount of states and the amount of connections.
   2. For every state, create a State object with its state ID. Add this State object to the StateMachine object.
   3. For every connection, create a Connection objects with its connection ID, the state ID from which this arrow points, and the state ID to which this arrow points. Add these Connection objects to the StateMachine object.
3. Create the controllers in controllers.cpp. Write a function such as ControllerCollection getTestControllers(emc::IO *io, WorldModel *wm). Then create the controllers as follows.
   1. Initialize an std::Vector<StateMachine> controllers;.
   2. In *the same order* as the definition of the state IDs, for each, controller, create a StateMachine object with its initial state ID, the amount of states and the amount of connections.
   3. For each state in every controller, create a State object. Pass to the constructor the state ID. In addition, pass the name of a void(emc::IO*) function that you write in the same source file. This is the body that will be executed in every iteration of the event loop. For example, if the active state is 'driveBackwards', then the function body could be io->sendBaseReference(-1*MAX_FORWARD_SPEED,0,0);. Lastly, pass to the constructor a pointer to an emc::IO object. Add each State object to its corresponding StateMachine object and add each StateMachine object to the vector controllers.
   4. When all StateMachine objects are made, return a ControllerCollection object made from the vector of Statemachine objects, the ID of the initial controller to be active and the pointer to the WorldModel object.
4. Create the monitors in monitors.cpp. Write a function MonitorCollection getEscapeRoomControllerMonitors(WorldModel *worldModel). Then fill it in as follows. Note that the monitors for the plan are created in a separate (but similar) function as the monitors for the controllers.
   1. Initialize an std::vector<Monitor> monitors; and an std::vector<int> activeIds;. These will contain the monitors for the controllers and the IDs of all active controller monitors.
   2. Create each monitor by passing to the constructor of the Monitor class a pointer to the WorldModel, the ID of the monitor and the name of the monitor function. The monitor function is defined in the same source file and is of the form void mymonitor(WorldModel *wm). This function should look at the WorldModel and execute wm->addControllerEvent(monitorID) (or addPlanEvent for plan monitors). Then add the Monitor object to the vector of monitors and fill the vector of initial active monitor IDs.
   3. Return a MonitorCollection of the Monitor objects and the initial active monitor IDs.
   4. Repeat the above steps for the plan monitors in a separate function.
5. Integrate everything in main.cpp by simply editing the function names to the ones that you just created. The event loop does not have to be edited because of the modular design.

The Escape Room Challenge

The simulation

For the first challenge, we built a wall-follower combined with an exit detector. As can be seen in the simulation gif, PICO follows the wall and corrects itself if it comes too close to a wall. If PICO almost `runs into' a wall, it rotates 90 degrees to the right, and follows the next wall. If it finds an exit, it turns left, and follows the corridor towards the finish line.

Simulation of escape room challenge.

Result of the Escape Room challenge

As can be seen in the gif below, during the first attempt PICO drove into the wall immediately. This has to do with the margins we set on PICO to avoid the walls, which were not set around PICO but only in front of it. Therefore, the margin was not large enough, and it drove into a wall.

The first try of the escape room challenge.

PICO was repositioned, and another try was given. During the second run, PICO drove towards the wall, and turned right when it was 0.3 meters in front of it. It then followed the wall on its left, until it found another wall in front of it. Then it turned right again, to follow the wall on its left. Due to a slight gap in the wall PICO thought it had found the exit, and turned towards it. Since this was not the actual exit, the challenge failed.

The second try of the escape room challenge.

Learning points Escape Room challenge

The outcome of the Escape Room Challenge itself is that the plan was not robust enough. The small gap in the wall made PICO think that the exit was found already, resulting in a collision with a wall and failing the Escape Room Challenge. The overall learning points that can be taken from this are:

• The margins set on PICO should be set a bit larger, since as can be seen in the second .gif above, PICO almost bumped into one of the walls and made the 'referee' shut it down.
• When defining a distance w.r.t. a wall / object, an average of a couple of data points should be taken (so a cone of points) instead of a single datapoint. This to make sure PICO will be able to cope with the real world which is never perfect.

The Hospital Challenge

Strategy

For the hospital challenge, a strategy is established consisting of 4 controllers: Orient, Localize, ExecuteRoute and Terminate. Combining these controllers enables PICO to autonomously execute a route in which an arbitrary number of cabinets are visited. However, these controllers only enable PICO to function correctly for the map and its variations due to static obstacles, and deviations in the provided map versus the real world. The dynamic obstacles are avoided using global monitors, which trigger PICO to enter a state that deals with said obstacles. For now, the plan that does not take dynamic obstacles into account is considered to convey a clear image of the hospital challenge strategy.

At the start of every attempt of the hospital challenge, PICO starts in the Orient state. This state functions as a calibration for PICO to position itself in the most favorable position for the localization, which is the position in which PICO is looking into the room and not to a wall or a corner. The Orient controller thus heavily depends on the conditions of the starting area of PICO in the hospital challenge. This area is given prior to the challenge and consists of an area of one square meter in which PICO can be placed arbitrarily. For this reason, four possibilities are considered: the square meter lies in the middle of a room, against a wall, in a corner (and thus against two walls), or against three walls. In the event that the starting position does not lie against a wall, the Orient controller is redundant and thus eliminated from the strategy. For the other three cases, the Orient controller is illustrated below.

[TO DO: visualize Orient controllers]

Once PICO is oriented, PICO switches to the Localize state. In this state PICO has to fit the points it sees on the map that has been given to PICO before the challenge.

[TO DO: explain localization algorithm]

The Localize controller makes sure that if PICO is not able to localize itself in its current position, it wil start moving forward and localize during this forward motion. PICO will keep moving forward until it finds a wall in front. Then, it will make a 180 degree turn and start this process again until it is able to localize itself.

When PICO has localized itself, it plans a route based on its current position, the position of the cabinet it has to visit next, and all the checkpoints in between. The checkpoints are visualized as the green dots in the Test Map. In case multiple routes are possible, e.g. due to the fact that a room has multiple doors through which PICO can enter, PICO picks the route with the least amount of checkpoints.

Once PICO is localized and its route is planned, its state switches to the ExecuteRoute state. In this state, PICO will follow the route it has planned along the cabinets.

The ExecuteRoute controller starts with PICO rotating left, until the checkpoint it needs to visit next is in front of it. Then, if PICO does not see any obstruction in between itself and the next checkpoint, it simply moves towards it. If PICO notices obstruction between itself and the checkpoint, it moves forward towards the obstruction until it is almost against it. Then, PICO rotates left and aligns itself along the global X or Y direction depending on the amount of room that is present on either side of the obstacle. PICO moves along this global X or Y direction until the obstacle is out of its sight and thus has positioned itself next to the obstacle. Then, PICO drives along the the other global X or Y direction until it has passed the obstacle. Then, it again rotates left, until the checkpoint it needs to visit next is in front of it.

If the checkpoint reached is not the final checkpoint, PICO again starts rotating left, until the checkpoint it needs to visit is in front of it. If the checkpoint reached is the final checkpoint, it has reached the cabinet, and thus has to position itself in the area in front of the cabinet by rotating towards the cabinet. If PICO is correctly positioned in front of the cabinet, it makes a sound signal to indicate that it has reached the cabinet.

If the cabinet reached is not the final cabinet, PICO switches to the Localize controller again, to localize again, and plan its route towards the next cabinet. When PICO has reached the final cabinet, it gives a final sound signal, indicating that it has succesfully carried out its assignment.

Test map

Path Planning

Before the actual Path is planned a graph is generated of all possible paths in case there wouldn't be any obstructions (e.g. a closed door). To create a graph a minimal amount of checkpoints are wisely located on the map, visualized as the green dots in the Test Map. Once the graph is created and PICO has localised itself correctly the actual path planning can start. The graph for the hospital challenge is shown below

For PICO's path planning a Dijkstra Algorithm is used. The algorithm returns the path with the least amount of checkpoints. Another important feature is that the path can be updated in case an obstruction is detected. In that case, PICO will chose a new path based on the least amount of checkpoints. Once PICO knows a certain checkpoint transition is unreachable this knowledge will be used for all future paths.

Localization

Figure 1: Comparison extracting corners with/without AD-filter

PICO's localization is build up from two main blocks namely the extraction of corners using LRF data and using an unconstrained optimization problem.

First, the corner extraction. Initially for the escape room challenge a very simple local peak finder on the LRF data was used to extract corner points. Based on a local minimum/maximum it was determined whether it's a convex/concave corner. However, this after the first challenge this method seemed not sufficient. This is visualised in upper part of Figure 1.

In the literature [1] a geometric approach is discussed in order to robustly detect corners (and extendable to features). For convenience the method is referred to as the 'AD-filter'. The AD-filter uses purely the range [math]\displaystyle{ r_i }[/math] at every index of the LRF data. The equation for the metric-value AD shows that it evaluates the range of indices left and right of the current index. If index [math]\displaystyle{ i }[/math] is located on a straight line without any corners in the vicinity of [math]\displaystyle{ k }[/math]-indices then AD(i)[math]\displaystyle{ \approx }[/math]0. However, when closing in on a corner at index [math]\displaystyle{ i }[/math] then at some point (i-k)< j <(i + k) the trend of the range of the indices is not continuous. Therefore, AD(i) deviates from zero. If index [math]\displaystyle{ i }[/math] is exactly on a corner then the deviation from zero reaches a local minimum/maximum (see Figure 1). Minima smaller than zero represent concave corners, maxima greater than zero represent convex corners. Sequence of values approximately zero represent straight line segments.

In order to determine the minima and maxima of the AD-filtered data a highly modified version of the following peak-finder algorithm is used [2]. It enables to set upper and lower bounds for convex and concave peaks in the AD-filtered data because real-life LRF data is very noisy. For example, the following additional features are build-in to increase robustness and enhance selection of actual corners. A feature to detect corners at the end of line-segments. Another feature that clusters a set of corners to a single corner in the event of multiple corners within a certain Euclidean range (currently set to 0.1 [m]). As well as a feature that ignores corners that are placed at the end of the LRF array.

The benefit of the proposed method for extracting corners ended up being very robust and very fast. The only downside is that in order to get it working properly quite a few parameters (five) have to be tuned in order to get a desirable result. Finally, the parameters have been tuned such that only corners with a high confidence rate are extracted. Because, extracting corners that in the real-world aren't actual corners will result in issues for the localization algorithm explained hereafter.

As mentioned before, for the localization of PICO an uncontrained optimization problem is solved. This is done with a L-BFGS algorithm, the optimization toolbox dlib [3] is used to minimize an objective function. The localization problem is a combination of odometry data, previous localized position and worldmodel perception (the extracted corners defined earlier).

First, at the starting pose of PICO a global origin is placed. As soon as PICO starts to drive in any direction the odometry data is used to estimate a new position. PICO's perception of the environment changes, the percepted corners are plotted on a global scale using the algorithms initial value. The initial value for the algorithm is a combination of the last-known location and the odometry data. The problem is solved for a vector x = [dx dy d[math]\displaystyle{ \theta }[/math]]' which corresponds to the required x,y,[math]\displaystyle{ \theta }[/math] in order to get from the global origin to PICO's location.

For an m-amount of percepted corners the distance to an n-amount of total corners on the 'known' map are calculated. Resulting in an mxn-matrix, taking the minimum value of every row gives the distance between a percepted corner and it's closed pairing corner on the real map. Summing the quadratic minimum distance of all percepted corners results in the objective function. By using the L-BFGS the quadratic minimum distance of all percepted corners are minimised such that a local minima is reached. By reaching a minima a solution is found for [math]\displaystyle{ x }[/math]. However, the objective function is not convex, therefore there are multiple local minima that can be acquired.

In order to deal with multiple local minima the localization is to be executed every timestep (algorithm is solved in approx. 1 ms) such that based on the amount of percepted corners a maximum threshold is set that throws away a solution if the objective function value is above the threshold. In Figure 2 is shown with multiple coordinate systems how the algorithm works.

Figure 2: Visualization of localization procedure

Obstacle Avoidance

Because the hospital map contains both static and dynamic obstacles, an obstacle avoidance controller is designed. This controller prevents PICO from bumping into the obstacles by performing different tasksets depending on the nature of the obstacle. The controller is initialized when PICO detects an obstacle when it is rotated or driving towards the next checkpoint of its trajectory. In the event that PICO detects an obstacle when rotated towards its checkpoint, monitor 2 causes PICO to switch to state 6, which lets PICO drive up against the obstacle. Then, it goes in idle mode for 5 seconds to determine whether the obstacle is of static or dynamic nature.

If the obstacle has not moved after 5 seconds, the average of the data front left of PICO is compared to the average of the data front right of PICO. Afterwards, PICO moves to the side of which the average is the highest, indicating the most space as the sensor can see the furthest and thus the highest chance of avoiding the obstacle. PICO then keeps driving sideways until it has passed the obstacle with a margin, or drives the opposite way in the event that PICO cannot pass the obstacle on the side with the highest average. If the latter occurs, PICO tries to pass the obstacle on the other side plus margin. If it still cannot pass the obstacle then, monitor 8 enables PICO to delete the checkpoint and a new route is planned. If PICO is able to drive sideways untill the obstacle is out of its front sight and the extra margin is driven to ensure that PICO can pass the obstacle safely, it moves forward and checks the data on its left and right side to determine when it has passed the obstacle plus margin. This is monitored by monitor 9, which enables PICO to orient towards the checkpoint once more in the hope that the path towards the checkpoint is now clear.

If the obstacle moves within the 5 seconds in which PICO is idle, high alert mode is enabled. This mode causes PICO to define a safe zone of half a circle with a radius of 40 cm around itself. PICO then remains idle for another five seconds to check whether this obstacle is actually dynamic or that it is caused by sensor noise. If the safe zone became free within the five seconds that PICO was idle, it can be concluded that the obstacle was dynamic as PICO was not moving, so the obstacle was. It then drives forward at a low speed while closely monitoring its safe zone until it encounters another obstacle or is 0.2 metres within a checkpoint. It is also possible that PICO encounters a static obstacle when in high alert mode. For this reason, a failsafe is implemented by monitors 8 and 10 which enable PICO to switch to a state that makes PICO move in the opposite direction of the obstacle in the event that the obstacle has remain static for 5 seconds.

Simulations Hospital challenge

Testing prior to the Hospital challenge

<< TODO: create gifs >>

Since the hospital map was known a priori, during the last week of testing on PICO the hospital map was built on the robocup field. During the testing hours, PICO was able to complete a simplified challenge, as there were no obstacles present on its route.

<< TODO: elaborate on testing resutls >>

Results Hospital challenge

The hospital challenge was the final test for PICO and us to check whether the software could handle the imperfections of the real world. In the simulation, PICO is able to succesfully visit cabinets on the given map in any arbitrary order. It was also able to avoid obstacles and re-plan its route when e.g. a door was closed.

During the first try of the Hospital Challenge PICO oriented itself, planned its route towards the first cabinet and drove towards the door of the first room, into the hallway.

PICO did not care about the dynamic obstacle, since it was not in its direct path, and therefore kept going towards its next checkpoint, at the corner of the corridor.

Then, PICO drove past the obstacle with a slightly smaller margin than expected, but still avoiding collision with the obstacle.

As can be seen, PICO then does not turn towards the door when it passes by it. It moves towards the wall, since its localization is not correct and therefore it does not know where it is. When PICO arrives at the wall, it turns 180 degrees and moves backwards towards the corridor, trying to localize again.

Unfortunately this is not done correctly, and therefore PICO drives into a wall.

After try 1, at our request, the obstacle was removed from the corridor. This would have made the localization for PICO much more likely to succeed, as it did as such in the simulations. When starting PICO again, we did not hit 'pstart' again. This resulted in the odometry data not being reset, and therefore resulting in the orient controller not working correctly. As can be seen below, PICO did not localize correctly, and drove towards the 1st cabinet instead of its route towards cabinet 0.

Learning points Hospital Challenge