Mobile Robot Control 2024 Ultron:Solution 2

Exercise 2: Local Navigation

Methodology

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