路径规划局部路径规划算法——DWA算法（动态窗口法）（含python实现c++实现）

见贤思齐

文章目录参考资料1.DWA算法原理1.1简介1.2算法原理1.速度采样2.轨迹预测（轨迹推算）3.轨迹评价2.Python实现2.1参数配置2.2机器人运动学模型2.3DWA算法类实现2.4画图2.5主函数3.c++实现4.总结参考资料TheDynamicWindowApproachtoCollisionAvoidance基于室内多障碍物复杂环境的路径规划方法研究基于改进A*算法与改进DWA算法的无人驾驶汽车路径规划研究室内移动机器人路径规划方法研究机器人局部避障的动态窗口法1.DWA算法原理1.1简介动态窗口算法(DynamicWindowApproaches,DWA)是基于预测控制理论的一种次优方法，因其在未知环境下能够安全、有效的避开障碍物,同时具有计算量小,反应迅速、可操作性强等特点。DWA算法属于局部路径规划算法。DWA算法的核心思想是根据移动机器人当前的位置状态和速度状态在速度空间(v,ω)(v,\omega)(v,ω)中确定一个满足移动机器人硬件约束的采样速度空间，然后计算移动机器人在这些速度情况下移动一定时间内的轨迹，并通过评价函数对该轨迹进行评价，最后选出评价最优的轨迹所对应的速度来作为移动机器人运动速度，如此循环直至移动机器人到达目标点。对于无人驾驶汽车而言，情况类似，将车辆的位置变化转化为线速度和角速度控制，避障问题转变成空间中的运动约束问题，这样可以通过运动约束条件选择局部最优的路径。1.2算法原理DWA算法的思路是：先通过机器人数学模型采集机器人速度样本，并预测模拟出在样本速度下一段时间内生成的运动轨迹，并对这些运动轨迹进行标准评价，选择出一组最优轨迹，机器人按照最优轨迹运动。机器人的运动姿态和方向是由机器人当前的线速度及角速度(转向速度)共同决定的。DWA算法主要包括3个步骤：速度采样、轨迹预测（推算）、轨迹评价。1.速度采样由于移动机器人硬件、结构和环境等限制条件，移动机器人的速度采样空间Vs\mathbf{V}_{\mathrm{s}}Vs​中(v,ω)(v,\omega)(v,ω)有一定的范围限制。该限制主要分为三大类：速度边界限制根据移动机器人的硬件条件和环境限制,移动机器人的速度存在的边界限制,此时可采样的速度空间Vm\mathbf{V}_mVm​为Vm={(v,ω)∣v∈[vmin ,vmax ],ω∈[ωmin ,ωmax ]}(1)\tag{1}\mathbf{V}_m=\left\{(v,\omega)\midv\in\left[v_{\text{min}},v_{\text{max}}\right],\omega\in\left[\omega_{\text{min}},\omega_{\text{max}}\right]\right\}Vm​={(v,ω)∣v∈[vmin ​,vmax ​],ω∈[ωmin ​,ωmax ​]}(1)式中vmin 、vmax v_{\text{min}}、v_{\text{max}}vmin ​、vmax ​分别为移动机器人最小线速度和最大线速度,ωmin 、ωmax⁡\omega_{\text{min}}、\omega_{\max}ωmin ​、ωmax​分别为移动机器人最小角速度和最大角速度。加速度限制由于移动机器人的驱动电机的限制，故移动机器人的线加速度和角加速度均存在边界限制，假设最大加速度和减速度大小一样，故考虑加速度时可采样的速度空间Vd\mathbf{V}_dVd​为Vd={(v,ω)∣v∈[vc−avmax⁡⋅Δt,vc+avmax⁡∙Δt]ω∈[ωc−awmax⁡∙Δt,ωc+awmax⁡∙Δt]}(2)\tag{2}\mathbf{V}_d=\left\{(v,\omega)\midv∈[vc−avmax⋅Δt,vc+avmax∙Δt]ω∈[ωc−awmax∙Δt,ωc+awmax∙Δt]v∈[vc−avmax⋅Δt,vc+avmax∙Δt]ω∈[ωc−awmax∙Δt,ωc+awmax∙Δt]\right\}Vd​={(v,ω)∣v∈[vc​−avmax​⋅Δt,vc​+avmax​∙Δt]ω∈[ωc​−awmax​∙Δt,ωc​+awmax​∙Δt]​}(2)式中vc、ωcv_c、\omega_cvc​、ωc​分别为移动机器人当前时刻的线速度和角速度，avmax⁡、aw max a_{v\max}、a_{w\text{max}}avmax​、aw max ​分别为移动机器人最大线加速度和最大角加速度。环境障碍物限制局部规划还需要有动态实时的避障功能。考虑移动机器人的周围的障碍物因素，某一时刻移动机器人不与周围障碍物发生碰撞的可约束条件为Va={(v,ω)∣v∈[vmin⁡,2⋅dist(v,ω)⋅avmax⁡]ω∈[ωmin⁡,2⋅dist(v,ω)⋅aωmax⁡]}(3)\tag{3}\mathbf{V}_a=\left\{(v,\omega)\midv∈[vmin,2⋅dist(v,ω)⋅avmax−−−−−−−−−−−−−−−√]ω∈[ωmin,2⋅dist(v,ω)⋅aωmax−−−−−−−−−−−−−−−−√]v∈[vmin,2⋅dist(v,ω)⋅avmax]ω∈[ωmin,2⋅dist(v,ω)⋅aωmax]\right\}Va​=⎩⎨⎧​(v,ω)∣v∈[vmin​,2⋅dist(v,ω)⋅avmax​​]ω∈[ωmin​,2⋅dist(v,ω)⋅aωmax​​]​⎭⎬⎫​(3)式中dist⁡(v,ω)\operatorname{dist}(v,\omega)dist(v,ω)表示当前速度下对应模拟轨迹与障碍物之间的最近距离。在无障碍物的情况下dist⁡(v,ω)\operatorname{dist}(v,\omega)dist(v,ω)会是一个很大的常数值。当机器人运行采样速度在公式(3)范围时,能够以最大减速度的约束实现安全减速直至避开障碍物。注意:这个限制条件在采样初期是得不到的，需要我们先使用Vm∩Vd\mathbf{V}_m\cap\mathbf{V}_dVm​∩Vd​的速度组合采样模拟出轨迹后,计算当前速度下对应模拟轨迹与障碍物之间的最近距离,然后看当前采样的这对速度能否在碰到障碍物之前停下来，如果能够停下来,那这对速度就是可接收的。如果不能停下来,这对速度就得抛弃掉。我在代码中并没有采用这种做法，而是直接计算机器人当前位置（并不是模拟轨迹）与障碍物的最近距离来得到Va\mathbf{V}_aVa​，算是一种比较投机的做法。结合上述三类速度限制，最终的移动机器人速度采样空间是三个速度空间的交集，即Vs=Vm∩Vd∩Va(4)\tag{4}\mathbf{V}_s=\mathbf{V}_m\cap\mathbf{V}_d\cap\mathbf{V}_aVs​=Vm​∩Vd​∩Va​(4)2.轨迹预测（轨迹推算）在确定速度采样空间Vs\mathbf{V}_sVs​后，DWA算法以一定的采样间距（分辨率）在该空间均匀采样。在速度空间中,分别对线速度和角速度设置分辨率,分别用Ew、EvE_w、E_vEw​、Ev​表示采样分辨率，那么采样速度组的个数就可以确定下来,如下式所示。n=[(vhigh−vlow)/Ev]⋅[(whigh−wlow)/Ew]n=\left[\left(v_{high}-v_{low}\right)/E_v\right]\cdot\left[\left(w_{high}-w_{low}\right)/E_w\right]n=[(vhigh​−vlow​)/Ev​]⋅[(whigh​−wlow​)/Ew​]式中的vhigh,vlow,whigh,wlowv_{high},v_{low},w_{high},w_{low}vhigh​,vlow​,whigh​,wlow​是速度空间的上下限。上式说明线速度每间隔一个EvE_vEv​大小取一个值,角速度每间隔一个EwE_wEw​大小取一个值，由此组成了一系列的速度组。当采样了一组(v,ω)(v,\omega)(v,ω)后,通过移动机器人（无人车辆）的运动学模型进行轨迹预测（即位置更新）。因此，在轨迹预测阶段，只需要知道机器人（无人车辆）的运动学模型即可，然后在预测时间内保存预测轨迹便于后面处理。有关无人车的运动学模型可参考这篇博客。这边不妨使用差分驱动移动机器人的运动学模型，那么轨迹预测就是计算下式：xk=xk−1+v⋅cos⁡(θk−1)Δtyk=yk−1+v⋅sin⁡(θk−1)Δtθk=θk−1+ωΔtxkykθk=xk−1+v⋅cos(θk−1)Δt=yk−1+v⋅sin(θk−1)Δt=θk−1+ωΔtxk=xk−1+v⋅cos⁡(θk−1)Δtyk=yk−1+v⋅sin⁡(θk−1)Δtθk=θk−1+ωΔtxk​yk​θk​​=xk−1​+v⋅cos(θk−1​)Δt=yk−1​+v⋅sin(θk−1​)Δt=θk−1​+ωΔt​式中，(x,y,θ)(x,y,\theta)(x,y,θ)代表机器人的位姿，kkk代表采样时刻，Δt\DeltatΔt表示采样间隔。上面轨迹推算这块是我比较肤浅的理解，因为按照原论文是需要假设相邻时间段内机器人的轨迹是圆弧，然后再进行轨迹推算，感兴趣的朋友可以直接查看原论文。3.轨迹评价确定了机器人约束速度范围后，有一些速度模拟的轨迹是可行的，但是还有不达标的轨迹，这需要对采样得到的多组轨迹进行评价择优。通过标准评价轨迹，比较评分来选出最优轨迹，然后选取最优轨迹对应的速度作为驱动速度。对每条轨迹进行评估的评价函数如公式(5)(5)(5)所示。G(v,ω)=σ(α⋅heading⁡(v,ω))+σ(β⋅dist⁡(v,ω))+σ(γ⋅velocity⁡(v,ω))(5)\tag{5}G(v,\omega)=\sigma(\alpha\cdot\operatorname{heading}(v,\omega))+\sigma(\beta\cdot\operatorname{dist}(v,\omega))+\sigma(\gamma\cdot\operatorname{velocity}(v,\omega))G(v,ω)=σ(α⋅heading(v,ω))+σ(β⋅dist(v,ω))+σ(γ⋅velocity(v,ω))(5)heading(v,ω)(v,\omega)(v,ω)是方位角评价函数，用作评估在当前采样速度下产生的轨迹终点位置方向与目标点连线的夹角的误差Δθ\Delta\thetaΔθ；由于我们想要用评价函数越大表示越优，所以用π−Δθ\pi-\Delta\thetaπ−Δθ来参与评价，即heading(v,ω)=π−Δθheading(v,\omega)=\pi-\Delta\thetaheading(v,ω)=π−Δθ。dist⁡(v,ω)\operatorname{dist}(v,\omega)dist(v,ω)是距离评价函数，表示当前速度下对应模拟轨迹与障碍物之间的最近距离；如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。velocity(v,ω)(v,\omega)(v,ω)是速度评价函数，表示当前的速度大小，可以直接用当前线速度的大小来表示。它越大，表示规划轨迹上的速度越快，评价得分越高。以上三种评价函数只是给了个大体的意思，并不绝对，例如有的人是把评价函数作为代价，代价越小，轨迹越优。可根据自己的想法进行评价函数的设置，但无论怎么变，这三种评价函数都是需要的。α、β\alpha、\betaα、β和γ\gammaγ均为评价函数的系数。由于局部路径规划的过程需要多传感器的采集,采集信息无法做到连续,这样也会使得评价后差别较大,所以可以进行归一化处理（平滑处理），其中σ\sigmaσ表示归一化。归一化处理过程如下式(6)所示:σ⋅ heading (v,ω)=normalize-heading(i)=heading⁡(i)∑i=1nheading⁡(i)σ⋅dist⁡(v,ω)= normalize-dist (i)=dist⁡(i)∑i=1ndist⁡(i)σ⋅velocity⁡(v,ω)= normalize-velocity (i)= velocity (i)∑i=1n velocity (i)(6)\tag{6}σ⋅ heading (v,ω)=normalize-heading(i)=heading(i)∑ni=1heading(i)σ⋅dist(v,ω)= normalize-dist (i)=dist(i)∑ni=1dist(i)σ⋅velocity(v,ω)= normalize-velocity (i)= velocity (i)∑ni=1 velocity (i)σ⋅ heading (v,ω)=normalize-heading(i)=heading⁡(i)∑i=1nheading⁡(i)σ⋅dist⁡(v,ω)= normalize-dist (i)=dist⁡(i)∑i=1ndist⁡(i)σ⋅velocity⁡(v,ω)= normalize-velocity (i)= velocity (i)∑i=1n velocity (i)​σ⋅ heading (v,ω)=normalize-heading(i)=∑i=1n​heading(i)heading(i)​σ⋅dist(v,ω)= normalize-dist (i)=∑i=1n​dist(i)dist(i)​σ⋅velocity(v,ω)= normalize-velocity (i)=∑i=1n​ velocity (i) velocity (i)​​(6)其中，iii代表第iii条模拟轨迹，nnn为约束条件下的全部采样轨迹总数。由上述公式(5)和公式(6)可以得出一条满足避开障碍物并朝着目标点快速行进的路径，使得机器人完成局部路径最优规划。DWA算法的流程如下图所示。2.Python实现完整程序请移步github仓库。本次代码的参数配置和画图代码参考了AtsushiSakai/PythonRobotics。2.1参数配置使用一个类来保存需要设置的参数。importnumpyasnpimportmatplotlib.pyplotaspltimportcopyfromcelluloidimportCamera#保存动图时用，pipinstallcelluloidimportmathclassConfig:"""simulationparameterclass"""def__init__(self):#robotparameter#线速度边界self.v_max=1.0#[m/s]self.v_min=-0.5#[m/s]#角速度边界self.w_max=40.0*math.pi/180.0#[rad/s]self.w_min=-40.0*math.pi/180.0#[rad/s]#线加速度和角加速度最大值self.a_vmax=0.2#[m/ss]self.a_wmax=40.0*math.pi/180.0#[rad/ss]#采样分辨率self.v_sample=0.01#[m/s]self.w_sample=0.1*math.pi/180.0#[rad/s]#离散时间间隔self.dt=0.1#[s]Timetickformotionprediction#轨迹推算时间长度self.predict_time=3.0#[s]#轨迹评价函数系数self.alpha=0.15self.beta=1.0self.gamma=1.0#Alsousedtocheckifgoalisreachedinbothtypesself.robot_radius=1.0#[m]forcollisioncheckself.judge_distance=10#若与障碍物的最小距离大于阈值（例如这里设置的阈值为robot_radius+0.2）,则设为一个较大的常值#障碍物位置[x(m)y(m),....]self.ob=np.array([[-1,-1],[0,2],[4.0,2.0],[5.0,4.0],[5.0,5.0],[5.0,6.0],[5.0,9.0],[8.0,9.0],[7.0,9.0],[8.0,10.0],[9.0,11.0],[12.0,13.0],[12.0,12.0],[15.0,15.0],[13.0,13.0]])#目标点位置self.target=np.array([10,10])12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758值得注意的是，这边障碍物只给了位置，并没有给定大小，因为这边相当于把障碍物的大小合并到了机器人本体大小上，也即代码中的robot_radius上。2.2机器人运动学模型主要用于轨迹推算。defKinematicModel(state,control,dt):"""机器人运动学模型Args:state(_type_):状态量---x,y,yaw,v,wcontrol(_type_):控制量---v,w,线速度和角速度dt(_type_):离散时间Returns:_type_:下一步的状态"""state[0]+=control[0]*math.cos(state[2])*dtstate[1]+=control[0]*math.sin(state[2])*dtstate[2]+=control[1]*dtstate[3]=control[0]state[4]=control[1]returnstate12345678910111213141516171819'运行运行在这里顺便保存了线速度和角速度作为状态分量，便于后面轨迹计算。2.3DWA算法类实现通过一个类来实现DWA算法的速度采样、轨迹预测、轨迹评价三个主要步骤。classDWA:def__init__(self,config)->None:"""初始化Args:config(_type_):参数类"""self.dt=config.dtself.v_min=config.v_minself.w_min=config.w_minself.v_max=config.v_maxself.w_max=config.w_maxself.predict_time=config.predict_timeself.a_vmax=config.a_vmaxself.a_wmax=config.a_wmaxself.v_sample=config.v_sample#线速度采样分辨率self.w_sample=config.w_sample#角速度采样分辨率self.alpha=config.alphaself.beta=config.betaself.gamma=config.gammaself.radius=config.robot_radiusself.judge_distance=config.judge_distancedefdwa_control(self,state,goal,obstacle):"""滚动窗口算法入口Args:state(_type_):机器人当前状态--[x,y,yaw,v,w]goal(_type_):目标点位置，[x,y]obstacle(_type_):障碍物位置，dim:[num_ob,2]Returns:_type_:控制量、轨迹（便于绘画）"""control,trajectory=self.trajectory_evaluation(state,goal,obstacle)returncontrol,trajectorydefcal_dynamic_window_vel(self,v,w,state,obstacle):"""速度采样,得到速度空间窗口Args:v(_type_):当前时刻线速度w(_type_):当前时刻角速度state(_type_):当前机器人状态obstacle(_type_):障碍物位置Returns:[v_low,v_high,w_low,w_high]:最终采样后的速度空间"""Vm=self.__cal_vel_limit()Vd=self.__cal_accel_limit(v,w)Va=self.__cal_obstacle_limit(state,obstacle)a=max([Vm[0],Vd[0],Va[0]])b=min([Vm[1],Vd[1],Va[1]])c=max([Vm[2],Vd[2],Va[2]])d=min([Vm[3],Vd[3],Va[3]])return[a,b,c,d]def__cal_vel_limit(self):"""计算速度边界限制VmReturns:_type_:速度边界限制后的速度空间Vm"""return[self.v_min,self.v_max,self.w_min,self.w_max]def__cal_accel_limit(self,v,w):"""计算加速度限制VdArgs:v(_type_):当前时刻线速度w(_type_):当前时刻角速度Returns:_type_:考虑加速度时的速度空间Vd"""v_low=v-self.a_vmax*self.dtv_high=v+self.a_vmax*self.dtw_low=w-self.a_wmax*self.dtw_high=w+self.a_wmax*self.dtreturn[v_low,v_high,w_low,w_high]def__cal_obstacle_limit(self,state,obstacle):"""环境障碍物限制VaArgs:state(_type_):当前机器人状态obstacle(_type_):障碍物位置Returns:_type_:某一时刻移动机器人不与周围障碍物发生碰撞的速度空间Va"""v_low=self.v_minv_high=np.sqrt(2*self._dist(state,obstacle)*self.a_vmax)w_low=self.w_minw_high=np.sqrt(2*self._dist(state,obstacle)*self.a_wmax)return[v_low,v_high,w_low,w_high]deftrajectory_predict(self,state_init,v,w):"""轨迹推算Args:state_init(_type_):当前状态---x,y,yaw,v,wv(_type_):当前时刻线速度w(_type_):当前时刻线速度Returns:_type_:_description_"""state=np.array(state_init)trajectory=statetime=0#在预测时间段内whiletime