matlab练习程序（DWA）

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;

结果如下：