机器人局部避障的动态窗口法的运动轨迹分析和对比

机器人局部路径规划方法有很多，ROS中主要采用的是动态窗口法（但是和原论文中的dwa方法不一样，具体见最后）。动态窗口法主要是在速度（v,w）空间中采样多组速度，并模拟机器人在这些速度下一定时间（sim_period）内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加速性能限定速度采样空间在一个可行的动态范围内。

 

 

 

 

 

为了更加直观的感受速度如何采样以及如何排除会碰到障碍的速度，将速度采样的伪代码列车如下：
Sample velocities code demo:

首先在V_m∩V_d的范围内采样速度：  

allowable_v = generateWindow(robotV, robotModel)  

allowable_w  = generateWindow(robotW, robotModel)  

然后根据能否及时刹车剔除不安全的速度：  

for each v in allowable_v  

for each w in allowable_w  

dist = find_dist(v,w,laserscan,robotModel)  

breakDist = calculateBreakingDistance(v)//刹车距离  

if (dist > breakDist)  //如果能够及时刹车，该对速度可接收  

如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组  

同时注意：为了简化每组速度对应轨迹的计算，改算法假设机器人在往前模拟轨迹的这段时间（sim_period)内速度不变，直到下一时刻采样给定新的速度命令。

动态窗口采样的轨迹如下图所示：

 

 

 

总结起来三者构成的评价函数的物理意义是：在局部导航过程中，使得机器人避开障碍，朝着目标以较快速度行驶，缺一不可。

流程清楚以后，dwa算法的demo如下：

BEGIN DWA(robotPose,robotGoal,robotModel)  

laserscan = readScanner()  

allowable_v = generateWindow(robotV, robotModel)  

allowable_w  = generateWindow(robotW, robotModel)  

for each v in allowable_v  

for each w in allowable_w  

dist = find_dist(v,w,laserscan,robotModel)  

breakDist = calculateBreakingDistance(v)  

if (dist > breakDist)  //can stop in time  

heading = hDiff(robotPose,goalPose, v,w)   

//clearance与原论文稍不一样  

clearance = (dist-breakDist)/(dmax - breakDist)   

cost = costFunction(heading,clearance, abs(desired_v - v))  

if (cost > optimal)  

best_v = v  

best_w = w  

optimal = cost  

set robot trajectory to best_v, best_w  

END  

引申到ROS:

在ROS的dwa应用中，好像只用了窗口采样速度，到故障物的最小距离以及刹车距离都没有计算，如果某条轨迹上有障碍，那直接丢弃这条轨迹。并且ROS中的评价函数也不是用的这个，采用的是Gerkey的论文《planning and control in unstructured Terrain》中的评价函数。

参考：
dwa：
1.Fox.《The Dynamic Window Approach To CollisionAvoidance》
2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》
3.

运动模型：
4.
5. https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lectur...
6.

最后贴出matlab仿真代码：

% -------------------------------------------------------------------------  

%  

% File : DynamicWindowApproachSample.m  

%  

% Discription : Mobile Robot Motion Planning with Dynamic Window Approach  

%  

% Environment : Matlab  

%  

% Author : Atsushi Sakai  

%  

% Copyright (c): 2014 Atsushi Sakai  

%  

% License : Modified BSD Software License Agreement  

% -------------------------------------------------------------------------  

 

function [] = DynamicWindowApproachSample()  

 

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=[10,10];% 目标点位置 [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 2;  

4 2;  

4 4;  

5 4;  

5 5;  

5 6;  

5 9  

8 8  

8 9  

7 9  

6 5  

6 3  

6 8  

6 7  

7 4  

9 8  

9 11  

9 6];  

 

obstacleR=0.5;% 冲突判定用的障碍物半径  

global dt; dt=0.1;% 时间[s]  

 

% 机器人运动学模型  

% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],  

% 速度分辨率[m/s],转速分辨率[rad/s]]