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

机器人局部避障的动态窗口法的运动轨迹分析和对比机器人局部路径规划方法有很多,皿中主要采用的是动态窗口法(但是和原论文中的dwa方法不一样，具体见最后)。动态窗口法主要是在速度(v,w)空间中采样多组速度，并模担机器人在这些速度下一定时间(SimDeriod)内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该篁法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加速性能限定速度采样空间在一个可行的动态范围内。1机器人的运动模型在动态窗口算法中，要模拟机器人的轨迹,需要知道机器人的运动模型。它采用的是假设两轮移动机器人的轨迹是一段一段的圆弧或者直线(旋转速度为0时)，一对(外,")就代表一个圆弧轨迹。具体推导如"1.1模型1简电,程序中最常见,但不是动态窗口论文中的模型C首先假设机器人不是全向移动的.即不能纵向移动,只能前进和旋转(外,%).计算机器人轨迹时.先考虑两个相翎时刻,如下图所示：为简单起见,由于机器人相钠时刻(一般码盘采样周期ms计)内,运动更离短,因此可以将两相邻点之间的运动轨迹看成直线,即沿机忘人堂打,系X轴移动了/*t.只需将该段距离分别投影在世界坐标系X轴和y轴上就能得到t+1时刻相对于t时刻机器人在世界坐标系中坐标移动的位移x和AyOx=vtcos(f)Ay=tsin(0f)以此类推.如果你想推算一段时间内的轨迹,只需要将这段时间的位移增录索计求和就行了：x=x+UAtCoS(E)y=y+vtsin(t)tt-¥WACX如果机器人是全向运动的，有y轴速度，那轨迹如何推算呢？机器人坐标系的V轴有了速度,依朗芦面冢，只需将机器人在机器人生标y轴移动的距离投影到世界坐标系即可：x=ytcos(4+g)=vyrsin(0f)Ay=vyhtsin(4+g)=vyt>s(仇)此时的轨迹推演只需将y轴移动的柜离叠加在之前的计算公式上即可：x=x+vtcos(f)-vytsin(0t)y=y+vtsin(t)+vytcos(0t)在RoS匕飞遂推演中色是白用的该公式,baseOCa1PIanner的轨迹采样程序也是使用的该公式(1ine256insimp1e_trajectory_generator.cpp)：/htrc:/wiki.ros.org/navigation/iUtoria-s/?3：doub1edt-(current_tir.e-!astatine).toSec;32doub1edda_x-(v×cos(th)-vysin(th)*dt;33doub1ede1ta-y(vxsin(th)÷vy*cos(th)*dt;34doub1ede1ta-t-Vth"dt；×*deite-×36y"de"a_y;3!th-de1a-th;1.2模型2：上面的计菖中，假设相邻时间段内机器人的轨迹是直线.这是不准确的.更精确的做法是用圆弧来代替°正如dynamicwindowapproach论文原文中推导的那样.也如该博客卜.推导的那样(博客推导不严谨)：在这里，依然假设不是全向运动机器人，它做圆弧运动的半径为：Vr=w当旋转速度W不等于。时，机器人坐标为：VV、x=x-sin(0f)+sin(r+W1)yycos(e)cos(f+WAC)“&+ve13还有一些其他的推导方法,如文蛾5,6中得到的公式如下:（rad）xvtcos（夕+色）Ay=tSin（G+当）上面3个模型中,在程序中见得最多的就是模型1。2速度采样机器人的轨迹运动模型有了.根据速度就可以推算出轨迹.因此只需采样很多速度，推算轨迹，然后评价这些轨迹好不好就行了。速度如何采样是dynamicwindowapproach的第二个核心：在速度(1W)的二维空间中,存在无穷多组速度。但是根据机器人本身的限制和环境识剂可以将采样速度控制在一定范围内：2.1 移动机器人受自身最大速度最小速度的限制：Kn=»£%nJn，½n*1W£IWrnmWmaJ2.2 移动机器人受电机性能的影响：由于电机力矩有限,存在最大的加减速限制,因此移动机器人轨迹前向模拟的周期Simperiod内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：i-g.1ve-tvc+f1-"5u7c-tuzc÷tJ其中以是机器人的当前速度,其他标志对应最大加速度和最大减速度2.3 基于移动机器人安全的考虑：为了能绣在碰到障碍物前停下来,因此在最大减速度条件下，速度有一个范闱：,pr.fc)Ir<2<1ist(r.fc)eA3J'1<1ist(r,fc)J;1|其中dist(v,w)为速度(V,W)对应轨迹上离障礴物:'/*瀛；,由父线所示：-×2×注意：这个条件并不是在采样一开始”能得到的e翕要我们模拟出来机器人轨迹以后,找到用碍物位置，计算出机器人到障碍物之间的距离.然后看当前采样的这对速度能否在碰到障碍物之前停下来，如果能够停下来，那这对速度就是可接收的admissib1e.X：.洋/4ZEi速度就得抛弃掉C-Jw，心好。血即“为了更加直观的感受速度如何采样以及如何排除会碰到障碍的速度，将速度采样的伪代码列车如下：Samp1eve1ocitiescodedemo:首先在VmGV_d的范围内采样速度：a11owab1e_v=generateWindow(robotV,robotMode1)a11owab1e_w=generateWindow(robotW,robotMode1)然后根据能否及时刹车剔除不安全的速度：foreachvina11owab1e_vforeachwina11owab1e_wdist=find_dist(v,w,1aserscan,robotMode1)breakDist=Ca1CUIateBreakingDiStanCe(V)刹车距离if(dist>breakDist)如果能够及时刹车，该对速度可接收如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组同时注意：为了简化每组速度对应轨迹的计算，改算法假设机器人在往前模拟轨迹的这段时间(Si1T1period)内速度不变，直到下一时刻采样给定新的速度命令。动态窗口采样的轨迹如下图所示:3评价函数在采样的速度组中.有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹进行评价C在原始论文中,采用的评价函数如下：G(3)=o(cheading。，,9)+,dist(,s)+OCCitya2)3.1方位角评价函数heading(v,w)地用来N价机器人在当时设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距.如卜图所示：3.23.33.4朦论文中是采用的180。的方式来评价，也就是。越小，评价得分越«空隙distiv,w)代表机器人在当前轨迹上与最近的荫碍物之间的跑璃,如果在这条轨迹上没有国碍物,那就将其设定一个家式。速度ve1。City(V,w)用来评价当前轨迹的速度大小：平滑处理'：2“S!>H也就是：I-化，上面三个部分计算出来以后不是直2分在归一化以后，再相加。如何归一化？每一项除以每一项的总和:normaI,head(i)=norma1_vo1ocity(i)=head(i)1head(i)dist(i)屋山SF)ve1odty(i)".1ve1ocity(i)其中.n为采样的所有轨迹i为待评价的当前轨迹.为什么要归一化？归一化的目的是Smoothing:譬如障碍物距离.机器人传感器检测到的最小障碍物距离在二维空间中是不连续的.这条轨迹能够遇到阳碍.旁边那边不一定能遇到。并且这能这条轨迹最小的障碍物距离是1m,旁边那条就是IOm：那么障碍物距离的这种坪价标准导致评价函数不连续.也会导致某个项在评价函数中太占优势，如这里的离獐碍物距离IOm相对于Im就太占优势。所以将它们归一化：,.，、disc(i)M)"酝而而这样,都变成同一的百分比了,每个烽碍向最小,/笔？WxKG一份。M”1-7少总结起来三者构成的评价函数的物理意义是：在局部导航过程中，使得机器人避开障碍，朝着目标以较快速度行驶，缺一不可。流程清楚以后，dwa算法的demo如下:BEGINDWA(robotPose,robotGoa1,robotMode1)Iaserscan=readScanner()a11owab1e_v=generateWindow(robotV,robotMode1)a1Iowab1e_wgenerateWindow(robotW,robotMode1)foreachvina11owab1e_vforeachwina11owab1ewdist=find.dist(v,w,1aserscan,robotMode1)breakDist=Ca1cu1ateBreakingDistance(v)if(dist>breakDist)/canstopintimeheading=hDiff(robotPose,goa1Pose,v,w)/c1earance与原论文稍不一样c1earance=(dist-breakDist)/(dmax-breakDist)cost=CostFunction(heading,c1earance,abs(desired_vv)if(cost>optima1)best_v=vbest_w=woptima1=costsetrobottrajectorytobest_v,best_wEND引申到ROS:在RoS的C1Wa应用中，好像只用了窗口采样速度，到故障物的最小距离以及刹车距离都没有计算，如果某条轨迹上有障碍，那直接丢弃这条轨迹。并且RoS中的评价函数也不是用的这个，采用的是Gerkey的论文p1anningandcontro1inunstructuredTerrain中的评价函数。参考：dwa：1. Fox.TheDynamiCWindowApproachToCo11isionAvoidance2. MarijaSeder.dynamicwindowbasedapproachtomobi1erobotmotioncontro1inthepresenceofmovingobstac1es3.运动模型：4.5. https:www.cs.princeton.edu/courses/archive/fa1111cos495C0S495-1ectur.6.最后贴出mat1ab仿真代码:%Fi1e:DynamicwindowApproachSamp1e.m%Discription:Mobi1eRobotMotionP1anningwithDynamicWindowApproach%Environment:Mat1ab%Author:AtsushiSakai%Copyright(c):2014AtsushiSakai%1icense:ModifiedBSDSoftware1icenseAgreement%