Embedded Motion Control 2016 Group 7

Group Members

ID-Number	Name	E-mail
0816253	Emile van Halsema	e dot v dot halsema at student dot tue dot nl
0805999	Nard Strijbosch	n dot w dot a dot strijbosch at student dot tue dot nl
0816608	Raymond Kerstens	r dot j dot c dot m dot kerstens at student dot tue dot nl
0814199	Frank Heck	f dot j dot m dot heck at student dot tue dot nl
0816361	Ids van den Meijdenberg	j dot w dot a dot v dot d dot meijdenberg at student do tue dot nl

Initial Design Embedded Motion Control Group 7

The project is dived in two parts: the corridor challenge and the maze challenge. The presentation of task-skill-motion system architecture can be found here: File:System architecture Presentation Group 7.pdf

Corridor challenge

For the corridor challenge the goal is to take the first exit.

Requirements

For this corridor challenge a number of requirements have to be met for an arbitrary corridor:

• Reach exit as fast as possible.
• Be able to recognize walls.
• Don't hit the walls.
• Be able to recognize and take the first intersection.
• Be able to rotate and translate.
• Be able to recognize maze exit.
• Stop when exit is reached.

Specifications

These requirements can be rewritten to specifications:

• Reach end within 1 minute.
• Recognition walls and intersection.
  • Objects in straight line (with variation under 5 cm) are considered walls.
  • Opening of at least 0.5 m is an intersection.
• Don not hit walls.
  • Distance to walls at least 10 cm.
• Be able to drive.
  • Drive in direction of point (updated 30 times a second).
  • Maximal 0.5 m/s during straight line.
  • Maximal 1.2 rad/s rotation.
• Recognize exit and stop
  • Drive straight for 3 seconds after being 30 cm from intersection, then stop.

Functions

In order to meet these specifications the system is decomposed into multiple functions:

• Intersection recognition: identifying where the intersection is.
• Path planning: calculating the optimal path through the corridor.
• Safety features: Ensuring that PICO does not hit the walls.

Maze challange

For the maze challenge the goal is to find the exit in a maze. The location of the exit is unknown, there might be loops and there is a door in the maze.

Requirements

The requirements of the corridor challenge also hold for the maze challenge but are elaborated with:

• Be able to map the maze.
• Be able to locate itself in map.
• Be able to recognize maze components (doors and walls).
• Be able to open a door.
• Be able to detect the maze exit.

Specification

These additional requirements can be written to specifications:

• Mapping
  • Update map 30 times per second.
  • Remember path.
  • On the driven path map walls, doors, crossings.
• Be able to localize itself in map.
  • Localize position with precision of 5 cm.
• Recognition possible doors.
  • Recognize dead end of at least 30 cm.
• Open door.
  • Beep when in front of detected door.
• Recognize maze exit.
  • Stop when no walls are detected in front.

Functions

The functions from the corridor challenge can then be completed to obtain the functions required for the maze challenge:

• Mapping: Save all intersections, doors, and connections.
• Optimal control: determine the best strategy for completing the maze

Interfaces

To keep track of the states in which the robot is, the states will be printed in the screen. Also is the idea to show the mapping for the maze challenge on the screen for debugging.

Mazesolver Corridor Challenge

For the corridor challenge a maze solver is used to determine which setpoints to go to. The maze solver is built using knowledge of the situation that will occur and therefore a more advanced maze solver algorithm will be needed for the maze challenge. The determination of which setpoints to go to is based on a standard test for the corridor. The correct nodes on the path which should be taken for the corridor will be listed as setpoints. If the first setpoint is not listed yet, a setpoint will be created to drive forward. The list will be updated with 10 Hz, as the whole loop used for the corridor challenge is updated with 10 Hz.

Setpoint Check

To determine whether a setpoint is reached, a test based on odomotry will be used. A setpoint will be seen as reached if the robot is within a margin of the actual setpoint. This test is defined as follows:

"The robot should be within the set margin for x and y position and within another set margin for the orientation."

If the robot has not reached the setpoint, the setpoint will be send again. Once the setpoint is reached, a message will be send to the maze solver such that the next setpoint in the list will be sent and the second setpoint gotten from the mazesolver will be send. .

Movement

One of the key elements of this project, is that PICO should be able to drive autonomously. From a higher level, the software receives a setpoint in absolute coordinates with respect to the starting position of the robot. With this information, the robot can move towards this location. However, it should also be avoided to collide into the walls. Therefore, a potential field is implemented which is a relatively easy method. The key elements are the following two forces:

• [math]\displaystyle{ F_{repp} }[/math]: Repelling force to ensure the robot will not collide with the walls
• [math]\displaystyle{ F_{attr} }[/math]: Attracting force responsible for the movement towards the setpoint

The potential field is a method from where the laser data range and angle with respect to the orientation of PICO are used. The wall forces will push PICO away from the walls, while the setpoint will attract PICO, see the Figure below.

For every laser range and angle, the following Equation is executed to calculate the force vector of that data point:

[math]\displaystyle{ F_{repp,i} = \frac{r_{scale}}{1375r_i^3 - 232.5r_i^2 + 12.19r_i} }[/math]

where [math]\displaystyle{ r_{scale} }[/math] is a scaling constant which is used to optimize the repelling force vectors. Afterwards, the contribution in the x- and y-direction can be computed with help of the angle for each laser data. The attracting force is computed by the following Equation:

[math]\displaystyle{ F_{attr} = e_x^2 + e_y^2 }[/math]

in where [math]\displaystyle{ e_x }[/math] and [math]\displaystyle{ e_y }[/math] are the relative error between PICO and the setpoint in meters. Afterwards, this force is normalised to ensure that [math]\displaystyle{ F_{attr} }[/math] will not go to zero when PICO is almost at the desired setpoint. Note that [math]\displaystyle{ F_{attr} }[/math] is with respect to the base coordinate frame. Since [math]\displaystyle{ F_{rep} }[/math] is in the relative coordinate frame, [math]\displaystyle{ F_{attr} }[/math] is rewritten to relative coordinates with a transformation matrix.

The parameters above are scaled in such a way that the following desired behaviour is exacted:

• When PICO is located close to a wall, [math]\displaystyle{ F_{rep} \gt \gt F_{attr} }[/math]
• When PICO is not located at a close distance to a wall, [math]\displaystyle{ F_{rep} \lt \lt F_{attr} }[/math]

Subsequently, the total force [math]\displaystyle{ F_{tot} }[/math] is calculated. From here, an angle [math]\displaystyle{ \phi }[/math] is obtained which can be used to calculate the desired velocities in the x- and y-direction, see the Figure below.

Afterwards, the velocity in x- and y-direction of the PICO is simply calculated by

[math]\displaystyle{ v_{x} = \text{cos}(\phi)v_{max} }[/math]

and

[math]\displaystyle{ v_{y} = \text{sin}(\phi)v_{max} }[/math].

For the rotational velocity, the error between the setpoint orientation and PICO's current rotation is used.

Final Design for Maze Challenge

Coordinator

The coordinator is the function that controls in what state the robot is and what state it should go. The states are placed in a switch-case with as variable the integer "state". There are several states: Setpoint reached, Driving, Possible door, No fit or setpoint, Stuck, Stuck driving and Reset.

Setpoint reached

In the setpoint reached state first a new rectangle fit is made out of the laser data. The fit is analyzed to determine the possible setpoints locally. Using the local distance to the setpoint that is reached and the angle of the main rectangle the odomotry can be updated. Next, the global mapping can be updated and a new setpoint can be determined using the mazesolver. If all goes as planned the state will be updated to the driving state. If not, the state will be updated to the No fit and setpoint state.

Driving

The driving state is build up with a big if else that checks for certain situations, such as recognition of a door or the reaching of a setpoint. If some situation occurs, the state variable will be changed to make sure that the robot will be in a new state to deal with the situation properly. If there is no special situation, the robot can drive using the potential field to make sure that the robot will not collide into a wall.

Possible Door

When a door is recognized and the robot is in front of the door, the robot will get into this state. In this state a new rectangle fit will be made. After beeping and waiting for 5 seconds, a new fit will be made. The length of the rectangle in front of the robot is compared in the two situations to determine if the door has been opened. If the door has been opened, the mapping is cleared and new nodes will be made. The setpoint at the door will be set to a high value to make sure that the robot will not return through the door unless it has run through the whole maze several times. Next the state will be updated to the Setpoint reached state to make a new fit and determine new setpoints. If the door has not opened, the door was no door but a dead end and the robot will turn around to continue its search for the door.

No fit or setpoint

In this state two situations are distinguished: having an unreliable fit or having no setpoint. If the fit is reliable, but there is no setpoint, the local point that is in front of the robot with the least distance to the robot will be set as new setpoint. If the fit is unreliable, the robot will put a new setpoint 20 cm further in the corridor in which the robot is standing.

Stuck

If the robot can not reach its current setpoint it will get into this state. If the robot has no official main setpoint, as made in the first state, the robot will immediatly go to the reset state. If the setpoint cannot be reached, a temporary setpoint in the middle of the intersection will be placed, which will be driven to in the next state.

Stuck driving

In this state the robot will try to drive to the temporary setpoint. Once this setpoint is reached, the robot will go to the normal driving state to try again to drive towards the original setpoint. However, if the temporary setpoint is not reached, the robot will go to the reset state.

Reset

In this state the robot will get once things have gone wrong. All the mapping will be deleted. Next a new fit will be made and a new setpoint will be determined. If the fit is unreliable or there cannot be created a setpoint, the robot will turn 0.5 radians and try again until it has succeeded to get a reliable fit and a setpoint. Once a setpoint is determined, the state will change to the driving state.

Environment identification

To subtract relevant information from the laser data an environment identification algorithm is used. This algorithm determines the dimensions of the corridor where pico is positioned, and it determines all the position and location of the visible sideroads. Using this information the world model can be updated, and the odometry data can be corrected.

Algorithm

The general algorithm consists out of the following steps

1. Preprocess laser data
2. Determine the dimensions of current corridor
3. Determine location and dimension of side roads
4. Determine reliability of the found dimensions

Determine current corridor

Optimization problem formulation

[math]\displaystyle{ \begin{align} &\underset{x}{\operatorname{maximize}}& & f(x)=(A+B)\cdot(l+m) \\ &\operatorname{subject\;to} & & e_i(x) \leq 0, \quad i = 1,\dots,n \\ &&& x_i \leq x_{max}, \quad i = 1, \dots,5 \\ &&& x_i \geq x_{min}, \quad i = 1, \dots,5 \\ \end{align} }[/math]

where

• the objective function [math]\displaystyle{ f(x) }[/math] is defined by the area of the rectangle,
• [math]\displaystyle{ e_i(x) }[/math]the smallest distance of point [math]\displaystyle{ i }[/math] to the sides of the rectangle, hereby the error is positive if the point is inside the rectangle, and
• [math]\displaystyle{ n }[/math] are the number of points.

Determine side roads

Optimization problem formulation

[math]\displaystyle{ \begin{align} &\underset{x}{\operatorname{maximize}}& & f(x)=C\cdot D \\ &\operatorname{subject\;to} & & e_i(x) \leq 0, \quad i = 1,\dots,n \\ &&& x_i \leq x_{max}, \quad i = 1, \dots,3 \\ &&& x_i \geq x_{min}, \quad i = 1, \dots,3 \\ \end{align} }[/math]

where

• the objective function [math]\displaystyle{ f(x) }[/math] is defined by the area of the rectangle,
• [math]\displaystyle{ e_i(x) }[/math]the smallest distance of point [math]\displaystyle{ i }[/math] to the sides of the rectangle, hereby the error is positive if the point is inside the rectangle, and
• [math]\displaystyle{ n }[/math] are the number of points.

Mapping

In the environment identification the current corridor and side roads are found relative to PICO. This can be translated to directions relative to PICO and positioning error relative to PICO. This is calculated in Local direction recognition. With the current location and orientation of PICO the directions relative to PICO can be rewritten to global directions from the origin. The mapping is build on the fact that the maze consists of crossings and turns. Each crossing consists of directions pointing away from the crossing. As the maze is axis aligned, all directions point either North, West, South or East. A crossing can thus be represented by multiple directions, which belong to the same intersection. A corner can be represented by two directions belonging to the same intersection, and a dead end by a single direction belonging to a intersection. In order to connect crossings, corners and dead ends the directions have to be connected. Hence the maze is represented by nodes(intersections) and edges(connected nodes) which in turn results in a undirected graph. In order to store the maze the position, orientation, intersection (number), connected direction are saved for each node together with the direction ID (which is used also for the connected direction) and the number of times that direction has been passed(which is used in the Maze solving algorithm later).

Maze challenge attempts

On 8 juni 2016, the final Maze challange was held at the RoboCup soccer field. In total, 7 groups implemented their code on PICO to see which group was able to finish the maze. Each group had two attempts at solving the maze. The maze is depicted below and as one can see consists of multiple dead-ends, one loop, a rather tricky placed door, and an open-space. The starting position was the U-shaped corridor in the bottom of the figure. The open-space was located behind the door. At the end of the open-space, the exit was located. Below, the two attempts are discussed together with the results and a video of the challanges.

Attempt 1

Below, a YouTube-link to the first attempt of the Maze challange at the RoboCup soccer field can be found. For the first attempt, the Mazesolver introduced a preference with respect to which direction PICO should go at an intersection. When PICO approaches an intersection, the first preference is to the left, the second preference to the right and else PICO should go straight.

https://www.youtube.com/watch?v=8nTIp-3s3bo

It is observed that the initialisation is executed correctly, since PICO correctly fits the rechtangles and goes to the left at the initial intersection. Afterwards, PICO recognizes the second intersection and decides to go to the left again. Unfortunately, due to the unlucky placement of the door PICO did not recognize the intersection including the door. Therefore, PICO drives forward and goes to the left afterwards. Due to the preference to the left, PICO drives back to it's starting position. However, the Tremaux algorithm ensures that PICO does not visit places where it already have been. Therefore, it goes to the right at the intersection, hence it can be concluded that the mapping was executed correctly. Unfortunately, PICO detects a dead-end which results in a turn-around. This is due to the fact that the rectangle fit does not 'see' the corridor to the right, and therefore marks the corridor as a dead-end. Subsequently, PICO drives to the starting position. From then on, PICO starts panicking and undesired actions are happening resulting in a collission with the wall. The problem of the wrongly detected dead-end could be fixed by implementing a certain trust region in the local node recognition. That is that only in a certain region around PICO, local nodes should be recognized. However, this re-adjustment will have consequences for the rest of the code and therefore was not implemented easily during the Maze challange.

Attempt 2

Below, the YouTube-link to the second attempt of the Maze challange can be found. Since the first attempt was not succesfull, another method of the Mazesolver was implemented. This time, PICO randomly chooses a direction to go when PICO is at an intersection. Maybe PICO was now able to atleast find the door and open it.

https://www.youtube.com/watch?v=J7_U3Ru8vq4

From the video, it is observed that again the initialisation phase was succesfull. Note that this time PICO randomly chooses to go to the right at the second intersection. Unforunately, PICO again approaches the door from the side for which is it difficult to recognize the door. From that on, PICO start panicking again and does undesired actions. However, PICO was able to reset hisself resulting in a clean slate resulting in correctly taking turns. Sadly, PICO again recognizes a dead-end due to the fact that one side-road is not fitted correctly. Therefore, PICO turns around. After multiple can find the door and is able to open the door, although a little bit of luck was needed. Unfortunately, PICO did not enter the open-space and was therefore not able to finish te Maze.

Overal Conclusion

Conclusion

During this group-project we learned a lot regarding coding in C++, but also using a compact code architecture. It turned out that our approach was different and unique compared to all other groups. The rectangle fit idea was worked out and used as one of the main functions used for other code components such as local node recognition and the mapping. Although the rectangle idea seemed wonderfull on the first hand, there are some drawbacks. One of the biggest drawback is that the local node extraction is complex to implement. Furthermore, the odometry correction should have been implemented in a different manner since the current odometry correction was not reliable enough due to the low update rate. It appears that there is always room for improvement. Furthermore, programming in C++ was a new experience for most of us. The EMC environment provided on Ubuntu gave us the tools to develop our program skills, and gave us hand on experience of embedding the code on an actual robot which was really great. To conclude, the biggest lessons learned during this project are:

• The effectiveness of a coordinator
• When the main file is explicitly separated in multiple files, the structure from the composition pattern is easier to understand
• Simulation does not guarantee correct performance during experiments
• Start with a correct model-formulation before writing C++ code
• Start easy
• Make sure that the magic parameters are defined to ensure that the code is easily scaleable
• How to ensure robustness and take undesired behaviour into account

Recommendation

As mentioned earlier, there is always room for improvement. When we had some more time to improve, the following aspects could be improved:

• Odometry correction: as mentioned earlier, the current implementation is not reliable enough to ensure proper correction. Therefore, another method should be implemented, preferably using the rectangle fits.
• Local node recognition: the current node recognition works fine, however there is still room for improvement. During the Maze challange it appeared that the trust region of the local node recognition should be lowered to ensure that corridors are not characterised as dead-ends.
• Visualisation: it is always nice to have some fancy visual aids which can be used for debugging. For example, the vectors of the potential field could be represented.