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 can be a predefined fixed point or dynamically specified.

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

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