A list,

DWA algorithm is called Dynamic Window Approach. Its principle is to sample multiple sets of velocities in the velocity space (V, W) and simulate the movement trajectory of these velocities within a certain time. Then, these trajectory are scored by an evaluation function, and the optimal velocity is selected and sent to the next computer.

1 Principle Analysis







2 Speed sampling

The trajectory model of the robot is available, and the trajectory can be calculated according to the speed. So you just sample a lot of velocities, calculate trajectories, and evaluate how good those trajectories are. (a) mobile robot restricted by their own maximum speed minimum speed (2) of the mobile robot is affected by the motor performance: due to the motor torque is limited, there is the biggest limit acceleration and deceleration, so the cycle of the mobile robot path forward simulation sim_period inside, there is a dynamic window, the window is the speed of the robot can achieve actual speed: (3) Considering the safety of mobile robot: In order to stop before encountering obstacles, there is a range of speed under the condition of maximum deceleration.

Ii. Source code

% ------------------------------------------------------------------------- function [] = ADynamicWindowApproachSample()  close all; clear all; disp('Dynamic Window Approach sample program start!! ') x=[0 0 pi/2 0 0]'; % robot's initial state [x (m), y (m), yaw (Rad), v (m/s), w (Rad/s)] goal = (12, 5); % Target position [x(m),y(m)] % Obstacle position list [X (m) y(m)] % obstacle=[0 2; % 42; % 4 4; % 5 4; % 5 5; % 5 6; % 5 9%8 8 8 9 9 7 9]; Obstacle = [0.9 2; 0.8 2; 0.7 2; 0.6 2; 0.4 2; 0.2 2; 0.1 2; 0 2; 0 2.1; 2.2 0; 0 2.3; 0 2.4; 2.5 0; 0 2.6; 0, 2.7; 0 2.8; 0 2.9; 0 3; 0 3.1; 0 3.2; 0 3.3; 0 3.4; 0 3.5; 0 3.6; 0 3.7; 0 3.8; 0 3.9; 0 4; 0 4.1; 0 4.2; 0 4.3; 0 4.4; 0 4.5; 0 4.6; 0 4.7; 0 4.8; 0 4.9; 0 5; 0 5.1; 0 5.2; 0 5.3; 0 5.4; 0 5.5; 0 5.6; 0 5.7; 0 5.8; 0 5.9; 0 6; -0.1 6; -0.2 6, 0.3 6, 0.4 6, 0.5 6, 0.6 6, 0.7 6, 0.8 6, 0.9 6; 2 2; % U obstacles began to 2, 2.1; 2, 2.2, 2, 2.3, 2, 2.4, 2, 2.5, 2, 2.6, 2.7; 2 2.8; 2 2.9; 2 3; 2 3; 2 3;%U barrier sealing start 2 3.1; 2 3.2; 2 3.3; 2 3.4; 2 3.5; 2 3.6; 2 3.7; 2 3.8; 2 3.9; 2 4; 2 4; 2 4.1; 2 4.2; 2 4.3; 2 4.4; 2 4.5; 2 4.6; 2 4.7; 2 4.8; 2 4.9; 2 5; 2 5; 2 5.1; 2 5.2; 2 5.3; 2 5.4; 2 5.5; 2 5.6; 2 5.7; 2 5.8; 2 5.8 2 6.5; 2 6.5; 2 6.6; 2 6.7; 2 6.8; 2 6.9; 2 7; 2 7.1; 2 7.2; 2 7.3; 2 7.4; 2 7.5 2, 7.6, 2, 7.7, 2, 7.8, 7.9; 2 2 8, 6 3; % % 6 3.1; % 6 3.2; % 6 3.3; % 6 3.4; % 6 3.5; % 6 3.6; % 6 3.7; % 6 3.8; % 6 3.9; % % % 6, 4, 6, 4.1; 4.2; % 6 4.3; % 6 4.4; % 6 4.5; % 6 4.6; % 6 4.7; % 6 4.8; % 6 4.9; % 6 5; % 6 5.1; % 6 5.2; % 6 6 6 5.4; 5.3; % % 5.5; % 6 5.6; % 6 5.7; % 6 5.8; % 6 5.9; % 6 6; % 6 6.1; % 6 6.2; % 6 6.3; % 6 6.4; % 6 6.5; % 6 6.6; % 6 6.7; % 6 6.8; % 6 6.9; % 6 7; 7 2; 7 2.1; 7 2.2; 7 2.3; 7 2.4; 7 2.5; 7 2.6; 7 2.8; 7 2.9; 7 3; 7 3.1; 7 3.2 7 3.3; 7 3.4; 7 3.5; 7 3.6; 7 3.7; 7 3.8; 7 3.9; 7 4; 7 4.1; 7 4.2; 7 4.3; 7 4.4; 7 4.5; 7 4.6; 7 4.7; 7 4.8; 7 4.9; 7 4.9 5; 7 5.1; 7 5.2; 7 5.3; 7 5.4; 7 5.5; 7 5.6; 7 5.7; 7 5.8; 7 5.9; 7 6; 7 6.1; 7 6.2; 7 6.3; 7 6.4; 7 6.5; 7 6.6; 7 6.7 7 6.8; 7 6.9; 7 7 7.1; 7 7.2; 7 7.3; 7 7.4; 7 7.5; 7 7.6; 7 7.7; 7 7.8; 7 7.9; 7 8; 2 8; 2.1 8; 2.2 8; 2.3 8; 2.4 8; 2.5 8; 2.6 8; 2.7 8; 2.8 8; 2.9 8; 3 8; 3.1 8; 3.2 8; 3.3 8; 3.4 8; 3.5 8; 3.6 8; 3.7 8; 3.8 8; 3.9 8; 4 8; 4.1 8; 4.2 8; 4.3 8; 4.4 8; 4.5 8; 4.6 8; 4.7 8; 4.8 8; 4.8 8; 4.9 8; 5 8; 5.1 8; 5.2 8; 5.3 8; 5.4 8; 5.5 8; 5.6 8; 5.7 8; 5.8 8; 5.9 8; 5.9 8; 5.9 8 6 8; 6.1 8; 6.2 8; 6.3 8; 6.4 8; 6.5 8; 6.6 8; 6.7 8; 6.8 8; 6.9 8; 7 8; 2 2; 2.1 2; 2.2 2; 2.3 2; 2.4 2; 2.5 2; 2.6 2 2.7 2; 2.8 2; 2.9 2; 3 2; 3.1 2; 3.2 2; 3.3 2; 3.4 2; 3.5 2; 3.6 2; 3.7 2; 3.8 2; 3.9 2; 4 2; 4.1 2; 4.2; 4.3 2; 4.4 2; 4.5 2; 4.6 2; 4.7 2; 4.8 2; 4.9 2; 5 2; 5.1 2; 5.2 2; 5.3 2; 5.4 2; 5.5 2; 5.6 2; 5.7 2; 5.8 2; 5.9 2; 6 2; 6.1 2; 5.3 2 6.2 2; 6.3 2; 6.4 2; 6.5 2; 6.6 2; 6.7 2; 6.8 2; 6.9 2; 7, 2, 2, 3; % 3; 2.1%3; 2.2%3; 2.3%3; 2.4 2.6 3; % % 2.5 3; % % % 2.7 3, 2.8 3; 2.9 3; % 3 3; % 3; 3.1%3; 3.2%3; 3.3%3; 3.4%3; 3.5%3; 3.6%3; 3.7%3; 3.8%3.9 3, % 4 3; % 4.1 3; % 4.2 3; % 4.3 3;Copy the code

3. Operation results



Fourth, note

Complete code or write to add QQ1575304183