改进JPS的无人机路径规划研究

唐嘉宁,闫搏远,陈云浩,颜 衡,程俊涛

(1.云南民族大学 电气信息工程学院, 昆明 650000;2.云南民族大学 无人自主系统研究院, 昆明 650000)

0 引言

路径规划是无人机在执行飞行任务时的关键问题。随着无人机领域的发展,在灾后探测、环境检测、电力巡检等复杂的环境飞行时,快速准确地规划出一条飞行轨迹就尤为重要。全局规划是无人机完成导航等任务的前提。全局规划是指在设定好起点和终点后,根据规划算法规划出一条从起点到终点的与障碍物碰撞的最佳路径[1]。常用的全局规划算法有Dijkstra算法[2]、A*算法[3]、概率路线图PRM方法、快速扩展随机树RRT算法[4]。

由于A*算法代价函数的优越性和对搜索路径的直接性,因此在全局规划中得到广泛应用。但使用A*算法进行全局路径规划时,实时性较差,节点的计算量大,内存占用大,计算时间长。随着节点数的增加,A*算法的规划时间也相应的变长。因此,对于A*算法的这些问题,Harabor等[5]提出了跳点搜索算法(jump point search,JPS)。在路径规划时,JPS算法不仅删除大量的对称性中间节点和无用节点,还保留了关键的跳点,这样就可以减少搜索路径,有效提高了路径规划效率[6]。宋晓茹等[7]为了提高移动机器人在复杂环境下的可靠性和智能性,用一种“块”操作方法加快搜索跳点的速度,同时删除了代表路径方向发生改变的中间转折点。JPS算法定义了“强制邻居”和“跳跃点”,去除规划路径时的冗余点,保留下关键点(即跳跃点)。将这些跳跃点连接起来,就可以形成一个完整的全局路径。Zhao等[8]通过将JPS算法应用到移动机器人的路径规划上,证明了JPS算法要优于A*算法。对于全局规划路径的速度问题,Iakushkin等[9]提出了一种双向JPS算法,这种方法从起点和终点同时搜索路径,提高了路径的查找速度,减少了规划时间,提高了全局规划的效率。Traish等[10]为了获得更快的路径规划,提出通过记录障碍物边界来减少搜索过程中的迭代计算。黄智榜等[11]为了解决规划出的路径可能存在的碰撞问题,将跳点进行了一种危险化评估,改进了跳点的筛选方式,并同时对障碍物进行膨胀化处理,从而降低了危险碰撞度的平均值。黄健萌等[12]为了得到更多有价值的路径,使用多段高阶多项式对所得路径进行轨迹优化,对JPS的搜索规则做了改进。邱磊[13]提出了一种双向JPS算法,从正、反2个方向采用改进的JPS算法交替进行路径搜索。蔡佳成等[14]为了解决路径规划时在大尺度复杂的场景下存在内存资源消耗过大、规划出的路径不够平滑的问题,提出了一种通过融合安全势场等级函数和优化的Floyd算法的改进JPS算法。黄智榜等[15]为了解决JPS算法规划出的路径距离障碍物过近的问题,提出了一种基于规划路径碰撞危险度的改进JPS算法,该算法通过对传统跳点规则进行危险评估来使跳点与障碍物保持距离。周熙栋等[16]通过提出分层栅格地图路径规划算法,来解决移动机器人在大范围非结构化场景下的路径规划问题。张庆等[17]通过利用切比雪夫距离代替欧式距离来改进启发函数,用来解决在场景较大的栅格地图中冗余节点过多的情况。黄健萌等[18]为了解决在路径规划时存在的轨迹不够平滑和规划效率不高等问题,提出了一种对目标路径序列进行优化处理并且同时对JPS搜索规则进行改进的方法。

现在也有很多无人机路径规划算法,比如RRT算法、DWA算法、PRM算法等。但这些算法由于自身的特性都有一些不足之处。RRT算法是一种是树型算法,由根节点作为起点进行随机生长,它的优点是可以快速高效地规划出一条路径,但由于其随机性采样的原因,生成的路径并不一定是最优路径。由于JPS算法是通过起点与目标点之间的方向进行跳点搜索,不存在采样的随机性,故JPS算法可以很好地弥补RRT算法的不足。DWA算法主要通过对速度空间中的速度进行多组采样,并通过这些采集来的速度对接下来一段时间内的轨迹进行预测,通过评价函数对得到的多组预测轨迹进行评价来选择一条最优轨迹。DWA算法由于只考虑速度和加速度的限制,所以它的计算复杂度低。但由于DWA算法的前瞻性不足的原因,在无人机遇到“C”字形状的障碍物时,不能很好地避障,而JPS算法在这种情况时,可以根据跳点搜索规则下的强制邻居搜索规则来解决这一情况。PRM算法也是通过采样进行路径规划的算法,与RRT算法不同的是,PRM算法在进行路径规划时,首先要将空间中连续的点转换为离散的点,然后在采样区域随机撒点,将落在障碍物上的点剔除出去,将保留的点作为中心,在中心的一定范围内进行邻域搜索,再将它们连接成无数条路径,对连接的路径进行碰撞检测,如果没有碰撞那么就将该条路径保存。但在遇到狭窄通道时,无障碍物的区域采样点过于密集,障碍物多的地方采样点又过少,可能会存在无法找到最优路径的情况。而JPS算法由于自身的跳点搜索规则,在无障碍物时可以直接跳过中间的搜索,在障碍物密集区域根据自然邻居搜索和强制邻居搜索规则可以有效地解决PRM算法遇到的问题。由于相对于其他路径规划算法,JPS算法具有以上优势,故选择改进JPS算法作为无人机的路径规划算法。

在上述分析和讨论的基础上,提出了一种在三维空间进行路径的S-JPS算法,并将此算法在三维空间下与JPS算法进行对比实验。通过比较,证明了S-JPS算法具有较高的综合性能。仿真实验验证了S-JPS具有JPS算法的快速规划性能和域扩展性能,在特殊地图上仍然具有灵活性。

1 JPS算法

对于A*算法运行过程中openlist列表的计算量大、运行时间长等问题,提出了JPS算法。从图1可以看出,为了使节点的展开方向保持不变,就需要在水平方向上连续移动,但A*算法仍需要计算水平方向上的每个节点的代价值,因此JPS算法基于这种情况,省略了中间的迭代计算过程,提出了跳跃搜索方法。在JPS算法中,起始的节点考虑了各个方向上的邻居节点。当由目标函数确定了路径规划的方向时,JPS算法就会继续向这个方向扩展,但是在遇到障碍物或者由算法定义的跳跃点之前,不需要计算这个方向节点的代价值。

图1 A*算法路径规划代价计算过程示意图

1.1 跳点搜索规则

JPS算法的跳点搜索规则分为没有障碍物时和有障碍物时2种情况。当没有障碍物时,使用自然邻居搜索规则,当存在障碍物时,使用强制邻居搜索规则。

1.1.1 自然邻居搜索规则

当没有障碍物时,使用自然邻居搜索规则。在图2所示的栅格地图中,灰色的区域表示可删除的冗余点。当前节点a的父节点为p(a),其中扩展节点的当前方向为p(a)到a的方向。自然邻居的搜索规则如图2所示,主要有2种情况:直线方向和对角线方向的搜索规则。

图2 自然邻居搜索规则示意图

在图2(a)中,父节点到当前节点的方向为水平方向,所以此时当前节点a的扩展方向为直线方向。如果父节点p(a)要到达节点2,那么在直线方向搜索时,虚线和直线的路径代价相同,也就是说父节点p(a)如果要到达节点2可以不用经过节点a并且路径的代价不会增加。那么就称节点2为可删除的冗余节点。根据上面提到的规则,除了节点1以外的节点都是可以被去除的冗余点,那么就可以将白色节点1看作是节点a的自然邻居。

在图2(b)中,节点a为对角线方向搜索,父节点p(a)不经过节点a到达节点2路径最短,在图中虚线所表示的路径为最短路径。和直线方向搜索的情况一样,白色的节点看作是自然邻居,其他节点作为冗余节点删除。

1.1.2 强制邻居搜索规则

当有障碍物时,使用强制邻居搜索规则。在图3所示的栅格地图中,障碍物用黑色栅格表示,其他区域为可通过的路径。当前节点a的父节点为p(a),当有障碍物存在时,无论是按照直线方向搜索还是按照对角线方向搜索,都无法避免节点a直接到达节点1。强制邻居的搜索规则如图3所示,主要有2种情况:直线方向和对角线方向的搜索规则。

图3 强制邻居搜索规则示意图

1.2 代价函数

假设集合A表示展开点的集合,集合A={a1,…,ai,…,an},其中ai为展开的第i个节点,n是展开的节点数,astart是起点,agoal是终点。D表示路径的距离,d(astart,a1)表示从起点到第一个节点的距离,d(ai,ai+1)表示在寻路过程扩展的任意2个节点之间的距离,d(an,agoal)表示从最后一个扩展点到目标点的距离。路径长度的目标函数为:

(1)

代价函数为:

f(n)=g(n)+h(n)

(2)

在式(2)中,f(n)表示起点到目标点的代价函数,g(n)表示当前节点到起点的代价函数,h(n)表示当前节点到目标点的预计代价函数,其中:

(3)

常用的预计代价函数公式有:曼哈顿距离(式(4))、欧几里得距离(式(5))、切比雪夫距离(式(6))、八位数距离(式(7))。

h(n)=|x2-x1|+|y2-y1|

(4)

(5)

h(n)=max(|x2-x1|,|y2-y1|)

(6)

(7)

式中:(x1,y1)为当前节点的坐标,(x2,y2)为目标点的坐标。

采用欧式距离作为预计代价函数。

JPS算法寻路过程如图4所示。

图4 JPS算法寻路过程示意图

在图4中,绿色栅格是起点,红色栅格是终点,黄色栅格是搜索的跳跃点。从图4可知,JPS算法丢弃了大量不需要扩展的节点,相对于A*算法,大大提高了搜索效率。

1.3 算法流程

JPS算法流程如图5所示。JPS算法的流程与传统A*算法的流程相似。都需要对openlist和closelist进行操作,当前节点的下一节点由代价函数f=g+h更新。JPS算法在节点运动的方向为:直线和对角线之间寻找跳点。

图5 JPS算法流程

2 改进JPS算法

在传统的JPS算法里,前端的路径规划里规划出的节点较多,效率低。在后端的轨迹优化里只考虑了距离信息,没有考虑方向信息,轨迹优化比较困难。针对这2个问题,提出基于JPS算法改进的S-JPS算法。

2.1 引入新的启发函数

在S-JPS算法中,提出一个新的启发函数f(n)来解决前面提到的2个问题,如下:

f(n)=aL-bcosα

(8)

式中:a、b表示距离和方向的权重,L表示距离信息,如式(9)所示;cosα是方向信息,表示父节点和扩展节点之间的角度以及扩展节点和目标点之间的角度,它表示根据这个节点扩展所需要的方向成本。

当2个方向相同时,方向代价最小,cosα的值为+1;当2个方向相反时,方向代价最大,cosα的值为-1。

(9)

式中:dx1和dx2分别表示当前节点S和目标节点G的横坐标;dy1和dy2分别表示当前节点S和目标节点G的纵坐标。

2.2 节点更新规则

S-JSP算法可以更高效、准确地规划出路径。虽然JPS算法可以显著地减少扩展节点的数量,但它会使障碍物区域的扩展节点数目增加,这样将会增加后端轨迹优化的难度。在这个基础上,对拐点进行了修整,使路径的平滑度得到了进一步提高。对于所有的扩展节点(a0,a1,…,aN),如果相邻节点形成的直线an-1an和anan+1的斜率不同,则连接节点an-1和an+1。如果直线an-1an+1没有穿过障碍物,则舍弃原来的an-1an和anan+1,保留线段an-1an+1。然后比较an-1an+1和an+1an+2,以此类推。

如图6所示,蓝色线段为JPS算法规划出的路径,一共得到5个规划点。在JPS算法的基础上改进得到S-JPS算法,使用S-JPS算法进行规划,得到的新路径,用红色线段表示。S-JPS算法将点a0和a2连接起来,因为路径a0a2没有通过障碍物,所以舍弃了原来的路径a0a1和a1a2,保留了路径a0a2。继续连接点a0和a3,由于路径a0a3穿越障碍物,所以舍弃a0a3,保留路径a0a2。再次从点a2开始,连接a2a3和a3a4,依次类推,最后将路径a2a3、a3a4、a4a5、a5aN舍弃,保留路径a2aN。从图6中可以看出,扩容点a1、a3、a4、a5被舍弃,只保留节点a2。虽然算法的扩展节点保持不变,但改进后的规划路径更高效。

图6 基于JPS改进算法示意图

S-JPS算法的伪代码如下:

Algorithm1S-JPS

Input:initialastart,targetagoal,priority queue

Output:optimal path

1g(astral)=0,g(n)=infinite

2 while

3 if the queue is empty,return FALSE break

4 calculate theh(n)=aL-bcosα

5 remove the nodenwith the lowestf(n)=g(n)+h(n) from the priority queue

6 mark nodenas expanded

7 if the nodenis the goal stateagoal,return TRUE; break

8 for all unexpanded jumping neighborsmof the noden

9 ifg(m)=infinite

10g(m)=g(n)+Cnm

11 push nodeminto the queue

12 ifg(m)>g(n)+Cnm

13g(m)=g(n)+Cnm

14 end

15 fori=1 ton-2(number of points,up date every cycle)

16 whilen-i>0

17 ifanan+2does not pass through the obstacles

18 discardan+1

19 else

20 break

21 end

22 end

3 基于Bezier曲线和直线混合的轨迹优化

基于JPS算法提出的S-JPS算法虽然能够生成最优路径,但是没有考虑无人机的运动学模型,得到的路径不是光滑的,路径的曲率也不是连续的。针对这个问题,提出一种基于Bezier曲线和直线混合的轨迹优化算法对轨迹进行平滑处理,提高全局路径的平滑度。其中Bezier曲线的一般形式如下:

(10)

式中

(11)

B5(t)=(1-t)5a0+5(1-t)4ta1+
10(1-t)3t2a2+10(1-t)2t3a3+
5(1-t)t4a4+t5a5

(12)

利用5次Bezier将规划出的路径优化成平滑、曲率连续的路径,实现无人机导航的轨迹生成。5次Bezier曲线的表达式如式(12)所示,曲线本身需要6个控制点,除起点和终点以外还有4个中间控制点。对于本文算法,如果路径有N个拐点,则整个路径有N+2个路径点:a0,a1,a2,…,aN,aN+1,N+1条折线。因此,轨迹优化后的N条Bezier曲线和N+1条直线段采用5次Bezier曲线直线段,它们的路径和曲率都是连续的。

由于该算法需要Bezier曲线的曲率与直线段连续,因此Bezier曲线与直线段连接处的曲率为0。如图7所示,由于Bezier曲线的凹凸性,优化出的轨迹在线段内[19-20]。其中红色轨迹为Bezier曲线处理后的轨迹。用Bezier曲线代替原直线段的最重要的部分为控制点的选取。

图7 Bezier曲线的凹凸性

如图8所示,图中ai-1、ai、ai+1为相邻的3个关键拐点,这3个点的坐标分别为(xai-1,yai-1)、(xai,yai)、(xai+1,yai+1),其中拐点的平滑突变区又有4个关键控制点Pi、Pi+1、Pi+2、Pi+3。式(13)为控制点Pi的坐标:

图8 Bezier曲线平滑策略

将上述求解过程中控制点Pi的xai-1和yai-1用xai+1和yai+1替换,就是控制点Pi+3的坐标。Pi+1和Pi+2为中间的2个控制点,坐标为:

(15)

4 仿真分析与实验

4.1 仿真分析

为了对改进的算法进行验证,对传统JPS算法和S-JPS算法进行仿真分析。在同一个地图环境下,起点相同,分别选取6个不同的目标点,进行三维路径规划。实验所用的硬件平台为Intel(R) Core(TM) i7-8750H CPU@ 2.20 GHz,运行内存为16 GB,操作系统为Ubuntu18.04。在ROS中随机生成一个三维地图,三维地图的3个不同视角的视图如图9所示。

图9 随机生成三维环境示意图

将本文算法与JPS算法同时进行仿真测试。白色栅格为JPS算法规划出的节点,黑色栅格本文算法规划出的节点。在生成的三维环境中依次进行选取目标点进行规划,仿真结果如图10所示。

图10 JPS算法与S-JPS算法仿真结果

为便于观察,在图10中规划出节点的地方已用黑色圆圈圈出,并用红色箭头指出。通过仿真结果可以看出,在目标点和起点相同时,本文算法规划出的节点要比JPS算法规划出的节点少。表1为JPS算法与S-JPS算法在同一地图中的实验数据对比情况。S-JPS算法相比于JPS算法,时间代价减少98.6%,路径代价减少81.1%,拜访节点个数减少99.7%。

表1 JPS算法与S-JPS算法实验数据对比

4.2 实验

为了验证S-JPS算法在无人机中实际运行的有效性,将JPS算法与S-JPS算法同时在同一地图进行规划,将算法移植到自主搭建的无人机上,如图11(a)所示。无人机采用D435I双目相机获取二维点云数据,结合陀螺仪和里程计数据进行三维地图的构建,最后通过S-JPS算法对无人机进行路径规划。

图11 自主搭建的无人机与实验环境

如图12所示,在无人机实际起飞后,可以在Rviz工具中显示出越过障碍物时经过Bezier曲线和直线混合的优化轨迹。其中蓝色轨迹为无Bezier曲线和直线混合的轨迹优化,红色轨迹为基于Bezier曲线和直线混合的轨迹优化。从实验结果可以看出,蓝色轨迹会出现穿越障碍物或者距离障碍较近的问题,而红色轨迹则不会穿越障碍物并且距离障碍物会保持一定的安全距离。

图12 无人机起飞过程中的Rivz界面

5 结论

路径规划作为无人机完成给定任务的最重要的环节,在路径规划当中引入了改进的JPS和Bezier曲线算法。首先,利用基于距离和方向的启发式函数,更准确地描述了从当前点到目标点的估计代价,以降低时间代价、路径代价和拜访的节点数。最后通过Bezier曲线和直线混合的轨迹优化算法将规划出的路径进行平滑处理,使它更满足无人机的动力学约束。实验结果表明,相比于传统JPS算法,S-JPS算法在时间代价、路径代价和拜访节点数目上都远小于JPS算法。

对于提出的算法,还存在以下改进空间:由于JPS是在A*算法的基础上提出的,在较为空旷的区域时A*算法的效率要高于JPS,在障碍物密集区域时JPS算法效率又远高于A*算法。所以在S-JPS算法中,在启发函数阶段设置一个阈值k,当障碍物密度小于阈值k时使用A*算法的启发函数;当障碍物密度大于阈值k时,使用S-JPS算法的启发函数,动态启发函数公式如式(16)所示。通过定性的分析可以得出改进后算法的规划时间要小于S-JPS算法,规划效率要高于S-JPS算法。

(16)

式中:H为当前障碍物的密度,k为设置的障碍物密度阈值。