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} = \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.

3.Principle of the repulsive force component

• Target attraction

Calculate the direction and distance from the robot's current position to the target position, and calculate the magnitude and direction of the target attraction based on the distance.

• Repulsion of an obstacle

The laser sensor data is traversed to calculate the distance from each obstacle to the robot, and the magnitude and direction of the repulsive force is calculated based on the distance. If the obstacle distance is less than a certain range, the repulsive force takes effect, causing the robot to avoid the obstacle.

• Total force calculation

The total force on the robot in the potential field is obtained by combining the target attraction and the repulsive force of all obstacles. The magnitude and direction of this total force represents the total force on the robot in the potential field.

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

Answers of the Questions

Artificial Potential Field

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

Advantages:

• The robot can walk smoothly to the destination and avoid obstacles.

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.

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.

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?