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.

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 Walls 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

• TODO: Add gif from simulations for multiple rooms

The actual 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.

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.

Results and learning points

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 tree is generated of all possible paths in case there wouldn't be any obstructions (e.g. a closed door). To create this tree a minimal amount of checkpoints are wisely located on the map,visualized as the green dots in the Test Map. Once the tree is created and PICO has localised itself correctly the actual path planning can start.

For PICO's path planning a Dijkstra Algorithm is used which makes use of checkpoints.