移动机器人平滑JPS路径规划与轨迹优化方法

黄健萌 吴宇雄 林谢昭

(福州大学机械工程及自动化学院， 福州 350116)

0 引言

随着移动机器人被广泛应用于农业等诸多领域，移动机器人导航技术逐渐成为研究热点。路径规划是移动机器人完成导航任务的核心技术之一，其目的是让机器人在充满障碍物的复杂环境中找到从起点到终点的最佳无碰撞路径[1]，通常是以规划出的路径最短作为优化目标。目前常用的路径规划方法有栅格法、人工势场法、可视图法等[2]，以及蚁群算法、粒子群算法等智能算法[3-4]。栅格法简单有效，是目前使用最广泛的一种方法。

栅格法中，Dijkstra算法[5]搜索时只考虑已花费成本，虽然能保证找到最短路径，但会扩展大量无用节点。BFS算法采用启发函数快速接近目标点，不考虑已花费成本，其效率高于Dijkstra，但不能保证找到最短路径。A*算法[6]兼顾了两者的优点，在考虑已花费成本的同时采用启发式搜索，并能保证找到最短路径。但A*算法规划的路径通常是一组曲折路径，仅是离散空间上的最优解，与连续空间上实际最优解有较大差距。“视野线”法[7-9]常被用来对A*算法规划的路径进行平滑后处理，但只在后处理时使用平滑方法往往无法使路径跨过障碍物，优化效果有限，平滑后的路径有时仍与最优解存在较大差距。为此，Theta*算法[10]提出了任意角度规划路径的新思路，该方法在A*算法的基础上，采取实时收缩父节点，即边搜索边平滑的策略，与采用后处理平滑的路径规划方法相比，能更好地接近最优解。在此基础上许多学者进一步提出了Lazy-Theta*、Strict-Theta*、Batch-Theta*等Theta*类算法[11-13]，在效率和平滑处理等方面进行改进。由于Theta*类算法是在A*算法基础上的改进，因此随着栅格环境的增大，存在扩展节点急剧增加、搜索时间大幅延长的情况。文献[14-16]提出的跳点搜索(Jump point search,JPS)算法采用了完全不同的节点扩展方式，与A*算法相比，JPS算法有效减少了需要维护的节点数量，尤其是在大栅格环境中，有效提高了寻路效率，但JPS所规划路径仍然只是离散空间的最优路径。

采用栅格法规划的路径通常无法直接用于机器人运动，需进行轨迹优化处理。圆切线[8,17]方法虽然能生成可行轨迹，但并未考虑机器人运动的速度、惯性等物理参数。通过微分平坦[18]，多项式对各类机器人[19-20]在轨迹优化方面有着广泛应用，相比单段多项式，多段多项式可以更好地适应复杂环境。文献[21]利用多段高阶多项式的特性将加速度相关导数作为优化目标。文献[22-23]将多段多项式约束在安全范围内，以避免轨迹发生碰撞。目前，主要依靠迭代获取多段多项式轨迹优化的最优解。

本文在JPS算法的基础上，提出一种平滑JPS(SJPS)算法。首先针对现有平滑方法的缺点提出一种路径平滑方法，对JPS搜索规则进行修改，遍历对称路径，并利用本文平滑方法对每条所得路径进行平滑处理，再权衡长度与角度两指标，最后对所得初始路径用多段多项式进行轨迹优化，改进时间分配问题，加快迭代效率，并结合SJPS的特点对端点进行合理约束。

1 路径平滑

“视野线”方法在部分情况下存在去除必要节点，留下冗余节点的问题。如图1所示，路径上每个栅格中心点均为路径点，若按“视野线”方法平滑，则路径将无改变，因为起点与转折点连线检查会将原本必要的节点去除。为得到稳定且良好的平滑效果，本文提出以2个目标对路径点序列进行优化，即路径每个折角处均在障碍物栅格的顶点上，且与路径转角进行接触的障碍物位于转角小于180°一侧。

1.1 路径单侧有障碍物的情形

本文路径平滑遵循以下步骤：

(1)将起点作为平滑路径点的第1个点，记为qc(xc,yc)。

(2)将qc依次与后续JPS路径点连接，直到找到连线Lm经过障碍物，对应连接路径点qm(xm,ym)，并记录前一条没经过障碍物的连线Lm-1，对应路径点qm-1(xm-1,ym-1)，并将点qm-1和qm之间的路径离散成单个栅格移动的路径点。若连接到了终点也未经过障碍物，则终点为最后一个平滑路径点，结束搜索。

(3)再将点qc依次与步骤(2)中离散的路径点(包括qm)连接，直到找到一条经过障碍物的连线作为新的Lm，对应连接点作为新的qm，前一条连线记为新的Lm-1，对应路径点为新的qm-1。

(4)从Lm经过的障碍物栅格中，从可能顶点中找到一栅格顶点qj(xj,yj)，使点qc和qj的连线与Lm的夹角达到最大，则qj为新的平滑路径点。将qj作为新的起始点qc从步骤(2)开始新一轮搜索。

以图2的情况为例，首先将起点作为第1个平滑路径点，并作为搜索起点qc，从第2个点(4,5)开始，将后续路径点依次与qc连接，每次与qc连接的点记为qm。直到连接qc(2,3)和点(8,11)时，其连线经过障碍物，则将两点间的路径离散成单个栅格移动的路径点，再将点qc依次与离散的路径点连接。连接到点(5,8)时，两点连线经过障碍物栅格(5，7)，记为Lm。然后选取障碍物栅格左上角顶点(4.5,7.5)作为下一个平滑路径点，并以其为新的起始点继续搜索。点(4.5,7.5)作为起始点搜索时，连接到终点也未经过障碍物，因此将终点作为最后一个平滑路径点。最终产生的平滑路径即图2中的虚线路径。

1.2 平滑路径点选取

对于步骤(4)中障碍物栅格顶点的选取，使用一定的规则。如图3所示，当经过障碍物的连线Lm非水平或垂直时，挑选连线上经过的障碍物，连线水平或垂直时，则只挑选第1个遇到的障碍物栅格。对于挑选的栅格(xobs,yobs)，每个栅格有4个顶点，但对于每次平滑路径点搜索只有一个方向的顶点有可能被选取，延用步骤(2)中的qm-1、qc和步骤(3)中的qm标记，可能被选取的顶点应落在由qm、qc和qm-13点形成的三角形区域内。若步骤(2)中没有按要求离散，则三角形区域将存在被遗漏的障碍物栅格。如图4所示，顶点的选取有16种可能情况。先将点qm到点qc和qm-1方向的两向量Vm,c和Vm,m-1叉乘判断两向量的顺逆时针关系，再根据qm和qc连线斜率和Vm,c的朝向选择顶点(xj,yj)

(1)

(2)

(3)

(4)

(5)

式(1)分别对应图4中的16种情况，对应情形的向量位于哪一侧则选取该侧顶点。

选出各障碍物栅格顶点(xj,yj)后，将其与qc连线，并求其连线与Lm的夹角θ为

(6)

其中

V1=[xm-xc,ym-yc]

(7)

V2=[xj-xc,yj-yc]

(8)

从所有夹角中选出最大夹角所对应顶点qj作为下一个平滑路径点和新的搜索起点qc。

1.3 路径双侧均有障碍物的情形

当路径双侧均有障碍物时，可能出现如图5a所示情况。图5a中虚线路径为按上述规则优化后的平滑路径，图5b中虚线为理想平滑路径。可知最后一个转折处，转折角小于180°处所在区域并无障碍物，不满足第2个目标。这是因为寻找第3个新路径点时，在从终点开始逐个连线进行搜索的过程中，受到了右侧障碍物的干扰。

为此，在上述平滑路径搜索步骤中增加1个局部平滑步骤。每3个连续路径点产生一折角，从第3个新路径点开始，每产生一个路径点O3(x3,y3)，则设其前2个点依次为O2(x2,y2)、O1(x1,y1)，W1(W2)为O2指向O1(O3)的单位向量。如图6所示，3点连线在障碍物一侧的夹角为

θ′=α+β+γ=90°+β+γ

(9)

若该转角θ′<180°，则有

β+γ<90°⟹β<90°且γ<90°

(10)

因此在图6所示的局部坐标系下，W1(W2)总指向第三(一)象限，则2个单位向量的和向量W12(x′12,y′12)满足

(11)

由式(11)可知，W12总指向局部坐标系第四象限，即障碍物应在象限，于是可得符合要求的转角所对应的障碍物坐标(xp,yp)为

(xp,yp)=round((x2,y2)+W′12)

(12)

式中W′12——W12的单位向量

round()——四舍五入函数

通过上述方式可判断O2处所在转角小于180°一侧对应栅格是否为障碍物，其他方向判断与此同理。

(13)

式中 ceil()——往正无穷大方向取整的函数

步长s过长将导致选取平滑路径点时遗漏障碍物。

2 平滑JPS算法

2.1 搜索规则

平滑JPS(SJPS)算法与原JPS算法相比有以下区别：

(1)终止条件。在开放列表第1次弹出终点时，SJPS不停止搜索，将第1次得到的路径(未平滑)的长度作为约束值fz=l1，继续扩展所有使启发函数满足f(n)≤fz的跳点，以遍历已搜索路径，直到不再有满足要求的跳点。图8是一个简单的例子，图8a为JPS路径。在SJPS中，终点被弹出前，只有跳点1、2、3、4和起点被弹出，终点首次被弹出后将计算当前所得路径，即图8a路径的长度，作为约束值fz，继续扩展开放列表中满足f(n)≤fz的跳点，因此跳点5将继续被扩展。

(2)更新与记录父节点方式。1个跳点可能被2个以上跳点所扩展，在JPS中，只记录使启发函数最小的跳点，旧父节点将被完全舍弃，而在SJPS中，被舍弃的父节点将被单独保存，仅在回溯路径时被使用，并且即使被扩展跳点已处于关闭列表仍然会进行更新。

(3)路径回溯。在JPS中，终点被弹出后，从终点开始循环查找父节点，直到找到起点。在SJPS中，如区别(2)所述，回溯路径时1个节点可能有多个父节点，此时将对每个父节点均进行路径回溯。如图8b，终点有2个父节点为跳点3和5，回溯路径如图8b所示两条对称路径。每条回溯的路径，均对其进行平滑处理，如图8c所示。最后从所获平滑路径中，选取最理想的路径为最终路径，如图8d所示。

2.2 路径选择

大多数路径规划方法都以长度作为指标进行寻找，但在一些情况下，更长一点的路径可能会有更少的转折和总转角。因此，设立阈值Lz，对于每一条回溯的路径，在平滑后记录其长度和总转折角。先将第1条平滑路径作为当前路径长度lnow和总转折角φnow，然后遍历其余平滑路径长度lother和总转折角φother进行对比，若满足约束条件

(14)

式中w1——角度倾向权重

w2——长度倾向权重

其中之一，则将lother和φother作为新的lnow和φnow，对应路径作为新的当前路径，并继续遍历对比。最后所得当前路径则为最终路径。

3 轨迹优化

3.1 基于多段多项式的轨迹优化

直线路径不利于机器人的运动，在SJPS规划路径基础上利用多项式进行轨迹优化处理。将多项式各阶导数作为轨迹的位置、速度(v)、加速度(a)和加加速度(j)等参数，单段多项式难以适应较为复杂的环境，因此使用多段高阶多项式可以更好地在各种场景进行轨迹优化。为了能限制加速度导数，使用五次多项式。使用多段多项式平滑路径，首先将轨迹按路径点分段，设共分成m段。每小段路径拟合为一多项式曲线，则对第n段路径有

(15)

对于整条路径，则有

(16)

式中T0、T1、…、Tm——路径端点代表的时刻

由式(16)可知，通过求导可得任意时刻t的位置(P=f(t))、速度(v=f′(t))等参数并施加约束。结合本文平滑路径点的特点，先约束多项式曲线经过起、止点，再限制其他路径点只朝向与其接触的障碍物相反的方向(如1.3节所述与W12相反的方向)

(17)

式中Pi——第i个路径点

S——约束放宽的长度

m段多项式对应m+1个路径点，约束的放宽对解的质量有重要影响。每个路径点只限制了一段多项式曲线，因此需施加连续性约束，使不同段的多项式曲线在路径点处位移、速度和加速度连续

(18)

可根据需要选取最小化目标函数的导数阶数，此处以最小化j为目标以增加运动平稳性，因此轨迹优化问题将转换为如下最优化问题

(19)

该优化问题属于QP问题。优化后的一些物理量可能超过预设值，可对优化后的时间比例乘一系数

(20)

式中M——速度等物理量预期值

2月7日，水利部与中国气象局在京签订《关于加快水利和气象发展的合作备忘录》。双方将加强合作与信息共享，推进水利跨越式发展，加快气象现代化建设，促进水利、气象事业全面协调可持续发展，共同提高国家综合防灾减灾能力和水平。水利部部长、党组书记陈雷，中国气象局局长、党组书记郑国光出席签字仪式并代表双方签订备忘录。

以此控制相关物理量的最高值，这不会影响优化的效果[24]。

优化时的轨迹存在重新碰撞障碍物的可能，可通过约束多项式端点的方式避开障碍物。如图9所示，当某段轨迹被检测到碰撞后，进行连线检查，当有连线未经障碍物时，将该连线端点，及上一经过障碍物的连线的其中一个端点，3个点作为路径点按前述方法做平滑，得到新的平滑路径点作为轨迹的约束，使轨迹经过该点。

3.2 时间分配

上述QP问题，实际上可以看作是寻找最优时间分配问题。通常先给定一个时间分配初值，再用迭代的方法寻找最优解。对于时间初值的给定，目前有采用均速运动推算时间，即按路径长度比例分配时间的方法[21]，虽然能收敛到最优解，但缺乏合理性，初值与最优解偏差较大，将增大迭代难度，增加运算时间。合理的时间初值可以大大减少迭代次数。

针对起止为静止状态的时间分配，设移动机器人从起点开始，平稳加速到最大允许速度时均速运动，接近终点时再平稳减速。若以最小化j为目标，则加速期间加速度应有一个平缓的变化率，因此借助余弦函数设加速度函数为

(21)

(22)

式中wj——期望的最大j值

vmax——最大允许速度

且从静止加速到最大允许速度的时间为2π/wv。为使加速度不大于给定值amax，令

(23)

到达最大允许速度前的位移函数为

(24)

优化后的轨迹往往长于初始轨迹，因此将初始路径乘一常系数ws，即s′i=wssi，并令sinit=spro1(2π/wv)，对于首段位移的时间分配，有

(25)

末段位移时间的计算与首段位移同理。对于首、末段之间位移s′i的时间分配则有

(26)

式中SFsum、SBsum——s′i之前、之后的位移

TFsum、TBsum——s′i之前、之后的累计时间

4 仿真与实验分析

Matlab R2019b提供了ROS与导航相关工具箱，为验证本文方法，在 I5-6300HQ型便携式计算机上使用Matlab R2019b软件与ROS平台对本文算法进行仿真与实验。

4.1 与Lazy-Theta*对比

Theta*类算法为得到更短的路径，通常将路径点设置于栅格顶点而非中心点。为方便对比，将SJPS的起始点在平滑前设置为当前栅格的栅格顶点，使其与Lazy-Theta*的起始点一致。此处SJPS在路径选择中，将Lz设为1倍栅格长度，w1、w2均设为30，启发函数采用欧几里得距离。Lazy-Theta*为八邻域搜索，启发函数同样采用欧几里得距离。在长宽为40×40场景随机分布40个边长为1.5倍栅格长度的方形障碍物，以不同精度划分为栅格环境分别进行20次实验并取平均值，结果如图10所示，图10横坐标为横/纵栅格数。

由图10a可知，与Lazy-Theta*相比，SJPS的路径长度稍短，总体较为接近。SJPS平滑性[25](总转折角)明显低于Lazy-Theta*。由图10b可知，Lazy-Theta*和SJPS在低栅格精度时，搜索效率接近，随着栅格精度提高，Lazy-Theta*的运行时间因为搜索节点大幅增加而急剧增长，而SJPS得益于跳点搜索，运行时间随精度的提高上升较为平缓。其中栅格数为400×400时，Lazy-Theta*的平均运行时间为21.897 s，SJPS的平均运行时间为0.247 s。

4.2 与JPS对比

在长宽为40×40场景随机分布160个边长为1的方形障碍物并划分为100×100栅格环境，在该环境进行的SJPS与JPS对比示例如图11所示。

图11b SJPS在路径选择中，将Lz设为1倍栅格长度，w1、w2均设为30，启发函数两者均为欧几里得距离。如图11a所示,仅在后处理对路径进行平滑，无法使路径跨越障碍物，除非将平滑处理变成另一意义上的路径规划，因而很难接近实际最优解。图11b中，SJPS通过挑选有价值的路径，以本文方法逐一进行平滑，再以一定方法进行路径选取，以此可以得到更好的路径。将障碍物数量设为160(10%障碍物)、320(20%障碍物)和480个(30%障碍物)分别进行20次实验并取平均值，其数据如表1所示。由表1可知，与平滑后处理的JPS相比，SJPS对长度和总转角有改善作用，运行时间增加7.06%～10.60%。对使用后处理的JPS，后处理的时间为0.8～1.2 ms。

表1 100×100栅格环境下比较结果Tab.1 Results comparison in 100×100 grid environment

4.3 轨迹优化

对图11b所得SJPS路径进行轨迹优化，设起点和终点为静止状态，以栅格长度为单位，允许速度、加速度最大值分别为vmax=0.25 m/s、amax=0.25 m/s2，wj为1，ws为1.1，结果如图11c所示。其中实线为最终优化结果，虚线为本文方法初始解(未考虑碰撞)，与最终解贴合程度较高，星号线为相同总时间下按路径比分配时间的初始解(未考虑碰撞)，与最终解偏差较大。两种初始解均能收敛于同一最优解。

图11c的3种解对应规划轨迹的速度、加速度、加加速度如图12所示。本文方法初始解与最终解的各项参数吻合程度较好，采用路径比分配时间初始解的方法，在首、末段轨迹带来较大冲击，从而影响了其他轨迹。但通过迭代，两种初始解均获得了同一最优时间分配，最终结果符合最小化加加速度的目标。本文所得初始解在第7次迭代接近最终解，耗时约1.2 s。按路径比方法所得初始解，在第37次迭代收敛于同一最终解，耗时约6.5 s。

4.4 实验验证

使用两轮差速移动机器人，利用Matlab与ROS对移动机器人进行联合控制并将相关数据发送到Matlab，在如图13a所示的环境进行实验，基于Gmapping方法建立如图13b所示栅格地图，设膨化安全距离0.12 m，vmax=0.15 m/s、amax=0.15 m/s2，wj为0.5，ws为1.05，起点为(0,-0.98 m)，终点为(3.04 m,-1.54 m)。实际跟随误差约为0.015 m,规划最高车速0.150 m/s,最大车速跟随误差约0.015 m/s。

5 结论

(1)提出一种平滑JPS路径规划方法，将改进后的JPS算法与本文的平滑方法相结合，对路径增设了角度指标，以较小的长度牺牲,换取平滑性更为良好的路径。结果表明，在不同障碍物密度环境下SJPS相对平滑后处理的JPS长度减少了0.48%～1.80%，总转折角减少了16.93%～52.75%，搜索时间增加7.06%～10.60%。与Lazy-Theta*的对比结果表明，随着栅格环境精度的提高，SJPS的搜索时间增加较为平缓，不会出现搜索时间急剧增加的情况，且SJPS路径亦具有良好的平滑性。

(2)使用多段高阶多项式对初始路径进行优化，结合余弦函数提出了一种时间分配方法，可以更好地加快迭代速度，并结合SJPS的特点对多项式端点进行约束。通过仿真及实验验证了本文方法的可行性。