融合改进Bi-RRT和DWA算法的无人机动态路径规划
2024-05-09陈新洲
罗 毅, 陈新洲
(华北电力大学控制与计算机工程学院,北京 100000)
0 引言
随着人工智能的发展,无人机被广泛应用于搜索救援、智慧农业、数据采集等多个领域,高效的避障功能是无人机在复杂环境中成功执行任务的关键。避障依赖于智能的路径规划,即规划一条由起点通往目标点的满足一定约束的无碰撞路径[1],其核心是路径规划算法;根据不同的用途可分为全局路径规划算法和局部路径规划算法[2]。
全局路径规划要预知环境信息,如A*算法[3]、Dijkstra算法[4]、遗传算法[5]等,但上述算法难以应用于高维复杂环境[6]。快速扩展随机树(Rapidly-exploring Random Trees,RRT)算法[7]是一种基于随机采样机制规划全局路径的算法,其空间搜索能力强大,常被应用于具有高维复杂约束的路径规划,但存在采样过于随机、收敛速度慢、路径安全性低、路径不平滑等缺陷[8],许多学者基于RRT算法提出了多种改进算法。Bi-RRT算法[9]通过构建2棵树进行双向搜索提升了收敛速度,但还存在原算法的其他缺陷;RRT*算法[10]通过加入邻域搜索和重布线过程可使路径趋于最优,但迭代次数较多;为进一步提升RRT算法的性能,文献[11]提出了一种改进APF-RRT算法,引入目标偏置和人工势场减少了冗余采样点,加快了收敛速度;文献[12]利用三次贝塞尔曲线使规划的路径更加平滑;文献[13]提出了一种动态步长RRT算法,动态调整步长提升了搜索树的扩展速度和环境适应能力。为了规避未知障碍物需要使用局部路径规划算法,在未知环境中根据探测到的障碍物信息实时动态调整行进路线,如人工势场法[14]、动态窗口法(Dynamic Window Approach,DWA)[15]等。人工势场法实时性高且计算简单,但容易产生局部极小值和震荡现象;动态窗口法能实时输出机器人的速度指令,规划的轨迹更平滑且更便于控制机器人移动,但容易陷入局部最优。文献[16]通过修正速度窗口和优化评价函数改进DWA,提升了动态路径规划能力。
在复杂环境中,为使无人机避障成功,需要将全局和局部路径规划相融合。文献[17]融合IRRT*和DWA能实时更新路线,但规划的路线较长;文献[18]融合A*和DWA能同时规避动态和静态障碍物,但全局路径跟踪能力较弱;文献[19]融合改进Informed-RRT*和人工势场法能跳出局部规划极小值并实现动态避障,但渡越时间较长。
综上所述,本文通过启发式搜索、动态调整步长提升Bi-RRT算法的收敛速度,在路径与障碍物间设置安全距离,并对初始路径进行修剪、插值和平滑处理,降低路径成本,提升路径质量。通过修正和新增子评价函数优化DWA,提升轨迹评价结果的准确性和可靠性。最后融合改进Bi-RRT和DWA,提取全局路径的关键节点作为改进DWA的局部目标点,从而为无人机在复杂环境中快速规划出一条最优的动态避障路径。
1 改进Bi-RRT算法
Bi-RRT算法比RRT算法的收敛速度更快。该算法构建了2棵搜索树T1和T2,它们分别以起点和目标点为根节点同时进行随机采样和扩展,一次迭代可生成2个新节点,当T1和T2能在一定阈值内无碰撞连接时,将它们连接为1棵树T,回溯T的所有节点即可获得全局路径。然而,Bi-RRT算法还存在采样过于随机、路径安全性低和路径不平滑的缺陷,针对这些缺陷采用以下方法进行改进。
1.1 设置启发式函数
基于固定概率p对目标点采样的策略虽能有效减少非必要采样点,但当环境中的障碍物分布密集时,p较大会导致搜索树采样时过度偏向目标点而陷入局部陷阱;当环境较为空旷时,p较小会导致收敛速度慢。固定采样概率p无法使算法同时具备目标偏置性和环境适应性。于是,以T1为例,设置启发式函数F(x)替代p,即
F(x)=1-φ/eμx
(1)
式中:φ∈(0,0.5],为概率初始值;μ∈(0,1),为导数变化率;x∈[0,50],为自变量;在此范围内F(x)单调递增。设x的变化规律为
(2)
式中:Δx>0,为自变量增量;Col(n)为碰撞检测函数,Col(n)=0,表示路径可通行,Col(n)=1,表示路径发生碰撞。由启发式函数可得到新的目标点采样策略,即
(3)
式中:Qrand和Qgoal分别为采样点和目标点;r1,r2∈[0,1],都是按均匀概率分布取的随机值;L、W、H分别是工作空间的长、宽、高。
由式(3)可知,F(x)的输出值即是T1对目标点采样的概率。当T1对目标点采样时:若Col(n)=0,则x=x+Δx,F(x)增大,T1朝目标点扩展的速度加快;若Col(n)=1,则x=x-Δx,F(x)减小,T1朝其他方向扩展的概率增大,可避免T1过度偏向目标点而陷入局部陷阱。
1.2 设置动态步长
复杂环境中的障碍物分布通常是不均匀的,与固定步长相比,动态步长能充分利用已知的环境信息将当前扩展步长调整为合适大小[20]。以T1为例,设置动态步长η,即
η=max[ηmin,min(ηmax,d/n)]
(4)
式中:ηmin和ηmax分别为步长的最小、最大值;d为T1与目标点的最短距离;n为T1当前节点数量。
迭代前期,搜索空间较大,选取ηmax扩展T1;迭代中期,T1初步成型,改选d/n进行扩展;迭代后期,搜索空间较小,使用ηmin进行扩展。
1.3 设置安全距离
Bi-RRT算法会生成紧贴障碍物的路段,导致全局路径存在安全隐患。因此,需要在路径与障碍物间设置安全距离以判断路径的安全性,如图1所示。图1中,Qnew为待添加节点,Qnear为Qnew的父节点,l2为待检测路段,l1与l3同时关于l2及其所处的竖直平面对称,l4与l5在同一竖直平面上关于l2对称。将其他线段和l2之间的距离均设为W,W即为安全距离。对线段l1～l5同时进行碰撞检测,若均不发生碰撞,则l2为可通行路段,Qnew为可扩展节点;若有至少一条线段发生碰撞,则l2存在安全隐患,此时需要重新采样。
图1 安全距离Fig.1 Safe distance
1.4 路径插值与平滑
初始路径中通常包含大量冗余路段,且路径不够平滑,通过修剪路径[21]可有效消除冗余路段。而针对路径不够平滑的问题,解决方法之一是将路径关键节点作为基函数控制点构建三次B样条并替代原路径[22],三次B样条的表达式为
(5)
式中:Fj,3(t)为三次B样条的基函数;Vj为基函数控制点。
下面构建三次B样条,如图2所示。
图2 构建三次B样条Fig.2 Construction of cubic B-splines
图2中,红色虚线为修剪后的路径,其节点序列为
2 改进DWA
DWA的执行步骤为速度采样、轨迹预测和轨迹评分。首先在无人机的速度空间中预测出所有可行轨迹,再根据评价函数选取最优轨迹并输出与之对应的速度指令。在现实中需要对速度空间进行约束,即
(6)
(7)
式中:(x(t+Δt),y(t+Δt),z(t+Δt))为无人机在(t+Δt)时刻的坐标;θxy(t)和θz(t)分别为无人机在t时刻的偏航角和俯仰角。最后根据评价函数从中选取最优轨迹,传统评价函数的表达式为
G(v,w)=α×head(v,w)+β×obsdis(v,w)+
γ×vel(v,w)
(8)
式中:G(v,w)为总评价函数;head(v,w)用于衡量无人机当前航向与目标点方向之间的偏差,偏差越小,评分越高;obsdis(v,w)用于衡量当前预测轨迹与障碍物之间的最短距离,距离越大,评分越高;vel(v,w)用于调整速度大小;α,β和γ分别为各项子函数的系数。
下面对传统评价函数进行优化。
2.1 修正障碍物距离评价函数
首先将障碍物分为已知障碍物和未知障碍物,将已知障碍物的信息作为碰撞检测函数的输入,未知障碍物的信息则作为障碍物距离评价函数obsdis(v,w)的输入,同时设置无人机威胁空间对障碍物进行筛选,见图3。
图3 无人机威胁空间Fig.3 Threat space to the UAV
图3中,设点U为无人机,点X1～X3为未知障碍物,以U为中心,以r为半径的球体空间即为威胁空间,空间内的障碍物X2被判定为存在威胁的障碍物,空间外的障碍物X1和X3则直接忽略。于是得到新的障碍物距离评价函数,即
(9)
2.2 引入目标点距离评价函数
传统评价函数中,无人机由head(v,w)导航,通常导致无人机实际到达位置与目标点位置有偏差。goaldis(v,w)是目标点距离评价函数,可用于衡量无人机当前预测轨迹与目标点的最短距离,从而增强无人机的目标跟踪能力,即
goaldis(v,w)=[min(disg(v,w)-L),0]2
(10)
式中:disg(v,w)为无人机在速度(v,w)下的预测轨迹与目标点的最短距离;L为距离阈值。
当disg(v,w)>L时,goaldis(v,w)=0,不参与导航,无人机仍由head(v,w)导航;当disg(v,w)≤L时,无人机到达目标点附近,此时goaldis(v,w)>0,并且距离目标点越近goaldis(v,w)的值越大,在导航中发挥的作用也越大,目标导向性则越强,从而能引导无人机准确到达目标点位置。
由式(9)和式(10)得到新的评价函数,即
G(v,w)=α×head(v,w)+β×obsdis(v,w)+
γ×vel(v,w)+ε×goaldis(v,w)
(11)
式中,ε为goaldis(v,w)的系数。
3 融合算法
融合改进Bi-RRT和DWA,提出一种无人机动态路径规划算法,具体流程如图4所示。
图4 融合算法流程图Fig.4 Flow chart of the fusion algorithm
4 仿真验证
下面设计2组仿真实验,分别验证所提融合算法在全局路径规划和局部路径规划上相较于传统算法的优越性和高效性。
4.1 全局路径规划
建立仿真实验所需的工作空间,大小为800×800×100,并设置起点(20,20,20)和目标点(760,760,20),单位均为m。首先将改进Bi-RRT算法和普通Bi-RRT算法、Bi-RRT*算法生成路径的质量进行对比,并根据环境信息将改进Bi-RRT算法的安全距离设为2 m,如图5所示,图中的灰色区域均为静态障碍物,高度均为100 m。
图5 3种全局路径规划算法生成的路径Fig.5 Paths generated by three global path planning algorithms
由图5可知:普通Bi-RRT算法会生成大量冗余路段,且路径曲折;Bi-RRT*算法虽然通过重布线过程使得冗余路段显著减少,但路径仍不够平滑;改进Bi-RRT算法生成的路径既无冗余路段,也能始终保持平滑,满足无人机的动态特性要求。
3种全局路径规划算法的实验数据见表1。
表1 3种全局路径规划算法的性能
由表1可知,与普通Bi-RRT算法相比,改进Bi-RRT算法通过启发式搜索和动态调整步长加快了收敛速度,提升了环境适应能力,将规划时间减少了87.2%,迭代次数也显著减少,并通过优化路径使路径长度缩短了24.77%。Bi-RRT*算法虽然在一定程度上提升了路径质量,且路径长度缩短了12.43%,但迭代次数和时间成本明显增加,利小于弊。3种全局路径规划算法中,仅有改进Bi-RRT算法通过设置安全距离使路径始终保持安全性。综上所述,改进Bi-RRT算法规划的全局路径最优。
4.2 局部路径规划
在4.1节工作空间中随机加入未知动、静态障碍物,设动态障碍物的半径,在25～30 m之间,静态障碍物的高度为100 m。将图5中改进Bi-RRT算法生成的关键节点作为DWA的局部目标点进行仿真实验,将改进DWA与传统DAW生成航迹的质量进行对比,结果如图6和图7所示。图中,绿色与蓝色球体分别表示各动态障碍物的移动起点与终点,红色物体表示静态障碍物。为了便于观察航迹,在侧视图和主视图中仅显示未知障碍物模型。
图6 传统DWA
图7 改进DWA
由图6可知,在规避未知障碍物时,传统DWA通常需要规划出更长的避障轨迹,整体生成的路径较为曲折,与全局最优路径的贴合度较低,航迹不具备最优性,导致无人机需要规避的动态障碍物较多。
由图7可知,改进DWA生成的航迹不仅平滑,与全局最优路径的贴合度也更高,可靠性更高,满足无人机的动态特性要求。
对传统DWA和改进DWA的性能进行分析,由实验得到的数据见表2。结合表2的数据可知:与传统DWA相比,改进DWA对未知障碍物进行了筛选,使得obsdis(v,w)的输出不受无关障碍物信息的影响,结果更准确;在引入goaldis(v,w)后,无人机航行至目标点附近时开始由head(v,w)和goaldis(v,w)共同导航,目标跟踪能力明显增强;改进DWA在优化了评价函数后,对预测轨迹的评价结果更加准确,使无人机从起点航行至终点的时间减少了25.48%,航迹长度缩短了11.90%,迭代次数减少了27.34%,动态避障效率更高,效果更佳。
表2 传统DWA和改进DWA的性能对比
综上所述,所提融合算法在全局和局部路径规划方面较传统算法都更具优势,具备控制无人机在复杂环境中高效规划动态避障路径的能力。
5 结论
本文融合改进Bi-RRT和DWA提出了一种无人机动态路径规划算法。在全局规划方面,设置启发式函数和动态步长加快了Bi-RRT算法的搜索速度;设置安全距离提升了路径安全性;采取修剪、插值和平滑路径的措施获得了全局最优路径。在局部规划方面,优化评价函数提升了DWA预测轨迹评价结果的准确性,增强了动态避障能力。所提融合算法为无人机在复杂环境中的高效导航创造了有利条件。
