Mobile Robot Control 2024 Ultron:Solution 2

Exercise 2: Local Navigation

Methodology

Artificial Potential Field

The Artificial Potential Field (APF) algorithm achieves obstacle avoidance and navigation by simulating a potential field. The algorithm combines an attractive force on the target and a repulsive force on the obstacle and determines the direction and speed of the robot's motion by calculating the direction of the resulting force.

1.Principle of the repulsive force component

To prevent the robot from hitting obstacles. This approach draws on the concepts of electromagnetic fields and physical force fields, where obstacles are viewed as "charges" or "sources" of repulsive forces, allowing the robot to avoid them. If the distance from the robot to the obstacle is within a certain range, the repulsion is effective, otherwise the repulsion is ineffective

• The formula for calculating the repulsive force

[math]\displaystyle{ F_r = \frac{F_{max}}{d^2} }[/math]

Where:

[math]\displaystyle{ F_r }[/math] is the magnitude of the repulsive force.

[math]\displaystyle{ F_{max} }[/math] is a constant representing the maximum repulsive force.

[math]\displaystyle{ d }[/math] is the distance from the obstacle to the robot. The magnitude of the repulsive force is inversely proportional to the square of the distance to the obstacle. This means that the closer the obstacle, the greater the repulsive force.

• Decompose the repulsive force into components in the x and y directions. Calculate the force components in the x and y directions using trigonometry. The repulsive force of each obstacle is then accumulated to the total force.

[math]\displaystyle{ F_{\text{total}_x} = \sum_{i=1}^{n} \left( \frac{F_{\text{max}}}{d_i^2} \cdot \cos(\theta_i + \pi) \right) }[/math]

[math]\displaystyle{ F_{\text{total}_y} = \sum_{i=1}^{n} \left( \frac{F_{\text{max}}}{d_i^2} \cdot \sin(\theta_i + \pi) \right) }[/math]

Where:

[math]\displaystyle{ F_{\text{total}_x} }[/math] and [math]\displaystyle{ F_{\text{total}_y} }[/math] are the total repulsive forces in the x and y directions, respectively.

[math]\displaystyle{ d_i }[/math] is the distance from the robot to the[math]\displaystyle{ i }[/math]th obstacle.

[math]\displaystyle{ \theta_i }[/math] is the angle of the [math]\displaystyle{ i }[/math]th obstacle relative to the robot.

[math]\displaystyle{ n }[/math] is the total number of obstacles.

2.Principle of the attractive force component

• Determine target and current position

The coordinates of the robot's current location and the target location need to be determined. The current position is usually provided by the robot's odometer data, while the target position is a predefined fixed point.

• Calculate the size and direction of the target attraction

First, a parameter is initialised as the size of the attraction. The direction angle of the target position with respect to the current robot position is calculated using the atan2 tangent function. This direction angle indicates the direction in which the robot should move towards the target.

[math]\displaystyle{ \text{attractive_direction} = \text{atan2}(target\_y - current\_y, target\_x - current\_x) }[/math]

Where:

[math]\displaystyle{ \text{atan2}(y, x) \ }[/math] represents the arctangent function, which returns the direction angle relative to the origin for the point.

[math]\displaystyle{ (target\_x, target\_y) }[/math] are the coordinates of the target position.

[math]\displaystyle{ (current\_x, current\_y) }[/math] are the coordinates of the current robot position.

• The attractive force is decomposed into components in the x and y directions and then accumulated in the total force. In this way, the robot is attracted by the force of attraction from the target and moves towards the target

[math]\displaystyle{ F_{\text{total}_x} =\left( F_{\text{attractive}} \cdot \cos(\text{attractive_direction}) \right) }[/math]

[math]\displaystyle{ F_{\text{total}_y} = \left( F_{\text{attractive}} \cdot \sin(\text{attractive_direction}) \right) }[/math]

Where:

[math]\displaystyle{ F_{\text{total}_x} }[/math] and [math]\displaystyle{ F_{\text{total}_y} }[/math] are the total attractive forces in the x and y directions, respectively.

[math]\displaystyle{ F_{attractive} }[/math] is the magnitude of the attractive force, usually a constant value.

[math]\displaystyle{ \text{attractive_direction} }[/math] is the direction of the attractive force, representing the angle at which the robot is attracted towards the target.

3.Total Force Calculation:

• Sum up the x components of both attractive and repulsive forces to obtain the total force in the x direction

• Sum up the y components of both attractive and repulsive forces to obtain the total force in the y direction

• The total force on the robot is then represented by these two components, indicating both the direction and magnitude of the force.

4. Coordinate system transformation principle

Updates location information and adjusts the current location based on new movement data. Returns updated position information.

// Overloaded operator+= function
emc::OdometryData& operator+=(emc::OdometryData& start, const emc::OdometryData& move) {
    start.x += move.x * std::cos(start.a) - move.y * std::sin(start.a);
    start.y += move.x * std::sin(start.a) + move.y * std::cos(start.a);
    start.a += move.a;
    return start;
}

Dynamic Window Approach

The Dynamic Window Approach (DWA) algorithm simulates motion trajectories in velocity space [math]\displaystyle{ (v, \omega) }[/math] for a certain period of time. It evaluates these trajectories using an evaluation function and selects the optimal trajectory corresponding to [math]\displaystyle{ (v, \omega) }[/math] to drive the robot's motion.

Consider velocities which have to be

• Possible: velocities are limited by robot’s dynamics

[math]\displaystyle{ V_s = \{(v, \omega) \mid v \in [v_{\min}, v_{\max}] \land \omega \in [\omega_{\min}, \omega_{\max}]\} }[/math]

• Admissible: robot can stop before reaching the closest obstacle

[math]\displaystyle{ V_a = \{(v, \omega) \mid v \leq \sqrt{2 d(v, \omega) \dot{v_b}} \land \omega \leq \sqrt{2 d(v, \omega) \dot{\omega_b}}\} }[/math]

• Reachable: velocity and acceleration constraints (dynamic window)

[math]\displaystyle{ V_d = \{(v, \omega) \mid v \in [v_a - \dot{v} t, v_a + \dot{v} t] \land \omega \in [\omega_a - \dot{\omega} t, \omega_a + \dot{\omega} t]\} }[/math]

Intersection of possible, admissible and reachable velocities provides the search space: [math]\displaystyle{ V_r = V_s \cap V_a \cap V_d }[/math]

   for k = 1:len(ω_range)
       for i = 0:N
           x(i + 1) = x(i) + Δt * v_range(j) * cos(θ(i))
           y(i + 1) = y(i) + Δt * v_range(j) * sin(θ(i))
           θ(i + 1) = θ(i) + Δt * ω_range(k)
       end
   end

Then the objective function is introduced to score the trajectories and select the optimal trajectory.

[math]\displaystyle{ G(v, \omega) = \sigma ( k_h h(v, \omega) + k_d d(v, \omega) + k_s s(v, \omega) ) }[/math]

• [math]\displaystyle{ h(v, \omega) }[/math]: target heading towards goal
• [math]\displaystyle{ d(v, \omega) }[/math]: distance to closest obstacle on trajectory
• [math]\displaystyle{ s(v, \omega) }[/math]: forward velocity

Simulation Results

Artificial Potential Field

As shown in APF_simulation_map4, the Artificial Potential Field (APF) was tested on a given map. The robot starts from the center of the map and continues to rotate left and right to avoid obstacles as it travels, bypassing the fourth and fifth obstacles to get to its destination. However, if the robot starts at the bottom of the map and is blocked by a long obstacle, the dont_crash function is triggered, causing the robot to stop.

Dynamic Window Approach

As shown in DWA_simulation, the Dynamic Window Approach (DWA) was tested on a given map. The robot started moving and successfully avoided the first obstacle in its path. As it traveled, the robot also successfully avoided the second long obstacle and passed through the first big curve by turning left without any problem. However, the robot was stopped for some time by a third obstacle, during which the robot made a slow right turn to find the correct direction. After finding the right direction, the robot quickly bypassed the obstacle and reached its destination.

Real-Robot Results

Artificial Potential Field

As shown in APF method on Bobo, in real-world robot operation tests, the robot traveled smoothly to its destination and was able to avoid obstacles to prevent collisions.

In a relatively narrow passage, the BOBO robot continuously rotates left and right during its movement to avoid surrounding obstacles. After navigating through a densely obstructed area, the robot moves directly in a straight line towards the destination.

Answers of the Questions

Artificial Potential Field

1. What are the advantages and disadvantages of your solutions?

Advantages:

• The algorithms are simple and easy to understand and follow. The robot can walk smoothly to the destination and avoid obstacles.
• When the algorithm is running on the robot, the repulsive part of the algorithm ensures that the robot will not hit the obstacles around it, and even if a new obstacle suddenly appears on the map, the robot can react quickly without collision.

Disadvantages:

• When the robot reaches the center line between the two walls, it will keep turning and swinging left and right.
• When the robot reaches the local optimum point, the robot is trapped there.
• The performance of the algorithm is strongly influenced by the maximum attraction and maximum repulsion parameters. If the parameters are not adjusted properly, it is likely to lead to bad performance.

2. What are possible scenarios which can result in failures (crashes, movements in the wrong direction, ...) for each implementation?

• When the robot tries to pass through the narrow passage, it keeps rotating left and right at the centreline position of the passage causing it to get trapped.

3. How would you prevent these scenarios from happening?

• The problem of the robot being trapped in a locally optimal position is inherent to local navigation algorithms. This is because APF does not consider the global environment. To solve this problem, global path planning using either the A* algorithm or the Dijkstra algorithm is required.
• By adjusting the parameters of the attraction and repulsion potential fields, the shape and strength of the potential field can be changed. Increasing the range and strength of the attraction potential field can make the robot more inclined to move towards the target; adjusting the range of the repulsion potential field can prevent the robot from being subjected to too strong a repulsive force and falling into a local optimum point.
• Introducing small random perturbations in the motion control of a robot can help the robot jump out of the local optimal point.

4. For the final challenge, you will have to link local and global navigation. The global planner will provide a list of (x, y) (or (x, y, θ) ) positions to visit. How could these two algorithms be combined?

• The global planner computes an approximate path from the starting point to the destination, a process that generates a list of (x, y) coordinate points. The local planner then moves along these points towards the destination while avoiding obstacles and preventing collisions.

Dynamic Window Approach

1. What are the advantages and disadvantages of your solutions?

Advantages:

• Robots can effectively avoid obstacles detected in real-time, respond to environmental changes dynamically, adjust their speed and direction, and reach their destination while navigating around obstacles. This makes them suitable for path planning in dynamic environments.

Disadvantages:

• The robot does not always find the optimal path to their goal, especially in complex structures or long-distance environments.
• The acceleration and angular acceleration parameters of the algorithm have to be adjusted to achieve good results in robot testing. If these parameters are too large, the robot will not be able to stop in time at the position close to the obstacle thus leading to a collision.
• When the robot reaches the local optimum point, the robot is trapped there.

2. What are possible scenarios which can result in failures (crashes, movements in the wrong direction, ...) for each implementation?

• It is possible for the robot to fall into a local minimum and stop moving there.

3. How would you prevent these scenarios from happening?

• By combining the global planner with DWA, the robot will not be trapped in a local optimal point.

4. For the final challenge, you will have to link local and global navigation. The global planner will provide a list of (x, y) (or (x, y, θ) ) positions to visit. How could these two algorithms be combined?

• The global planner computes an approximate path from the starting point to the destination, a process that generates a list of (x, y) coordinate points. The local planner then moves along these points towards the destination while avoiding obstacles and preventing collisions.