Mobile Robot Control 2024 Ultron:Solution 2

From Control Systems Technology Group
Jump to navigation Jump to search

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 is as follows:

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



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

Testing Results