Mobile Robot Control 2024 Rosey

This wiki page describes the work undertaken by Team Rosey during the Mobile Robot Control course. The objective was to develop functional code for a robot to navigate a map, handling both known and unknown, static and dynamic obstacles, to reach specific waypoints efficiently and in a controlled manner.

The work is organized by weeks, showcasing the team's progress in solving the given assignments and advancing toward the Final Challenge. Design decisions are justified by evaluating potential advantages and disadvantages, providing a clear explanation of our approach.

Group members:

Name	student ID
Tessa Janssen	1503782
Jose Puig Talavera	2011174
Ruben Dragt	1511386
Thijs Beurskens	1310909
Pablo Ruiz Beltri	2005611
Riccardo Dalferro Nucci	2030039

Week 1 - Don't crash code

Exercise 1 - The art of not crashing.

Overall approach

Every member of the group worked on this exercise separately, after which we combined and discussed the different approaches. Though each implementation the members of our group came up with is slightly different, they all have the same starting approach. The robot is constantly looping through the laser data and when the returned laser range of one of the laser points is within a predefined distance, the robot is commanded to act on this. Additionally, most approaches only loop through the front/middle part of the laser data to make sure only obstacles that are actually in the robots path are taken into account. The different methods of going about this are explained below.

Implementation 1: Stopping

Riccardo Dalferro Nucci & Ruben Dragt

In this implementation the instruction to the robot is the most simple. The robot is instructed to keep moving forward using the io.sendBaseReference function and when the laser returns a distance within 0.3 meters, the robot is commanded to stop by setting the forward velocity in io.sendBaseReference back to 0. Using this method means the robot will not continue moving as long as the obstacle is within the 0.3 meters. If the robot is in front of a wall it will stay there forever, but with a moving obstacle such as a human walking by, the robot will eventually move forward again until the next obstacle. Video of the simulation can be found here.

Implementation 2: Turning away

Using this method, the robot is not instructed to just stop but to also turn away from the obstacle. The added benefit of this approach is that the robot will be able to start moving forward again, as the laser will eventually stop returning obstacles in the set distance.

• Method 1 Tessa Janssen - In this code, the robot loops over part of the data by setting the for loop from scan.ranges.size()/2 - range to scan.ranges.size/2 + range (setting variable range at 150 means a view of approximately 1.2 rad). Then, when an obstacle is detected in within a set distance e.g. 0.5 m, the forward velocity is set to 0, but the angular velocity is given a positive value. This results in the robot turning left when it has reached an obstacle. When there are no longer any obstacles the angular velocity is set back to 0 and the forward velocity is given a positive value. The downside of always turning left is that the robot can get easily get stuck in corners and will not take into account efficiency. Video of the simulation can be found here.

Figure 1: Coco driving around with the don't crash code (Thijs).

• Method 2 Pablo Ruiz Beltri - In this approach, a safety distance and a cone size (in number of laser points) are set. The cone is centered in front of the robot. While we are properly connected, the laser data in this cone is verified to be greater than the safety distance. If it is, the robot moves forward, if not the robot rotates. To keep a forward path, the direction of rotation is determined by assessing if the safety distance violation was in the left or the right of the cone. After testing this approach, it is concluded that it is vulnerable to corners, where safety distance violations come equally from left and right, leaving the robot in a blocked position.

• Method 3 Thijs Beurskens - Here, the cone of vision of the robot is determined by setting the minimum and maximum angle, which is then translated to the amount of points the for loop should cover. The turning direction of the robot is determined by checking on which side of the robot the obstacles are closer and thus turning where there is more space. This approach has again the benefit of being able to keep moving around a map/obstacle course while also being more clever on where it can go. This approach makes the robot less likely to get stuck in certain areas and makes it more robust to changing environments.
• Method 4 Jose Puig Talavera - My implementation for the dont_crash() code initially checks whether the robot is receiving laser and odometry data. If yes, command a forward motion of 0.5 meters per second. Then, read laser data through a cone of vision. If one of the laser range values is within or at 0.3 meters, stop the vehicle. Check distance to the right (i = 200), then check distance to the left (i =800). If left > right, turn +0.3 radians per second (counter-clockwise). Inversely, if right > left, turn -0.3 radians per second (clockwise). A video of the simulation can be found here. I personally like the part where it gets stuck at a point where the distance to the left and right are really similar && has an obstacle within 0.3 meters.

Exercise 2 - Testing your don't crash

During the first testing session it became clear that the parameters that were used in simulation needed to be adjusted to the robot in real life. Especially the velocity needed to be tuned down when using Coco/Bobo as they do not have the speed limiter built in. Moving too fast towards an obstacle meant that when it saw an obstacle within the safety range, it did not have enough time to stop before hitting the obstacle. Though tuning the velocities and obstacle distances was a doable fix and the code from method 3 (Thijs) ended up working very nicely in an environment with multiple different obstacles. The full video showing this approach can be seen here and a brief clip is shown in figure 1.

*INSERT SCREENCAP FROM TEST MAPS?*

Week 2 + Week 3 - Local navigation

Vector field histogram:

Figure 2: Local navigation using the Vector Field Histogram.

Thijs Beurskens, Pablo Ruiz Beltri & Riccardo Dalferro Nucci

Concept

The main idea behin the Vector Field Histogram (VFH) method is to treat objects as vectors in a 2D cartesian histogram grid and create a polar histogram to determine possible "open spaces" to get to the goal. A polar histogram is created, centered in the robot's current position, using the LaserData and a number of sectors that can be given as an input. Every Laser Data from min_angle to max_angle (the data behind the robot are considered as not visible, even though defined in the polar histogram) is then treated as a vector, where its magnitude and direction are computed based on the following formula.

[math]\displaystyle{ \beta = \arctan(y - y_{current}, x - x_{current}) }[/math],

[math]\displaystyle{ m = a - b * distance }[/math],

where a is the product of b and the maximum distance, which needs to be given as input along with b. Moreover, distance is the magnitude of the vector expressing the current position. Using a sendPath command, a visual representation of the sectors was given. Once the histogram is completly defined the program finds every valley, which means every sector with a value below threshold (also given as input) and chooses the best one, that is the one closest to the target direction. Every value of the histogram is normalized. A control input is then given to the robot to make it go in that direction. To note that a PID has been added to control the rotational speed, in order to best align the robot to the goal direction.

Simulation and Implementation

When simulating the code, the visual representation of the polar histogram was very useful, allowing to debug eventual errors in the code. The code was tested in the given maps file, with increasing difficulty. The simulations showed that with an increasing number of obstacles the robot movements were more nervous, up to the point where in map 4, due to a very narrow point of the map, the robot slightly bumped into a wall, which was caused by the fact that the robot has no knowledge of its dimensions. This problem was then solved by increasing the threshold. Screen recording of the simulation can be find in map1 and map4.

Once the program was working well in the simulation it was tested in the lab. As it can be seen from Fig. 2, the code showed good results also on the real robot.

Reflection

Advantages of the approach

The first advantage of this approach is the computational efficiency. Moreover, the implementation on the real robot showed a good robustness to noisy sensor data. Furthermore, the mathematical tools used, such as the polar histogram and the vectors, were quite familiar object, which allowed to implement the code without too many difficulties. Since the object density is computed on real-time LaserData the program is quite robust for what concerns moving objects or changing environments. Finally, since the code looks for closest free valley to the goal direction, the VFH method does not usually suffer from local minima.

Disadvantages of the approach

The first disadvantage is that this method might struggle with narrow passages and sharp corners. Moreover, since this code uses real time LaserData the robot needs to have a relatively low speed, to be able to compute the new direction before bumping into an obstacle.

What could result in failure?/How would you prevent these scenarios from happening?

As said above, a narrow passage or a sharp corner could result in failure. This could be fixed by properly tuning the threshold.

Artificial potential fields:

Tessa Janssen, Jose Puig Talavera & Ruben Dragt

Concept

The main idea behind an artificial potential field is calculating both an attractive potential towards a target and a repulsive potential away from obstacles, which are summed together to form the potential field. The resulting force vector is calculated by taking the negative of the gradient of the potential field.

[math]\displaystyle{ F(q) = ᐁU(q) }[/math]

The attractive potential is determined by the following formula

[math]\displaystyle{ U_{att}(q) = \frac{1}{2} * k_{a} * (||q-q_{goal}||)² }[/math],

which depends on the difference between the current position of the robot and the target position. The target position was set as a point 6 meters further in the y direction than the starting position, the current pose was calculated by using odometry data from the robot (with starting coordinate (1.5, 0, 0)). Because the odometry is not perfect, this introduced a small error to the final position of the robot, which was not corrected for in this exercise. The repulsive force is given by

[math]\displaystyle{ U_{rep,j}(q) = \frac{1}{2} * k_{rep,j}(\frac{1}{(||q-q_{j}||} - \frac{1}{ρ_{o}})^2 }[/math] if [math]\displaystyle{ ||q-q_{j}|| ≤ ρ_{o} }[/math]

or

[math]\displaystyle{ U_{rep,j}(q) = 0 }[/math] if [math]\displaystyle{ ||q-q_{j}|| ≥ ρ_{o} }[/math],

which uses the laser data to determine the distance from the robot to an obstacle. For this specific implementation, each laser point in the for-loop returning a distance that fell within the predefined space buffer ρ_o was registered as a separate obstacle, with the corresponding repulsive potential being added to the total potential field. The constants k_att and k_rep were used to adjust the relative importance of attracting and repulsing.

The forces are broken down into [math]\displaystyle{ F_x }[/math] and [math]\displaystyle{ F_y }[/math] components after calculating the analytical partials derivatives of the attractive and repulsive potential energies. The partial derivatives of the attractive potential energy are shown below:

[math]\displaystyle{ \frac{δU_{att}}{δx} = k_a(x-x_{j}) }[/math]

[math]\displaystyle{ \frac{δU_{att}}{δy} = k_a(y-y_{j}) }[/math]

The partial derivatives of the repulsive potential energy are the following:

[math]\displaystyle{ \frac{δU_{rep}}{δx} }[/math] = [math]\displaystyle{ \frac{k_{rep}(x-x_{j})(\sqrt{(x-x_{j})^2+(y-y_{j})^2}-ρ_{0}}{ρ_{0}((x-x_{j})^2+(y-y_{j})^2)^2} }[/math]

[math]\displaystyle{ \frac{δU_{rep}}{δy} }[/math] = [math]\displaystyle{ \frac{k_{rep}(y-y_{j})(\sqrt{(x-x_{j})^2+(y-y_{j})^2}-ρ_{0}}{ρ_{0}((x-x_{j})^2+(y-y_{j})^2)^2} }[/math]

The resulting force vector at the current location of the robot was used as input to the robot by calculating the angle corresponding to the vector.

[math]\displaystyle{ F_{x} = \frac{δU_{att}}{δx} + \frac{δU_{rep}}{δx} }[/math]

[math]\displaystyle{ F_{y} = \frac{δU_{att}}{δy} + \frac{δU_{rep}}{δy} }[/math]

where the commanded angle is

[math]\displaystyle{ θ_{T} = atan(F_{y},F_{x}) }[/math]

This was used as reference angular velocity for the robot, while y-velocity was kept mostly constant. Mostly constant here means that in the original implementation the velocity of the robot was kept low to allow the robot to have enough time to respond to all obstacles, but after testing, code was added that made sure the robot picked up its speed when there were no obstacles in sight.

Simulation and Implementation

Figure 3: Hero navigating a corridor using the artificial potential fields approach.

Testing the code in simulation revealed that tuning k_att, k_rep, ρ_o, and the forward and angular velocity were very important. Map 1 compared to map 4, for example, showed that when there are not a lot of obstacles, the robot can get away with higher attraction to the target, making the path more efficient. With more obstacles, however, the robot would get too close to the walls and the k_rep had to be retuned as well as increasing ρ_o. In the first few iterations of the code, the robots movements were very jittery. This was due to the calculation of the angle of the force vector. When moving up in difficulty through the different maps, the forward velocity and angular velocity had to be reduced in order to get a smooth trajectory. Screen recordings of the simulation were made for both map 1 and map 4.

One of the first test sessions after the code was working well in simulations, already showed a smooth trajectory as can be seen in the full video here or in the snippet in figure 3. The robot moves around the objects to the target approximately 6 meters away, but at a pace that is quite boring and seems inefficient from a users point of view. Code was then added to make the robot increase its speed when it no longer saw any objects within the buffer ρ_o. This helps slightly with efficiency, but only once it was already outside of the maze we set up. Another video including this speed increase can be found here.

Reflection

Advantages of the approach

One advantage about the potential fields is that it makes sense intuitively: walk away from obstacles and walk towards your goal. The mathematical components that are required to calculate the potentials, gradients and corresponding vectors are also familiar methods, making it a very doable method to implement from scratch. Another advantage is that the repulsion to obstacles is calculated on real-time laser data. This makes the robot quite robust to changing environments as new obstacles (walls around the corner or people standing in the way) are incorporated into the navigation immediately.

Disadvantages of the approach

A disadvantage that is clear from both the simulation and the test sessions is that in order for the robot to be able to recognize all obstacles in time, its velocity needs to be quite low. This speed-accuracy trade-off is especially evident in more complex environments with many obstacles. And although it is an advantages that the robot updates the obstacles around it constantly, the computational speed is not always sufficient to recognize moving obstacles such as people. In addition, in case of local minima in the gradient, the robot will get stuck while not having reached its target. Another disadvantage that was best seen in the simulation was that the robot most from left to right a lot when passing through a narrow street as the resulting force vector keeps shifting from left to right.

What could result in failure?/How would you prevent these scenarios from happening?

One situation that could lead to failure is a local minimum as explained above. In case the robot is faced with a wall directly between itself and the target and the repulsive potential is equal and exactly opposite to the attraction potential, the result will be net zero and the robot will not move. This might be prevented by including some code that makes sure that if the input to the robot is zero, but it has not yet reached it target it will go in a certain direction. This correction should of course be careful of nearby obstacles. Another potential failure is not recognizing a living obstacle passing by and crashing into it. Completely eliminating this is hard, but it can be mitigated by having a low speed and increasing the range in which it classifies obstacles. Both of these give the algorithm more time to process the obstacle.

Week 4 - Global navigation:

A* algorithm implementation:

Pablo Ruiz Beltri, Riccardo Dalferro Nucci & Ruben Dragt

Concept and Implementation

The A* algorithm is a widely used pathfinding and graph traversal algorithm. It combines the benefits of Dijkstra's Algorithm and Greedy Best-First Search to find the shortest path between nodes in a weighted graph. To do this it uses a value of the cost to reach a target and a value for the distance from that new node to the target. The code that calculates the shortest path between the nodes to the end goal by follows the next structure

1.Initialization:

The code starts with an open set called open_nodes containing the initial node "_start_nodeID", and another set "closed_nodes" where the visited nodes will be stored, to make sure that they are only visited once. The next step is to initialize a cost map to keep track of the cost from the start node to each node "h-cost", using the function "calculate_distance()". Then a heuristic map is initialized estimating the cost from each node to the goal called "g-cost", these are initialized at the start as infinity and will be recalculated for each visited node to the surrounding nodes. This way the nodes with no edge connecting them from the current node" will have a cost that is infinitaly large. The f-cost is the summation of the g-cost and h-cost is calculated for the initial node and the nodes connected to the inital node by edges.

2.Processing While the open set is not empty, select the node with the lowest f-cost from the open nodes set. If this node is the goal, the path is found. Otherwise, move the node from the open set to the closed set.

3.Expansion:

For each neighbor of the current node, calculate the tentative g-cost.

If this tentative g-cost is lower than the previously recorded g-cost for that neighbor, update it and change the parent node to the current node.

Set the f-cost for the neighbor (f = new g + h).

If the neighbor is not in the open set, add it.

4.Repeat:

Continue the process until the goal is reached or the open set is empty, indicating no path exists.

5.Store the optimal path:

To store the optimum path from the initial node to the goal (path_node_IDs), we start at the goal node and move to its parent node, continuing the storing of parents node until this becomes the inital node.

Probabilistic Road Map implementation:

Tessa Janssen, Jose Puig Talavera & Thijs Beurskens

Figure 4: Probabilistic Road Map based on the Global Navigation assignment.

Overall approach

For this exercise a framework of code to create a Probabilistic Road Map (PRM) was already given. We worked through the given exercises one by one to obtain the final result as is shown in figure 4. More detailed explanations of each sub-exercise are given below.

1. Inflate the map

The first exercise was to inflate the occupied spaces in a map to adjust for the size of the robot: inflatedMap(). This was done by looping over the pixels of the image by column and row and checking if the pixel has a value below the threshold that was previously defined in the assignment (thresh_occ = 130). If this is the case, the pixel is deemed occupied and a matrix of size radius x radius is looped through, centered around that pixel. Then every pixel in that square is given value 0. The variable radius can be set depending on the size of the robot that is used.

2. Check vertices

The second exercise was writing a function that checked if the vertices generated into the map were at least a certain distance away from each other: checkIfVertexIsValid(). Our function loops through the existing vertices in the Graph, and for every vertex calculates the distance between this one and new_vertex that is given as an argument. The distance between the new vertex and the existing ones is calculated by applying the Pythagorean theorem on the distances between the first and second coordinates of the vertices. If the distance is smaller than some allowed distance (here we chose 0.25 meters), the function returns false, meaning the vertex is not added to the Graph.

Figure 5: Coco driving through real-life map_compare.png using global and local planning.

3. Check edges

The final exercise is about checking if an edge between two vertices is valid: checkIfEdgeIsValid().

First this means checking if the two vertices an not too far away from each other (checkIfVerticesAreTooClose()). This function was filled with pretty much the exact same code used incheckIfVertexIsValid(), only now the function checks if the distance between two vertices is bigger than some allowed distance (here we chose 1 meter). If this is the case the function returns false and no edge is created between these two vertices.

A second requirement is that the edge between two vertices cannot cross an obstacle:checkIfEdgeCrossedObstacle(). This function converts the coordinates (first and second) of two vertices into their pixel equivalent and arranges them to make sure that pixel 1 is always to the left of pixel 2. The function loops over all pixels that lie in the square you can draw with the two pixels as opposite corners. Then for each pixel in this square it is checked if it is occupied, using the same threshold as before. If it is indeed occupied, the distance from this pixel to the line connecting the corner pixels is calculated. If this distance is within a 2 pixel range, it is said to be in the 'edge' territory and the function will return true: the edge crosses an obstacle. This method was chosen because it only considers obstacles a problem when they are actually on the edge between two vertices. This makes efficient use of the map as will not remove edges only because they are too close to obstacles.

Combining A* and PRM

Combining the two frameworks described above results in a path planned such as in figure **. The A* algorithm uses the PRM graph as input vertices to find the optimal path to the starting position to the target.

Combing Local and Global planning

For a first iteration of combining the local and global planning, the code from the local planners in week 2 - Local Navigation was used to replace the path planner in this assignments framework. This meant that the global planner would provide the local navigation with its next target as defined by the A* algorithm. In this version the Vector Field Histogram method was used. From the simulation it became clear that parameters such as when a 'target' was reached needed to be tuned. Due to the local navigation avoiding obstacles along the way, the robot would sometimes 'miss' a vertex in the planned path. To mitigate this and keep the robot from needlessly searching for this one vertex, the radius in which a target was marked as reached was increased to xx. This meant that the robot only had to pass nearby in order to continue to the next one. This first version of global+local planning was tested in the lab by building the map_compare to scale. The full video can be seen here and a brief snippet is show in figure 5.

Week 5 + Week 6 - Localization

1. Week 5

1.1 Initializing the particle filter

1.2 Calculating the pose estimate

2. Week 6

2.1 Correcting particle with LiDAR

2.2 Re-sampling the particles