本文共 6597 字，大约阅读时间需要 21 分钟。

一、简介

DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给下位机。

机器人的轨迹运动模型有了，根据速度就可以推算出轨迹。

二、源代码

% -------------------------------------------------------------------------    function [] = ADynamicWindowApproachSample()     close all;  clear all;     disp('Dynamic Window Approach sample program start!!')    x=[0 0 pi/2 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]  goal=[12,5];% 目标点位置 [x(m),y(m)]  % 障碍物位置列表 [x(m) y(m)]  % obstacle=[0 2;  %           4 2;  %           4 4;  %           5 4;  %           5 5;  %           5 6;  %           5 9  %           8 8  %           8 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;          0 2.2;          0 2.3;          0 2.4;          0 2.5;          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型障碍物开始          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;%U型障碍物封口开始          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.9;          2 6;          2 5;          2 6.1;          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 7.1;          2 7.2;          2 7.3;          2 7.4;          2 7.5;          2 7.6;          2 7.7;          2 7.8;          2 7.9;          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;%           6 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 5.3;%           6 5.4;%           6 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.7;          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 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 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.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;          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 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;          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;%           2.1 3;%           2.2 3;%           2.3 3;%           2.4 3;%           2.5 3;%           2.6 3;%           2.7 3;%           2.8 3;%           2.9 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;%           3.9 3;%           4 3;%           4.1 3;%           4.2 3;%           4.3 3;

三、运行结果

四、备注

完整代码或者代写添加QQ1575304183

转载地址：https://blog.csdn.net/qq_34763204/article/details/116379654 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！