DWA英文全称Dynamic Window Approach即动态窗口方法，是一种局部路径规划算法，主要用来做局部避障。
算法原理是对当前速度和角速度在一定范围内进行采样，并对采样后的参数在一定时间内进行预测生成多组路径，计算每条路径的权重，最后选择最优权重的路径对应的速度和角速度输出给下级模块。
路径权重可以分为三部分：
1. 目标位置权重，生成的路径距离目标位置越近，则该路径越优。
2. 障碍物权重，生成的路径与障碍物相交，则可以舍弃该路径。
3. 全局路径权重，生成的局部路径距离全局路径规划给出的路径越近，则该路径越优。
常见的权重一般就以上三种，不过还可以根据实际需要设置特殊的权重，以便满足个性化需要。
下面给一个非常精简的版本。
程序是对前向速度在0.1:2范围内采样，角速度在-30:30范围采样，生成0.5秒内的路径，再判断路径的优劣（判断没有利用全局路径），最终选择最优路径。
matlab代码如下：
clear all;close all;clc; x = 0;y = 0; theta = -pi/2; %初始位姿 goalx = 40;goaly = 40; %目标位置 xv = [10;30;30;10;10]; %设两个障碍物 yv = [10;10;30;30;10]; xv1 = [35;40;40;35;35]; yv1 = [25;25;30;30;25]; plot(x,y,\'r*\') hold on; plot(goalx,goaly,\'g*\'); plot(xv,yv,\'b\') plot(xv1,yv1,\'b\') dt = 0.1; sig = 1; while(norm([x y]-[goalx goaly])>0.1) %判断是否到达目的地 selectv = 0; selectw = 0; W = 1000000; for v = 0.1:0.2:2 %只考虑前向速度 for w = -30:5:30 %角速度采样 path = zeros(6,2); ind = 0; for t = 0:0.1:0.5 %生成多条轨迹线 ind = ind+1; dtheta = w*pi/180.0 * t; dx = v*t*cos(theta+dtheta); dy = v*t*sin(theta+dtheta); path(ind,:)=[x+dx y+dy]; end d = [goalx goaly]-path; w1 = sum(sqrt(d(:,1).^2+d(:,2).^2)); %轨迹距离目的地距离，作为一个权重 in = inpolygon(path(:,1),path(:,2),xv,yv); %判断轨迹线是否通过障碍物 in2 = inpolygon(path(:,1),path(:,2),xv1,yv1); if sum(in)>0 || sum(in2)>0 %轨迹与障碍物相交，舍弃该路径 continue; end weight = w1; %这里只考虑最终位置和障碍物，没有考虑全局路径 if weight<W %选择权重最小的轨迹对应的线速度和角速度 W = weight; selectv = v; selectw = w; sig = sign(w); end end end if selectv==0 && selectw==0 %如果所有线束都在障碍物中，则延之前路径趋势旋转一定角度 theta = theta + sig*5*pi/180.0; continue; end theta = theta + selectw*pi/180.0*dt; %位姿递推 x = x+selectv*dt*cos(theta); y = y+selectv*dt*sin(theta); plot(x,y,\'r.\'); end axis equal;
结果如下：
请发表评论