Embedded Motion Control 2017 Group 4
Reports
File:Final design presentation.pdf
Group 4
| M.R.M. Beurskens | 0843080 |
| N.H.J. Janssen | 0842075 |
| M.C.W. Schouten | 0869793 |
| J.A.E. Verest | 0844791 |
| M.S. de Wildt | 0776941 |
| R.H.M. Winters | 0854048 |
Initial design
Introduction
In this design chapter, the initial design of a maze solving robot is specified in terms of requirements, specifications, components, functions and interfaces. It is implemented on the PICO hardware platform. A task-skill-motion framework is used in order to describe the robot behaviour. This will also be implemented in C++.
Requirements and specifications
PICO is required to autonomously navigate through a maze and find the exit without bumping into any of the walls. A door will be present in the maze, and must be opened by "ringing the bell" in a suitable location whilst standing still. Open spaces might be present, and the walls are approximately axis aligned. The maze might also feature loops. Therefore PICO is required to do the following:
- Recognize walls and corridors in a robust manner. The walls are axis aligned, however not perfectly. PICO should allow for an alignment error. It also has to stay away from the walls with a sufficient margin (e.g. 10 cm between robot and wall). Bumping into a wall will count as a failed attempt.
- Identify possible "door areas" in a robust manner. "The door will be situated at a dead end, a dead end being defined as a piece of wall with minimum length 0.5 meter and maximum length 1.5 meter with side-walls of at least 0.3 meter length". Alignment errors again need to be taken into account.
- Map the maze to determine whether the area has been visited and to build a world model. Odometry and a Laser Range Finder (LRF) are used to build a world map and determine the location in the world. The odometry and LRF data should be synchronized to avoid drift in world location and mapping (SLAM).
- Recognize corners as nodes in order to build an abstract and simplified maze representation consisting of nodes and lines.
- Cary out a maze solving strategy based on its world model and solve and execute corresponding routing. The solving routine and the routing should be able to handle loops and open spaces and are based on the simplified maze representation. This enables more general strategy composition and faster computation.
- Open doors by ringing a bell in the door area. No part of PICO should be located outside the 1.3 meter range of the door and PICO should be standing still whilst ringing the bell. The door opens completely within 5 seconds after PICOs request. Once the door is completely open, PICO needs to pass the open doorway to find the exit of the maze.
- The software should be easy to set up and deploy, i.e. using git pull and with a single executable.
- Clear the maze as soon as possible, with a total available time of seven minutes for two attempts. A maximum speed of 0.5 m/s translational and 1.2 rad/s rotational is given.
- Do not stand still for more than thirty seconds when inside the maze.
- Recognize when the maze is solved.
Components
Pico features a Laser Range Finder (LRF) to detect surrounding obstacles and wheel encoders to measure odometry data. The wheel base is holonomic, enabling it to turn on the spot and drive in any direction. An emergency button is present on the remote. PICO runs on batteries and should always be charging when not driving around.
The software runs on an Intel i7 processor running Ubuntu 14.04 LTS. The Robotic Operating System (ROS) is used as a framework for PICOs software. However, an intermediate software layer is provided which can be used to access sensor data and actuate the robot. Our own software layer is divided into five main components: actuation, sensing, world model, task manager and skill.
Functions
Configuration file
- Configuration: Defines a number of parameters. This is a header file so no inputs or outputs.
Actuation
- Move: This function calculates the translational velocity in x and y direction and the rotational velocity corresponding to the direction vector and net velocity outputted by the path planning function. The calculated velocities are sent tot the base actuators.
- Input: Direction vector, total velocity, Output: -.
- Stop: Stops the robot by sending the base velocities of magnitude zero.
- Input:-, Output:-.
- Rotate: Rotates the robot 90 or 180 degrees. First checks the robot has enough space to rotate without hitting the wall. If this is not the case, first a call to Move is made to move the robot away from the wall.
- Open door: Makes sure the robot stands still, rings the bell to open the door and waits until the door is opened.
- Input: LRF data, Output: -.
Sensing
- Get LRF data: Gets the data from the laser range finder.
- Input: -, Output: LRF data
- Get odometry data: Gets position data from omni-wheels.
- Input: -, Output: odometry data
World Model
- SLAM: Simultaneous Localization And Mapping, this updates the position of PICO in the world model using new sensor data.
- Input: LRF data, Odometry data, Output: World model
- Object recognition: This function will estimate which objects there are in the World Model using the geometries of the environment.
- Input: World model, Output: Objects with their coordinates.
Task Manager
- Task monitor: Determines which piece of code will be executed next.
- Input: -, Output: -
Skill
- Filter data: Moving average filter to minimize noise.
- Input: noisy data, Output: averaged data.
- Finish: Recognizes that the finish line has been crossed and that Pico can stop searching for the exit. The main script can be terminated.
- Input: World model, Output: -
- Strategy: Gives a general direction based on the maze solving algorithm that is chosen.
- Input: World model data, LRF data. Output: Target point.
- Path planning: Uses an Artificial Potential Field to output a direction vector considering the target point outputted by the strategy function and the LRF data.
- Input: Target point, LRF data. Output: Direction vector
Interfaces
The software functions have multiple inputs and outputs and can be grouped by functionality. If data from the sensing component is used by a function from the skill component, the data is transferred between these components, this is referred to as an interface. A rough overview of the interfaces is shown in Figure 1. The dashed line represents that the actions of the robot will influence what the sensors measure.
Corridor Challenge
Repulsion & Angle correction
The first thing that was considered for the corridor challenge was some kind of repulsion from the walls to make sure Pico does not bump into anything. For the repulsion a simple version of an Artificial Potential Field is considered, which is given by the formula
[math]\displaystyle{ F_x = \frac{1}{b} \frac{1}{N_1} \sum_{i=1}^{N_1} \frac{1}{(r_i-a)^3} cos(\phi _i) }[/math] [math]\displaystyle{ F_y = \frac{1}{b} \frac{1}{N_2} \sum_{i=1}^{N_2} \frac{1}{(r_i-a)^3} sin(\phi _i) }[/math]
where b is a scaling variable that can be used for finetuning, N is the length of the considered laser data vector and \phi_i is the angle corresponding to measured distance distance r_i. Variable a is a correction factor for the width of pico that can also be used during the finetuning.
For the calculation of the force in the forward direction, [math]\displaystyle{ F_x }[/math], only laser data in front of Pico is used. Al the other laser data is used for the calculation of [math]\displaystyle{ F_y }[/math], force in side ward direction. This is also shown in the figure 2. The resulting netto forces are used as an input for the actuation function which is further explained below. Note that if the laser sees something at a very large distance or does not see anything at all, the returned distance is very small or even negative. Therefore all distances smaller then 0.01 meter are set to 10 meter.
For the angle correction all the laser data at the right side of Pico is considered. The smallest distance to the wall and the corresponding angle are found using a for-loop. After subtraction of 90 degrees, the resulting angle is used as input for the desired rotation speed. This approach can be interpreted as a torsional spring connected to Pico and is visualized in figure 3.
Actuation
The main functionality of the actuation script is to scale velocity signals that are sent to Pico in such a way that the constraints on maximum velocities are respected. Besides, effects of angle wrapping are compensated to create useful odometry data to rotate with a certain angle. Scaling of the translational speeds in x and y direction is done using
- [math]\displaystyle{ \vec V_x = \vec F_x \sqrt{\frac{V_{max}^2}{\vec F_x^2 + \vec F_y^2}} }[/math]
- [math]\displaystyle{ \vec V_y = \vec F_y \sqrt{\frac{V_{max}^2}{\vec F_x^2 + \vec F_y^2}} }[/math]
- [math]\displaystyle{ \vec V_x = \vec F_x \sqrt{\frac{V_{max}^2}{\vec F_x^2 + \vec F_y^2}} }[/math]
in which Fx and Fy are the force signals from the repulsion script sent to the actuation script. This is clarified in the figure on the right. If an angular velocity exceeds the maximum value, the angular velocity simply gets scaled back to the maximum angular velocity using
- [math]\displaystyle{ \omega = sign(\omega) \cdot \omega_{max} }[/math]
such that it makes sure that Pico will still turn in the right direction.
When using the actuation script to rotate with a certain angle, the script will in each iteration return a relative rotation angle. The script in which the actuation script is called then has to integrate this relative angle until the desired angle has been rotated. However, the angle from the odometry data can jump from –π to π of vice-versa. If this happens, the relative angle will not be correct and has to be adjusted by adding or subtracting 2π.
World model
Making the turn
Driving between two walls can be easily done using the actuation, repulsion and angle correction code. To actually make a turn at the right moment to drive into the corridor, the corridor has to be recognized. From the world model, corner nodes and end nodes are received. We search for a corner node at the right and left of Pico (light purple area the figure below). If such a corner is found, and the distance to this corner node checked to see if the distance is reasonable (0.1m – 1.5m). Consider a corridor with the exit at the right, then the corner node will recognized at the right. The distance to the closest corner node present in the light green area in the figure below, is used to calculate the width of the corridor using the cosine rule. If no other corner node is recognized in the light green area or the recognized corner node is too far away, the distance to the first corner node (denoted with the number 2 in the figure below) is taken as the corridor distance.
To take the corner, half of the corridor distance b is driven forward, during which the repulsion in forward direction is not used. After traveling [math]\displaystyle{ \frac{b}{2} }[/math] meters, Pico stops and rotates 90 degrees. Then a distance of [math]\displaystyle{ c+e }[/math] is driven forward, where c is the distance as shown in the figure below, and e is a variable that can be used for tuning. This distance is driven forward to prevent Pico from recognizing corner node 2 again at his right and starts turning gain before the corner is completely taken. For a corridor at the left, the exact same approach is used.
Result
Final design
Design decision
To make a functioning robot, decisions have to be made with regard to maze solving algorithms. This chapter will focus on why and how the decisions are made. Three options were considered; the pledge algorithm, Trémaux’s algorithm and the random mouse algorithm.
The pledge algorithm lets the robot follow the wall either at its right-hand side or left-hand side (depending on the decision made). Whilst following the wall on its right/left-hand side, it counts the amount of corners taken. The counter is updated by adding 1 for a right turn and subtracting 1 for a left turn. If the robot is in its preferred direction (the counter is 0), then the robot will let go of the wall. It continues driving straight forward until it meets a new wall in front of it which it will then start following. The benefit of the pledge algorithm is that the robot should always finish the maze in a finite time without keeping track of a world map. A downside is that this is not necessarily the fastest way to solve the algorithm. The robot may for instance drive past the finish just because it does not look at what is at its left side. Another downside is that it is quite sensitive for errors. The whole pledge depends on the counter, if the counter gets mixed up, it may be possible that the robot will have to travel a lot of extra distance.
Trémaux’s algorithm, in contradiction to the pledge algorithm, does require to keep track of a world map, which makes it a lot more complicated immediately. By marking the points that have already been visited, decisions can be made such that it is guaranteed to find the exit of the maze. This does not necessarily have to be faster than the pledge algorithm. However, during the maze challenge, each group has two attempts. During the first attempt, Trémaux’s algorithm can be used to explore the maze and to find a way to the exit. In the second attempt, Pico can directly drive to the exit, because it already knows what path to follow, assuming that the first attempt was successful.
The final option we considered is the random mouse algorithm. This algorithm makes a random decision at each junction. Therefore the robot will eventually always find the exit of the maze, but this can take a lot of time.
Eventually, the decision has been made to use the pledge algorithm. The main reason to do this is the fact that the complexity is limited, while it is still an efficient way to solve the maze. Even though we established to create robust corner detection it would be a large step to expand this to a full Trémaux’s algorithm. During the corridor challenge we experienced that it is very important to tune the configuration parameters on the actual robot instead of the simulator. Therefore the decision has been made to create a robust realization of a relatively simple algorithm (pledge) instead of a complex algorithm (Trémaux) that only works in the simulator due to the fact that the practical test time is limited because the code is not finished in quickly enough. In other words, our focus is on finishing the maze challenge rather than implementing the most complex algorithm.
Interface
After the corridor challenge, the software architecture is changed a bit. Now the software consists of five main parts: Actuation, skill, sensing, worldmodel and Taskmanager. These different software parts with their corresponding in- and outputs are depicted in Figure 2. The software architecture is globally explained in this section. The important parts are further explained in the following sections.
Taskmanager
The taskmanager is the top part, which controls the whole software. Data comes in via the skill and sensing part, and data is sent to the strategy and actuation part. Based on the data from the other parts, the task manager makes decisions on what to do. First, it calls the variable ‘door detected’ to check whether Pico is in a door area. Then it will go to the initial state ‘start’ in which the startup procedure is implemented. It checks if there is a wall on the right side of Pico, and Pico will be aligned to this wall. When Pico is aligned to this wall, it will go to the following state ‘straight_pref’. In this state, Pico drives in its preferred direction and calls the boolean vector from the space recognition skill. This information is sent to the strategy skill, which sends the direction and state back. Based on this information it will stay in the case ‘straight_pref’ or switch to the state ‘straight’, ‘turn_left’, ‘turn_right’ or ‘turn_180’. When it stays in the same case, it will call the force struct from the repulsion skill. After that, the actuation vector is sent to the actuation part. When it switches to a state ‘straight’, the same is implemented as in the state ‘straight_pref’, but now the distance to the wall is added. This is implemented as a virtual spring, which will be further explained in the Maze solving algorithm section. In the states ‘turn_left’ and ‘turn_right’, the odometry data is used to check whether Pico has rotated enough.
Actuation
The input of the actuation part is an actuation vector, which contains the horizontal, vertical and angular velocity or a door request. The actuation part consists of four sub parts: move, stop, rotate and opendoor. The taskmanager decides which part is called. In the actuation part, the velocity is scaled, such that it does not exceed the maximum velocity. When the normalized velocity is calculated, this is sent to the actuators. When opendoor is called, an ‘opendoorrequest’ is sent to the actuator such that it will ring a bell. Sensing In the sensing part, the odometry and LRF data is obtained. This LRF data goes straight to different parts. By calculating the difference between the previous and the current data, the wrapping of the data is corrected and the relative rotation is calculated.
World Model
The world model is a function that detects nodes using the LRF. Using various algorithms, the data from the LRF can be filtered and the most important data points are saved as nodes. Important nodes concern mostly nodes that lie on the end of walls or that are corners. Using these nodes, the current world that PICO sees can be made. These nodes can also be used to recognize doors.
Skill
The skill part contains the skills of Pico, which are the calculation of the repulsion, the space recognition, the door recognition and the strategy. In the repulsion part, the repulsion forces are calculated using the LRF data. In the space recognition part, it is checked whether there is space to the left, right or front using the LRF data. In the door recognition, the door areas are calculated using the nodes from the World Model. When a Pico is in a door area the Boolean door detected will be ‘true’. The strategy part gets the boolean vector of the spaces from the task manager. Based on this spaces boolean vector, the strategy part will calculate the direction and desired state using the Pledge algorithm.
Visualization
The visualization is not a functional part of pico, but it is only used to visualize what happens in real time. The input is a vector with nodes, which are transformed to dots, lines and numbers in the visualization. It is mainly used to check if the "node detection" and "door recognition" is working correctly.
Space recognition
This function determines whether there is space or an obstacle to the left, right and top of Pico. In the config file, the beam width is determined in degrees. This value is converted to degrees and used to determine which LRF data points should be checked. As can be seen in the figure, there are 3 beams to be measured: left of Pico, above and to the right of Pico. By default, all spaces are defined true. If one measured LRF point is below the threshold value, indicated with an orange circle, the space_x variable will be set false. The threshold for the forward distance can be tuned independently of the others. The forward distance is slightly smaller than the other distances. This means that Pico will detect an obstacle in front of him later and will react later to this. This way a better wallfollowing can be realized. The function will need the scan information from the LRF as an input and will output variables space_left, space_top and space_right.
Maze solving algorithm
The Pledge algorithm is chosen to solve te maze. The pledge algorithm has a preferred direction and counts the corners the robot takes. A corner left counts as -1 and a corner right as +1. When the counter is 0, the robot goes in its preferred direction. The pledge algorithm is implemented in the following manner:
- If Pico is driving in preferred direction:
- if space_top = true: Go straight in preferred direction
- if space_top = false & space_left = true: rotate 90 degrees left
- if space top = false & spacel_left = false & space_right = false: rotate 180 degrees left
- Else
- if space_right = true: rotate 90 degrees right
- if space right = false & space top = true: go straight
- if space_right = false & space_top = false & space_left = true: rotate 90 degrees left
- if space_right = false & space_top = false & space_left = false: rotate 180 degrees left
In Figure 3 Pico is depicted with three beams: space_left, space_right and space_top. All beams are using the 10 degrees of lrf-data.
Repulsion & wall following
How the repulsion and angle correction are working is already explained in the section Corridor challenge. The final repulsion and angle correction are not changed very much in the final design, only some values are tuned. The wall follower is added in the final design, which makes sure Pico stays at a desired distance from the wall. The wall follower is implemented as a virtual spring. When Pico is too close to the wall, the repulsion becomes very high and Pico will be pushed from the wall. When Pico is too far from the wall, the virtual spring will give a force. Therefore, Pico will be pulled to the wall. Between the repulsion and virtual spring is some threshold, such that the forces are zero in that area. This makes sure that Pico does not correct itself the whole time.
World model
In order to recognize door areas a world model is constructed which uses landmark detection. Landmarks are defined as nodes, which are points in the LRF-data with certain properties which are collectively defined as either "end" nodes or "corner" nodes. The world model library provides a function which can be used to detect nodes in the landscape.
/*!
\brief scanNodes Scan for nodes and return a vector of the detected nodes.
\param scan A scan of the environment containing angle and distance data of the LRF.
\return Vector containing nodes detected during the scan.
*/
std::vector<Node> scanNodes(emc::LaserData scan);
The resulting node vector in this case is then used to recognize door areas using the doorCheck function, which returns true when a door area is detected and PICO is located there.
Node class
The Node class (data type) is defined as follows:
- ID (int): The ID of the nodes are set from right to left (in the scanning direction of the LRF) starting at ID 0.
- Type (int): The types are defined in the header file of the world model library worldmodel.h. Two types are currently in use: Cornernodes and Endnodes.
- Location (location): For each node the Location class is used to track the position in both carthesian and radial coordinates relative to PICO. When the location class is updated using the radial coordinates the carthesian coordinates are updated automatically. VOEG FIGUUR TOE ASSENSTELSELS
- Connections (std::vector<int>): A vector of node IDs. For each connected node the ID is stored in this vector. Thus it is possible to determine which nodes are connected by walls and which are not using the Node class.
- LRFindex (int): Index in the LRF scan of the particular data point corresponding to the current node. It is mainly used to determine the connection vector.
Node Recognition
ADD PIECE OF CODE
The world model is based on two types of nodes: endnodes and cornernodes. Finding nodes is done with the following checks over all data-points:
- The current data-point is a cornernode if the distance between the current data-point and the next data-point is larger than a given value.
- The current data-point is a cornernode if is passes the 90-degrees-test. This is a test that considers a previous data-point that lies at a set distance from the current data-point, and a next data-point that also lies at a set distance from the current data-point. Using the cosine rule, angles and distances to the three points, the actual distance between the previous and next datapoint can be compared to the distance between these points when applying Pythagoras. Pythagoras only holds if the angle is 90 degrees, so if the two values are equal, then Pythagoras is applicable and thus the corner is 90 degrees. [math]\displaystyle{ x_r }[/math]
[math]\displaystyle{ A = \sqrt{(x_2-x_1)^2+(y_2-y_1)^2} }[/math]
[math]\displaystyle{ B = \sqrt{(x_3-x_1)^2+(y_3-y_1)^2} }[/math]
- The current data-point is an endnode if the distance between the current data-point and the next data-point is smaller than a given value.
- A data-point that lies at the maximum range or at the maximum angles is considered an endnode.
The visualization of the corner nodes and the end nodes serves to debug and tune the object recognition.
Several tunable parameters are associated with the node recognition:
Wall Recognition
In order to recognize whether nodes are connected the LRF index of the subsequent nodes in is checked in order to see whether nodes are set at subsequent indexes of the LRF data. Two nodes can only have subsequent LRF indexes when the are separated by a large distance because of both the SPIKE check which compares distances, and a tunable parameter which determines the allowed overlap of nodes. Thus the nodes are not connected when the check is true.
Each node is checked in sequence. A connection is thus always added "backwards", connecting the node checked to the previous one by modifying the Connections variable of both.
Door Recognition
The doors are recognized based on the end- and cornernodes. It is known in advance that the node numbers are ordered depending on the angle of the LRF. The first node lies at the lowest recognizable angle, the last node at the highest recognizable angle. Based on this we can do several checks to see if a door should be recognized. The door check is done with a for-loop over the first to the fourth last nodes. This is because four nodes are required to check if it is a door. Let’s consider a certain node that needs to be checked, then the three following nodes are also considered to determine whether these four nodes make a door.
The first check, is a simple check that checks the absolute distances between the four nodes. Let’s consider four nodes that lie after eachother. The distance between the first two nodes has to be at least 0.3 meter according to the given requirements. Then the distance between the second and third node has to be at least 0.5 meter, but can at a maximum be 1.5 meter. The distance between the third and fourth node should once again be at least 0.3 meter. On each of these distances, margins are added to increase robustness. If a node passes this test, then it proceeds to the second check. If it does not pass this test, then it can’t be a door by definition and thus it not necessary to do the other tests.
The second check determines if the first and forth node are on the same side of the line through node 2 and node 3. This is to check if it is actually a corridor and not a S-curve. An example of this is shown in the figure below. The left ‘door’ would pass this test whilst the right one would not. To check if the nodes 1 and 4 lie on the same side, the following equation is used:
[math]\displaystyle{ D = (x-x1)(y2-y1)-(y-y1)(x2-x1) }[/math]
In this equation are x and y are the relative coordinates with respect to the robot of the points that do not lie on the direct edge of the door (in the figure above these are nodes 1 and 4). x1, y1, x2 and y2 are the coordinates of the nodes that lie on the sides of the doors (thus node 2 and 3 in the figure above). This means that variable D needs to be computed twice, once for node 1 and once for node 4. If the sign of these two variables becomes the same, then these points lie on the same side of the door and the following check can be done.
The third check makes sure that the nodes are of the correct type. As stated before, we have two node types; endnodes and cornernodes. Nodes two and three should by definition be cornersnodes. If the two nodes are both cornersnodes then these two nodes are connected and thus it could be a door. The other two nodes can either be endnodes or cornernodes.
The fourth check calculates the angles of the two cornernodes. This needs to be done to make sure that the third and fourth, and first and second node are connected. This check is simply done using the same algorithm that was used for corner detection.
The fifth and last check, creates a door area based on the location of the door. The robot needs to be within this door area before it may ring the bell. Officially this should be a separate check since it does not belong to checking whether it is a door or not. However, it would not be useful to check whether a door is recognized, but not to check if the robot is within the door area. For this reason and the reason of simplicity, this check is done within the door recognition.
