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;

結果如下：