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. This algorithm combines attractive and repulsive forces, and determines the direction and speed of the robot's movement by calculating the resultant force direction.

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.

• The formula for calculating the repulsive force

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

Fr is the magnitude of the repulsive force. k is a constant representing the maximum repulsive force. d 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 total repulsive force into components in the x and y directions

[math]\displaystyle{ F_{r_x} = F_r \cdot \cos(\theta + \pi) }[/math]

[math]\displaystyle{ F_{r_y} = F_r \cdot \sin(\theta + \pi) }[/math]

2.Principle of the repulsive 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

Using the coordinates of the current position and the target position, calculate the distance from the robot to the target point. Based on the distance, calculate the magnitude of the target attraction. The magnitude of the attraction is inversely proportional to the distance between the robot and the target, i.e., the further away from the target, the greater the attraction. The direction of the target attraction is calculated by calculating the angle of the direction of the target point relative to the current position.

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.

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