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

Real-Robot Results

Artificial Potential Field

In real-world robot operation tests, the robot traveled smoothly to its destination and was able to avoid obstacles to prevent collisions.

APF method on Bobo

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?

• 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.