Embedded Motion Control 2013 Group 10
Group Name
Team Picobello
Group Members
| Name: | Student id: | Email: |
| Pepijn Smits | 0651350 | p.smits.1@student.tue.nl |
| Sanne Janssen | 0657513 | s.e.m.janssen@student.tue.nl |
| Rokus Ottervanger | 0650019 | r.a.ottervanger@student.tue.nl |
| Tim Korssen | 0649843 | t.korssen@student.tue.nl |
Introduction
Nowadays, many human tasks have been taken over by robots. Robot motion control and embedded motion control in the future allow us to use robots for many purposes, such as health care. The (humanoid) robots therefore have to be modeled such that they can adapt to any sudden situation.
The goal of this project is to get the real-time concepts in embedded software design operational. This wiki page reviews the design choices that have been made for programming Jazz robot Pico. Pico is programmed to navigate fully autonomously through a maze, without any user intervention.
The wiki contains three different programs. The first program was used for the corridor competition. In this program the basic skills like avoiding collision with the walls, finding corners and turning are implemented.
After the corridor competition, a new design strategy was adopted. The second program consists of a low level code, namely a wall follower. By keeping the right wall at a fixed distance from Pico, a fairly simple, but robust code will guide Pico through the maze.
Besides this wall follower strategy, a high level approach was used. This maze solving program uses wall (line) detection to build a navigation map, which Pico uses to create and follow path lines to solve the maze. To update the position of Pico, the odometry information, local and global maps are used.
The latter two programs use Pico’s camera to detect arrows in the maze that point Pico in the right direction.
Data processing
Pico outputs a lot of sensordata. Most of the data needs to be preprocessed for use the maze-solving algorithm. The odometry data, laserscandata and imagedata are discussed below.
Odometry data
One of the incoming data types is the odometry. The odometry information is based on the angular positions of the robot wheels. These angular positions are converted into a position based on a Cartesian odometry system, which gives the position and orientation of Pico, relative to its starting point. For the navigation software of Pico, only the x,y-position and [math]\displaystyle{ \theta }[/math]-orientation are of interest.
Due to slip of the wheels, the odometry information is never fully accurate. Therefore the odometry is only used as initial guess in the navigating map-based software. For accurate localization, it always needs to be corrected.
This correction is based on the deviation vector obtained from the map updater. This deviation vector gives the (x,y,θ)-shift that is used to fit the local map onto the global map and therefore represent the error in the odometry. Because of the necessary amount of communication between the parts of the software that create the global map and the part that keeps track of the accurate position, these parts are programmed together in one node. This node essentially runs a custom SLAM (Simultaneous Localization and Mapping) algorithm.
Laserscan data
To reduce measurement noise and faulty measurements the laser data is filtered. Noise is reduced by implementing a Gaussian filter, which smoothens each data point over eight other data points.
[math]\displaystyle{ G(x)=\frac{1}{\sqrt{\sigma \pi}}e^{\frac{-x^2}{2 \sigma^2}} }[/math] with [math]\displaystyle{ \sigma = 2.0 }[/math]
Faulty measurements are eliminated in three different ways. First data points which deviate a lot from their neighbors are ignored in the Gaussian filter so that the filter does not close openings. When ignoring data points the Gaussian is normalized based on the used points.
Secondly the first and last 15 data points are ignored, decreasing the angle of view by 7.5 degrees at each side, but preventing the robot to measure itself. At last, only data points are used which are between a certain range, not only to prevent measuring the robot itself, but also to eliminate outliers.
Arrow detection
Binary image
From a full color camera image we want to determine whether there is an arrow and to determine its direction. Because this full color camera image is too complex to process, we transform it into a binary image. Since the arrows used in the competition are red, everything that is red (with a certain margin) is made white and the remaining is set as black.
Canny edge detection
Next edges are detected with a canny edge detection algorithm from openCV. This algorithm uses a Gaussian filter to reduce noise, similar to the one explained above. But it also uses the gradient of this Gaussian filter. At and edge there is a very strong transition from white to black, resulting in an extreme value in the gradient. So by thresholding the gradient, edges can be detected.
Find contours
The binary image from the canny edge detection is used as input in the FindContours function from openCV. This function finds contours by connecting nearby points, giving a vector of contours as output. Each contour consists of a vectors filled with points.
Arrow detection
Finally the image is processed so that we can start detecting arrows. Arrows are detected in two steps. First the ratio of the height and the width of each contour is determined, to see if the contour can be an arrow, see figure 1. The arrow used in the competition has a ratio of 2.9, with a lower and upper bound, this results in: [math]\displaystyle{ 2.4\lt \frac{dx}{dy} \lt 3.4 }[/math]
Next if a contour can be an arrow, the direction is investigated. This is done by determining the ratio of dx_right and dx_left, see figure 2. If the largest width (dy) is shifted towards the right dx_left > dx_right and the other way around. With some lower and upper bound this results into:
Right arrow if:
- [math]\displaystyle{ dx_{left} \gt \frac{5}{4} dx_{right} }[/math]
Left arrow if:
- [math]\displaystyle{ dx_{left} \lt \frac{4}{5} dx_{right} }[/math]
No arrow if:
- [math]\displaystyle{ \frac{4}{5} dx_{right} \lt = dx_{left} \lt = \frac{5}{4} dx_{right} }[/math]
If the largest width is in between the margins, the contour cannot be an arrow and is treated as such. This may not be the most robust way to detect an arrow, but during the tests it seemed to work just fine. A few other methods that can be used or combined to make the arrow detection more robust are circularity, shape matching, matching of moments or finding lines with the Hough transform.
Program 1: Corridor Challenge solver
Wall avoidance
The function ‘wall_avoidance’ has the simple function of preventing collisions. It therefore overrules the determined velocity and rotation if an object is very close.
The function calculates a region in which no objects are allowed. This region is elliptically shaped and its parameters are velocity and rotation rate dependent. This makes it possible to drive near objects if the velocity is low, for example when driving in a narrow corridor.
This function could be improved by not only using the rotation rate but also the rotation direction. Now Pico may stop when driving to the right if an object is to the left. The ellipse should then consist of more than two parameters.
.
Edge detection
For the corridor competition, Pico should first be able to detect an outside corner (see picture 1). To detect corners, the laser range data is used.
The robot searches left and right for large differences in wall distance. If the length ratio between two neighboring ranges exceeds a certain threshold, the position (1) of the outside corner is saved. Also the side of the corridor is set to left or right. If the first corner has been found, Pico searches at this side for other corners. This search algorithm starts at the first corner and checks for the shortest laser range distance. This coordinate is saved as second edge position (see picture 2). The coordinates of the edges are sent to the make turn function, which controls Pico’s movement.
If Pico is past the first corner, the first algorithm explained above cannot recognize this first corner anymore. To avoid problems, the second algorithm is activated for both the first and last corner (see picture x.x).
Turning into the corridor
If the first corner is passed, a boolean is set to true and the centering algorithm is replaced by the algorithm used to make a turn. This algorithm uses the two corners of the side exit determined by the corner detection algorithm.
First it drives a little towards the side exit but prevents a collision with the first corner. If the middle of the side exit is reached, L1 = L2 in figure XX, Pico is turned until both angles to the corners are equal, Theta1 = Theta2. It then drives in the exit while keeping the difference between the distances to the corners within a margin. If the absolute values of the angles to the corners are both larger than 90 degrees, the make turn algorithm is switched off and the centering algorithm is switched on again.
The algorithm could be improved by not only using the distances to the corners but also preventing coming to close to the other wall of the corridor. This is not a big problem since the wall avoidance algorithm prevents collisions.
.
Centering algorithm
This algorithm uses the shortest length to left and right and the corresponding angle. These properties are derived in the function ‘detectWalls’ and put together in one variable. Its output is a forward velocity and a rotation rate.
First the current corridor width is determined by summing up the shortest distances to the left and the right. This sum is wrong when a side exit is reached, but this is not a problem since a different function is used to make the turn.
If the distance to one of the sides is smaller than one third of the corridor width, it is determined if Pico is facing the wall or the middle of the corridor, see the red area in figure XX. If it is facing the wall it is turned back without driving forward, if it is facing the middle, it drives with a velocity of 0.2.
If Pico is in the middle third if the corridor, it also drives forward with a velocity of 0.2, see green area in figure XX.
The function could be improved by determining the angle that has to be made to face the right direction. This angle should not be used as the rotation rate, but the odometry should be used to determine if the preferred angle is reached.
Another improvement would be a better controller, which also steers if Pico is still in the middle part of the corridor. This was not implemented since in this stadium the function wall_avoidence was still very basic and could not handle driving and steering well.
.
Program structure
This program has a single node structure. All functions are defined internally in a so called ‘awesome node’. The wall avoidance has the highest priority in the node to avoid wall collision. The second most important function is the centering algorithm. if the edge detection has detected a side corridor, the turning algorithm is activated. A schematic view is shown in picture x.x.
.
Program evaluation
During the corridor competition, the program was fixed, but some bugs came up. The functions stay in middle and wall avoidance worked well, but during the last simulations before the corridor competition, the corner detection showed some undesired results.
The first and second corners are detected correctly. While turning however, Pico starts to recognize an outside corner at the end of the corridor, as depicted in the figure left. This third corner confuses the program, because the turning algorithm now questions which corners to use as a reference. Due to this unexpected corner, Pico was not able to solve the corridor competition. Therefore two new strategies were developed to win the upcoming competition: solving the maze. The wall follower and high-level maze solving program.
Program 2: Wall Follower
Structure
Dead end detection
The main reason for dead-end detection is to make sure Pico does not drive into corridors without an exit or crossing at the end. The main idea is that if dead-ends can be detected, Pico returns immediately, which can save a lot of time. Dead-ends are detected by closed contours in the filtered laser data. To find closed contours the distance (dL) between two subsequent data points is calculated with the cosine rule: [math]\displaystyle{ dL = \sqrt{R_1^2+R_2^2-2R_1R_2cos(d\theta)} }[/math] If two points are inside a closed contour, the distance between those points is small, see the red dots in figure 6. While they are not inside a closed contour the distance is large, see the green dots in the same figure. To determine whether the points are in- or outside a closed contour we use a threshold, [math]\displaystyle{ dL \lt dL_{max} }[/math].
Finally we say there is a dead-end, if the contour is closed for a certain range of points. Testing confirmed that a range of -60 to 60 degrees ensures that Pico does not only correctly detect dead-ends, but also detect them from a reasonable distance. If a dead-end is detected the robot turns left, until it finds an opening within the -60 to 60 degrees ranges and then continues following the right wall.
.
Priority algorithm
There is a simple decision making algorithm that can decide to follow the right wall, turn left or stop. This is done by checking the states of different modules and their desires. The wall follower always wants to follow the right wall, except when there is a dead-end or an arrow to the left. In these cases the robot will turn left. Arrows to the right are ignored, since the robot already wants to go that way. Finally if the robot is too close to a wall the wall avoidance wants Pico to stop. This results in the following priority: Wall avoidance > Arrow detection > Dead-end detection > Wall follower
Controller
The controller is a simple proportional controller, based upon a few essential values, defined in figure …
The essential values are the closest distance to the wall and their corresponding angles, defined in three different areas.
- Left: -130 until -10 degrees.
- Front: -30 until 30 degrees.
- Right: 30 until 130 degrees.
The wall follower always wants to keep [math]\displaystyle{ L_{right} }[/math] at a distance (d) of the wall, see figure…
The controller sends an angular velocity ([math]\displaystyle{ \dot{\theta} }[/math]) to Pico, which is proportional to the difference between this setpoint and the minimal distance to the right wall. So
- [math]\displaystyle{ \dot{\theta}\sim L_{right}{-}d }[/math]
To make sure that the robot drives as straight as possible, the angle towards the closest distance at the right wall ([math]\displaystyle{ \theta_{right} }[/math]) is kept at 90 degrees. Also
- [math]\displaystyle{ \dot{\theta}\sim\theta_{right}{-}\pi/2 }[/math]
The linear velocity (v) is also controlled proportionally. If [math]\displaystyle{ L_{front} }[/math] is large, the path is clear and meaning it is fairly safe to drive fast. So
- [math]\displaystyle{ v\sim L_{front} }[/math]
Both linear and angular velocity are limited by 0.2 and 0.7 respectively.
.
Program evaluation
A wall-follower is a fairly simple, but very robust program. It also benefits from dead-end and arrow detection; two features which both highly increase the performance. On the other hand, the simplicity of the program will eventually prevent the program from becoming highly intelligent. The lack of a map makes position determination, decision making and complex maze configurations hard to deal with. This is the program we used during the final competition, which resulted in the first place.
Program 3: Map based strategy
Program Structure and introduction
The wall following code is effective and robust, but a more challenging code was created. Here a total map of the maze is created and used to determine its path.
The laser data is filtered with the aforementioned Gaussian filter.
A local map is created from the laser data only. This results in lines representing the visible walls which are then sent to the map updater node. Then the local map and odometry are updated with an optimization algorithm to match the global map. The global map is updated and visualized together with the driven path.
This map is then sent to the node ‘create path’. This function also receives an arrow direction if an arrow is detected. If a service is asked from the ‘Line follower’ the end point of the last path is reached and a new one has to be sent back. This path consists of a line with an end point and an absolute angle that has to be matched with the odometry at the end of the line.
It is also possible to do this path planning with the local map, but then it is not possible to create a path going back when a dead-end is found. The global map consists of many optimization steps and is therefore much more reliable than the local map.
The line follower receives the new path and a global position. The line follower drives to the end of the line if the shortest distance to the line stays within a margin. Otherwise it will first drive back to the line and then again to the end.
Then the earlier described ‘wall avoidance’ node checks if no objects are close and sends the velocity and rotation rate to Pico.
The main advantage of this map based navigation is that arrows can be used smartly and the path planning allows for feed forward where the wall follower only uses feedback. The path planning algorithm can be improved for creating smooth corners and determining the path before the end of the line is reached. In this way to the total maze can be driven with the maximum velocity of 0.2.
Map builder
Odometry
Map updater
For navigation purposes, a global map is desired. This global map can be built out of the local maps from each time step. However, these local maps are saved in a vector format with the robot fixed reference frame as a base. The global map needs to be defined in a global perspective. Hereto the local map is transformed to a global vector base. Additionally, the position of the robot in the map is required. This is however a nice byproduct of the process of creating a map. The noisy and error sensitive odometry information can be enhanced using the translation and rotation necessary to fit the local map to the global map.
Firstly, the global vector base must be defined, this can be done using the robot’s initial position (at the start of the program). This position and orientation is chosen as the (x,y,theta) = (0,0,0) point of the global map.
Next, the local map created by the robot needs to be translated and rotated to the global base. This can be done by using an estimation of the robot’s position given by the odometry information (Odometry). Every point in the local map can be transformed to a point in the global map using the following transformation:
[math]\displaystyle{ \mathbf{x_{global}} = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \\ \end{bmatrix}. \begin{bmatrix} x_{local} \\ y_{local} \\ \end{bmatrix}+\begin{bmatrix} x_{robot} \\ y_{robot} \\ \end{bmatrix}. }[/math]