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

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

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

Samplevelocitiescodedemo:首先在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的论文《planningandcontrolinunstructuredTerrain》中的评价函数。参考：

dwa：

1.Fox.《TheDynamicWindowApproachToCollisionAvoidance》

2.MarijaSeder.《dynamicwindowbasedapproachtomobilerobotmotioncontrolinthepresenceofmovingobstacles》

3.运动模型：

4.

5./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