Embedded Motion Control 2018 Group 1

Group Members

TU/e Number	Name	E-mail
1279602	Ahmed Hizar (A.H.) Ahmed Ajmal	a.h.ahmed.ajmal@student.tue.nl
0914013	Jari (J.J.) van Steen	j.j.v.steen@student.tue.nl
0924842	Nazar (N.) Rozkvas	n.rozkvas@student.tue.nl
1279491	Arpit (A.) Aggarwal	a.aggarwal@student.tue.nl
1031018	Willem (W.) Verhoeven	w.b.verhoeven@student.tue.nl

Escape room

This part describes all of our progress in the first challenge of the course, which is the Escape Room challenge.

Initial design

Before the software could be created that makes an escape possible, an initial design report was created to assist in the design of the software. This initial design report can be found here: Initial design report group 1. The most important points from this report will be given on the wiki page.

Requirements & Specifications

The first step in the design process is the creation of requirements for the software, which gives a way to express PICO's required behaviour in the Escape room challenge. To translate this into software, the requirements have to be specified, which is done via specifications. These specifications are used in the software design. Both the requirements and corresponding specifications are given here:

Requirements	Specifications
PICO has to avoid all walls at any time.	PICO should always stay always stay away from walls at least 15 centimeters.
	PICO should aim for an equal distance to the right and left when moving through a corridor between 0.5 and 1.5 meters.
PICO should complete its task well independent of the initial conditions.	PICO has to scan its surroundings with a frequency of at least 5 Hz while moving to prevent bumping into a wall.
PICO should operate fully autonomously.	The final software should not make use of teleoperation.
PICO cannot translate or rotate faster than prescribed.	The maximum translational velocity of PICO is 0.5 m/s and the maximum rotational velocity of PICO is 1.2 rad/s.
PICO has to deal with small imperfections in the real world it operates in.	An uncertainty margin of 0.05 meter should be present when building a world model to deal with imperfections in the real world.
The software should be easy to set up.	The software should be set up according to the information on the wiki page.
PICO has to cross the finish line as fast as possible within the maximum time of 5 minutes.
PICO needs to stop when it crosses the finish line.	PICO needs to stop after it has entered the corridor and there is more than 1 meter distance to the left and to the right
PICO has to identify the exit of the escape room.	An algorithm is created which ensures that PICO identifies an exit between 0.5 and 1.5 meters wide when facing the exit under an angle of 60 degrees or less.
PICO has to keep looking for the exit if it cannot find the exit.	PICO will drive towards the point furthest away from at that moment if the exit is not detected.

Escape strategy

To escape the room as fast as possible for this challenge, a plan has been created on geometric level that makes PICO escape as fast as possible. This plan will be described here by first explaining the plan at the highest level and explaining some of the steps in more detail afterwards.

Overlaying plan

The highest level of this plan will first be described by a simple step-by-step algorithm.

1. Initialize the sensors and actuators.
2. Scan surroundings from the given position and orientation.
3. Try detecting the exit.
4. IF exit is found: Drive towards exit and skip to step 8. ELSE: Turn 180 degrees and perform step 2 and 3 once again.
5. Try detecting the exit.
6. IF exit is found: Drive towards exit and skip to step 8. ELSE: Move into the direction of the furthest point for a quarter of this distance.
7. Return to step 2.
8. Identify the 2 corridor walls and orient the robot to face the exit, then drive forward while remaining in the center.
9. Stop once the finish line has been crossed.

The details of the steps performed in this plan are written below.

Detection plan

Some addition explanation on steps described in the overlaying plan will be described now, starting with the explanation on how to detect the exit and the data in the corridor of the maze.

Line fitting algorithm

As a groundwork of the detection methods, a line fitting algorithm has been created. This is required to localize the exit as well as detecting the walls of the corridor of the maze. This line fit will be based on saved data from the LRF that has been transformed to cartesian coordinates. The line fit algorithm tries to fit a line between 2 different points of the LRF data. To do this, the data between the first and last point is averaged into a smaller number of points. Between the first and last of these averaged points, a line is fitted, which is easy to do for 2 points. By checking that the root mean square of the distances from the other averaged points is smaller than a certain threshold, it is determined whether the initial points form a line. This threshold value is based on the total number of points and the number of averaged points. If this linefit does work, the mathematical formula of the line as y = ax + b can be passed.

Exit detection

A very important task in the overlaying plan is to detect the exit from a certain given orientation. The algorithm on how to do that, using the previously described line fitting algorithm, is described now. The exit detection starts by trying to make a line fit between the first LRF data point and around 30 points away. This number of 30 has been determined by looking at the size of the room and the total amount of LRF data points. If a line can be fit correctly between these points, it means that a wall is detected. If it cannot create a good line fit, no wall is detected.

When a wall has been found, the next step is to extrapolate this line, and check if the distance between this line and the points of the LRF data exceeds a certain threshold at some point for a minimum amount of points in a row. If this is the case, there is either a corner or the exit located here. If the distance from PICO to these points increases, this means that the exit is found, while a corner is found if the distance from PICO to these points decreases. If the exit is located, only one point of the exit is known. Therefore, the line will be extrapolated even more, until the LRF data gives new value that fit on the same line. When this happens, the second exit point has been found, and the location of the exit is fully known. The knowledge of finding a corner is largely ignored for this challenge, the only benefit of knowing the location of the corner is that a new linefit can be made starting at this corner point.

Driving plan

Once the exit has been detected, a plan for driving towards the target needs to be made, this plan depends on the required task. The different driving tasks will be expanded upon here.

Driving towards exit

In order to drive towards the exit, the coordinate in the center of the two corner points of the corridor is used as a setpoint. PICO will drive towards this setpoint by turning until the exit is faced and after that, continue to drive forward in an open-loop controlled way. However, since errors can be made in this way, this process is continuously executed when finding the exit. This means that a loop will be executed in which the exit is continuously detected, after which PICO faces the exit and drives forward. This process is executed until PICO has reached a certain distance (20 centimeters) of the exit.

Driving through corridor

If the corridor is entered, a different driving plan applies, where the aim is to drive through the center of the wall. In this case, the detection algorithm provides the data of the two lines corresponding to the walls where PICO has to drive through. The next setpoint that PICO drives towards is a small distance (30 centimeters) in front of the current location, and is located in the center of the two walls. Apart from driving towards this point, PICO also aligns its orientation with the two walls. This process is looped until it has been detected that PICO escaped the corridor.

Driving towards furthest point

When no exit can be detected from the current position, PICO has to move towards the furthest point for one quarter of the total distance. This means PICO drives towards the point that is furthest away from the current position based on 2 scans, one from the initial orientation and one from the orientation after a turn of 180 degrees. This point is supplied as a setpoint to which PICO will drive in an open-loop controlled way. Opposite to the other two driving methods, this setpoint is not continuously updated, as the accuracy of moving exactly towards this setpoint is much less important. This means one simple setpoint is supplied, and the task is finished once driving towards this setpoint is finished.

Program structure

The program has been built according to the Initial Design (Initial design report group 1) having the following 3 structural blocks: Detection, Planning and Control. Each of these correspond to a separate class, connected via an additional class 'main'. The functionality of each class is described in more detail below.

State flags

Execution of the program is sequential, such that the methods are called one-by-one depending on the state of the robot. The states are controlled by boolean flags, which include:

• turn - a flag set to "true" by the planning block when no exit is detected and we wish to turn around to examine the other half of the room. It is unchecked when the control block completes the turn;

• drive_frw - a flag set to "true" by the planning block when exit is detected a PICO has to move in that direction or when no exit is detected and the robot should move in the direction of the furthest point detected;

• turned_once - a flag set to true by the planning block after the first turn in the case exit is not detected form the original orientation. This flag ensures that no continuous turning occurs. The value is set to "false" when drive_frw is set to "true";

• in_corridor - a flag set in the main function when the execution cycle for moving towards the exit it completed. This assumes that PICO is at the exit and a different algorithm for setpoints generation is executed in the planning block;

• escaped - a flag set by the detection block, when no reasonable data is detected after passing through the corridor. As this flag is turned to true, the program should stop executing.

Program classes

The whole program consisted of 4 C++ classes:

main.cpp

• This is the main function where the program starts.
• It initializes objects that are required for the hardware components interaction and program execution (e.g. emc::Rate, emc::IO).
• It initializes the objects storing the sensor data, setpoint for the control loop and the furthest detected point. The later one has the same type as setpoints, however initialized separately since the furthest point is updated every iteration of the main while loop.
• Inside the while loop, the three classes are executed and are constantly updated (Detection, Planning, Control).

detection.cpp

• This is the class that reads data from the sensors, filters it and does detection of primitive shapes and their interpretation. Output is wall line equation, corner points of the exit and one point for the furthest detected distance, with corresponding boolean flags
• Detection file includes methods to detect the walls, find exit and furthest point via line-fitting algorithm.
• The line fitting algorithm provides the equation of a line.
• Method to find exit compares the fitted lines with individual measurements, looking for large neighboring differences in the laser data.
• Method to find walls assumes that the data from the front and the sides of the robot should represent lines. This is verified by searching for laser data with distance closer than a specified value. It then fits the line over those points and spits them to the planning block.
• Method for the furthest point searches for the laser scan with the largest distance data.

planning.cpp

• This is the class that interprets the data and sets the setpoints for movement. The output is relative angle to the setpoint and distance to it.
• It fully exploits the flag commands mentioned earlier and bases its decision on them.
• There are two algorithms between which, the distinction is done depending on the phase that the PICO is in.
• Room logic method checks all the data provided by the detection class and interprets it in destination points.
• A series of corridor following commands access the corridor data when the flag in_corridor is checked. It sets a setpoint based on the two parallel lines describing the walls, keeping the robot in the middle of the corridor

driveControl.cpp

• This is the class that converts the setpoints to the commands to the actuators and tracks their execution.
• It heavily relies on the odometry data.
• Based on the flags turn or drive_frw it executes the functions for speed regulation till the odometry data states that PICO is at the desired position.

Simulation

With the program described above, PICO was able to escape the room in the simulation.

Escape room challenge

Unfortunately, the program for the Escape room challenge was not written perfectly and PICO have failed the challenge, not being able to find the exit. As it was found later, the reason for that is the difference execution of the drive controls in the simulator and on the robot. The simulator was built in such a way, that PICO does not stop moving is a given direction till a command is changed. However, the real robot executed the drive commands only for a certain period of time and then it stops.

As a result, during the first operation stage facing the wall, it did not complete the full turn to scan the surroundings and proceeded to the next stage, moving to the furthest point. The furthest point was not selected properly and PICO have hit the wall.

Another conclusion that our group has made is that there should be a direct repulsion algorithm implemented such that in case PICO is located too close to the wall, the first command would be to move from the wall to the safe (predefined) distance and maintain it during the whole challenge. Only if this condition is achieved, the regular program can be run.

Hospital Room Challenge

corridor simulation with an exit on the right side of the corridor.