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

Results and learning points