一种基于边界查找的双向跳点搜索无人车路径规划方法与流程

1.本发明涉及人工智能与无人系统领域，具体涉及自动驾驶路径规划技术领域，尤其涉及一种基于边界查找的双向跳点搜索无人车路径规划方法。


背景技术：

2.在无人系统领域无人车的自动驾驶中，路径规划是其关键技术之一，针对这一技术目前已经开展了大量的研究工作。无人车路径规划可以分为静态路径规划和动态路径规划。其中a*算法、dijkstra算法等目前主要应用于已知的静态场景中，而人工势场法、d*算法等则大多应用于环境未知动态场景中。其中跳点搜索(jump point search,jps)是对a*搜索算法的优化，通过剪枝规则，打破栅格的对称性，减少无效节点的访问，保留关键“跳点”，能让搜索在栅格上直线长“跳跃”，具有较好的路径寻优性能。但是，对于大规模和复杂化的栅格地图，jps跳点搜索算法在节点搜索和跳点识别阶段会消耗大量时间，导致路径寻优速度降低，且无法保证路径最优性，所得路径不符合无人车运动学约束，需进一步优化处理。
3.针对上述问题，文献《一种面向非结构化环境的改进跳点搜索路径规划算法》提出加权jps跳点搜索算法，存在如下问题：
4.(1)加权启发式估价函数只考虑了目标节点信息，未考虑起始节点信息，虽然提高了路径的搜索速度，但是不能保证路径最优性；
5.(2)采用微分平坦法对生成的路径节点作曲线拟合，未考虑安全距离，导致优化后的路径可能碰到障碍物。
6.文献《双向跳点搜索算法的移动机器人全局路径规划研究》提出双向jps跳点搜索算法，存在如下问题：
7.(1)在跳点识别阶段，从直线和对角线方向上迭代每个相邻单元格，会消耗大量时间，导致路径寻优速度降低；
8.(2)采用jps跳点搜索基本的启发式估价函数f(n)＝g(n) h(n)，可以搜索到路径，但是计算时间长。


技术实现要素：

9.本发明提供一种基于边界查找的双向跳点搜索无人车路径规划方法，从正向和反向进行跳点交替迭代搜索，通过边界查找提高水平和垂直方向上跳点的搜索速度，减少搜索跳点所涉及的大量迭代，设计改进的启发式估价函数提高路径寻优速度，保证路径最优性，采用微分平坦方法对生成的路径节点作曲线拟合，保证路径的连续性、平滑性和安全性。
10.为达到上述目的，本发明提供一种基于边界查找的双向跳点搜索无人车路径规划方法，包括以下步骤：
11.s1、使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域；
12.s2、将正向搜索和反向搜索的起始节点分别放入openlist1和openlist2中；
13.s3、正向搜索；
14.s4、反向搜索；
15.s5、从正向和反向进行跳点交替迭代搜索；
16.s6、搜索成功并保存正反方向搜索的路径节点即跳点坐标数据；
17.s7、采用微分平坦方法对生成的路径节点作曲线拟合；
18.所述的s1中，使用膨胀法对障碍物进行处理：
19.本专利中无人车和障碍物都可视为凸多边形，故采用minkowski和的方法求解临界多边形，以无人车宽度一半为半径的圆b0，作b0与障碍物m的minkowski和，以和的边界为障碍物拓展的边界。计算公式如下：
[0020][0021]
其中：表示两个向量和的向量和。
[0022]
所述的s3中，正向搜索具体包括以下子步骤：
[0023]
s31、正向扩展子节点，判断当前节点是否为反向搜索的当前节点，若是则跳转到s6，否则继续执行以下步骤；
[0024]
s32、依据剪枝规则去掉冗余的对称节点，确定强制邻节点和跳点：
[0025]
无人车在栅格地图上扩展子节点分为直线方向和对角线方向，对于直线方向，裁剪掉所有符合以下条件的节点n：
[0026]
len(＜p(a),
…
,n＞\a)≤len(＜p(a),a,n＞)
[0027]
其中：len(＜p(a),
…
,n＞\a)表示从p(a)节点不经过a节点到达n节点的最短路径，len(＜p(a),a,n＞)表示从p(a)节点经过a节点到达n节点的最短路径，a表示a节点，n表示n节点，p(a)表示节点a的父节点；
[0028]
对于对角线方向，裁剪掉所有符合以下条件的节点n：
[0029]
len(＜p(a),
…
,n＞\a)＜len(＜p(a),a,n＞)
[0030]
对于强制邻节点，根据以下规则进行筛选：
[0031]
len(＜p(a),a,n＞)＜len(＜p(a),
…
,n＞\a)
[0032]
s33、利用边界查找优化水平和垂直方向节点搜索和跳点识别，且将跳点添加到openlist1中：
[0033]
边界查找：
[0034]
从正向搜索和反向搜索两个方面记录障碍边界的位置，障碍物由其边界的位置来定义，通过对网格进行预处理并记录这些边界的位置，边界查找不会在垂直或水平方向上迭代每个相邻单元格，相反，它会查找这些方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，并通过直接查找来减少在算法中搜索跳点所涉及的大量迭代，当在水平方向上的边界比其上方和下方所在行的重新打开值更远时，会识别左、右两边的跳点，垂直方向的工作原理相同，只是检查相邻列而不是行，东西方向使用水平边界查找表，南北方向使用垂直边界查找表；
[0035]
跳点识别规则可表示为：b＝a kd，从a节点出发，通过在d方向移动k步到达b，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
[0036]
(1)节点b为目标节点；
[0037]
(2)节点b含有至少一个强制邻节点；
[0038]
(3)若d为对角线移动，存在c＝b k
i
d
i
，其中k
i
∈n，c是b的跳点，则b也是a的跳点；
[0039]
s34、采用改进的正向启发式估价函数计算openlist1中代价最小的跳点；
[0040]
正向启发式估价函数为：
[0041]
f(n)＝g
i
(n) h
i
(n) cross
×
0.001
[0042]
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数，g
i
(n)表示起始节点到当前节点的累计实际代价，h
i
(n)表示当前节点到目标节点的估计代价，cross表示原起始节点指向原目标节点的向量与当前节点指向原目标节点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达；
[0043]
s35、将代价最小的跳点添加到closelist1中。
[0044]
所述的s4中，反向搜索具体包括以下子步骤：
[0045]
s41、反向扩展子节点，判断当前节点是否为正向搜索的当前节点，若是则跳转到s6，否则继续执行以下步骤；
[0046]
s42、依据剪枝规则去掉冗余的对称节点，规则与正向搜索相同，确定强制邻节点和跳点：
[0047]
s43、利用边界查找优化节点搜索和跳点识别，边界查找优化原理和跳点识别规则与正向搜索相同，且将跳点添加到openlist2中：
[0048]
s44、采用改进的反向启发式估价函数计算openlist2中代价最小的跳点；
[0049]
反向启发式估价函数为：
[0050]
f
′
(n)＝g
j
(n) h
j
(n) cross
×
0.001
[0051]
其中：f
′
(n)表示反向搜索从起始节点到当前节点到目标节点的代价函数，g
j
(n)表示起始节点到当前节点的累计实际代价，h
j
(n)表示当前节点到目标节点的估计代价，且采用欧式距离作距离表达；
[0052]
s45、将代价最小的跳点添加到closelist2中。
[0053]
所述的s5中，从正向和反向进行跳点交替迭代搜索的过程如下：
[0054]
b
s
表示正向搜索的起始节点，b
g
表示反向搜索的起始节点，首先进行正向搜索，以b
s
为正向搜索的起始节点，b
g
为正向搜索的目标节点进行子节点扩展搜索，直到搜索到代价最小的跳点b1；切换为反向搜索，以b
g
为反向搜索的起始节点，b1为反向搜索的目标节点进行搜索，搜索到代价最小跳点b
n
；切换为正向搜索，以b
s
为正向搜索的起始节点，b
n
为正向搜索的目标节点进行搜索，以此进行交替迭代搜索，直到正向和反向搜索的当前节点重合，则说明已经发现了一条完整路径。
[0055]
算法交替迭代方法如下所示：
[0056][0057]
[0058]
其中：b
s
(x
s
,y
s
)表示原起始节点b
s
的横纵坐标，b
g
(x
g
,y
g
)表示原目标节点b
g
的横纵坐标，b1(x1,y1)表示正向搜索代价最小的跳点b1的横纵坐标，b
n
(x
n
,y
n
)表示反向搜索代价最小的跳点b
n
的横纵坐标。
[0059]
本发明具有如下有益效果：
[0060]
1.通过边界查找水平和垂直方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，以提高水平和垂直方向上跳点的搜索速度，减少搜索跳点所涉及的大量迭代；
[0061]
2.设计双向改进的启发式估价函数，从正向和反向进行交替迭代路径搜索，使得路径搜索时间和扩展节点大大减少，提高路径寻优速度，保证路径最优性；
[0062]
3.采用微分平坦方法对生成的路径节点作曲线拟合，避免与障碍物碰撞，保证路径的连续性、平滑性和安全性。
附图说明
[0063]
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本专利的其它特征、目的和优点将会变得更明显：
[0064]
图1基于边界查找的双向跳点搜索算法的流程框图；
[0065]
图2正向和反向交替迭代搜索的示意图；
[0066]
图3基于边界查找的双向jps跳点搜索算法示意图；
[0067]
图4采用微分平坦方法对实施例中生成的路径节点作曲线拟合的示意图。
具体实施方式
[0068]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。
[0069]
如图1所示，本发明提供了一种基于边界查找的双向跳点搜索无人车路径规划方法，具体包括以下步骤：
[0070]
s1、使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域，具体包括以下子步骤；
[0071]
s11、本专利中无人车和障碍物都可视为凸多边形，故采用minkowski和的方法求解临界多边形，以无人车宽度一半为半径的圆b0，作b0与障碍物m的minkowski和，以和的边界为障碍物拓展的边界。计算公式如下：
[0072][0073]
其中：表示两个向量和的向量和。
[0074]
s12、以一定的长度c将无人车移动平面离散化，划分为若干等分的栅格，对每个栅格进行编号，并将存在障碍物的栅格标记为黑色，表示不可通行，不存在障碍物的栅格标记为白色，表示可通行。
[0075]
s2、将正向搜索和反向搜索的起始节点分别放入openlist1和openlist2中；
[0076]
s3、正向搜索，具体包括以下子步骤：
[0077]
s31、正向扩展子节点，判断当前节点是否为反向搜索的当前节点，若是则跳转到s6，否则继续执行以下步骤；
[0078]
s32、依据剪枝规则去掉冗余的对称节点，确定强制邻节点和跳点：
[0079]
无人车在栅格地图上扩展子节点分为直线方向和对角线方向，对于直线方向，裁剪掉所有符合以下条件的节点n：
[0080]
len(＜p(a),
…
,n＞\a)≤len(＜p(a),a,n＞)
[0081]
其中：len(＜p(a),
…
,n＞\a)表示从p(a)节点不经过a节点到达n节点的最短路径，len(＜p(a),a,n＞)表示从p(a)节点经过a节点到达n节点的最短路径，a表示a节点，n表示n节点，p(a)表示节点a的父节点；
[0082]
对于对角线方向，裁剪掉所有符合以下条件的节点n：
[0083]
len(＜p(a),
…
,n＞\a)＜len(＜p(a),a,n＞)
[0084]
对于强制邻节点，根据以下规则进行筛选：
[0085]
len(＜p(a),a,n＞)＜len(＜p(a),
…
,n＞\a)
[0086]
s33、利用边界查找优化水平和垂直方向节点搜索和跳点识别，且将跳点添加到openlist1中：
[0087]
边界查找：
[0088]
从正向搜索和反向搜索两个方面记录障碍边界的位置，障碍物由其边界的位置来定义，通过对网格进行预处理并记录这些边界的位置，边界查找不会在垂直或水平方向上迭代每个相邻单元格，相反，它会查找这些方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，并通过直接查找来减少在算法中搜索跳点所涉及的大量迭代，当在水平方向上的边界比其上方和下方所在行的重新打开值更远时，会识别左、右两边的跳点，垂直方向的工作原理相同，只是检查相邻列而不是行，东西方向使用水平边界查找表，南北方向使用垂直边界查找表；
[0089]
跳点识别规则可表示为：b＝a kd，从a节点出发，通过在d方向移动k步到达b节点，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
[0090]
(1)节点b为目标点；
[0091]
(2)节点b含有至少一个强制邻节点；
[0092]
(3)若d为对角线移动，存在c＝b k
i
d
i
，其中k
i
∈n，c是b的跳点，则b也是a的跳点；
[0093]
s34、采用改进的正向启发式估价函数计算openlist1中代价最小的跳点；
[0094]
正向启发式估价函数为：
[0095]
f(n)＝g
i
(n) h
i
(n) cross
×
0.001
[0096]
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数，g
i
(n)表示起始节点到当前节点的累计实际代价，h
i
(n)表示当前节点到目标节点的估计代价，cross表示原起始节点指向原目标节点的向量与当前节点指向原目标节点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达；
[0097]
s35、将代价最小的跳点添加到closelist1中。
[0098]
s4、反向搜索，具体包括以下子步骤：
[0099]
s41、反向扩展子节点，判断当前节点是否为正向搜索的当前节点，若是则跳转到s6，否则继续执行以下步骤；
[0100]
s42、依据剪枝规则去掉冗余的对称节点，规则与正向搜索相同，确定强制邻节点和跳点：
[0101]
s43、利用边界查找优化节点搜索和跳点识别，边界查找优化原理和跳点识别规则与正向搜索相同，且将跳点添加到openlist2中：
[0102]
s44、采用改进的反向启发式估价函数计算openlist2中代价最小的跳点；
[0103]
反向启发式估价函数为：
[0104]
f
′
(n)＝g
j
(n) h
j
(n) cross
×
0.001
[0105]
其中：f
′
(n)表示反向搜索从起始节点到当前节点到目标节点的代价函数，g
j
(n)表示起始节点到当前节点的累计实际代价，h
j
(n)表示当前节点到目标节点的估计代价，且采用欧式距离作距离表达。
[0106]
s45、将代价最小的跳点添加到closelist2中。
[0107]
s5、从正向和反向进行跳点交替迭代搜索，如图2所示：
[0108]
b
s
表示正向搜索的起始节点，b
g
表示反向搜索的起始节点，首先进行正向搜索，以b
s
为正向搜索的起始节点，b
g
为正向搜索的目标节点进行子节点扩展搜索，直到搜索到代价最小的跳点b1；切换为反向搜索，以b
g
为反向搜索的起始节点，b1为反向搜索的目标节点进行搜索，搜索到代价最小跳点b
n
；切换为正向搜索，以b
s
为正向搜索的起始节点，b
n
为正向搜索的目标节点进行搜索，以此进行交替迭代搜索，直到正向和反向搜索的当前节点重合，则说明已经发现了一条完整路径。
[0109]
算法交替迭代方法如下所示：
[0110][0111][0112]
其中：b
s
(x
s
,y
s
)表示原起始节点b
s
的横纵坐标，b
g
(x
g
,y
g
)表示原目标节点b
g
的横纵坐标，b1(x1,y1)表示正向搜索代价最小的跳点b1的横纵坐标，b
n
(x
n
,y
n
)表示反向搜索代价最小的跳点b
n
的横纵坐标。
[0113]
s6、搜索成功并保存正反方向搜索的路径节点即跳点坐标数据，即从起始节点到目标节点所经过的路径节点b1～b
n
；
[0114]
s7、采用微分平坦方法对生成的路径节点作曲线拟合；
[0115]
首先将路径建模为多项式方程模型，具体为：
[0116][0117]
其中：p为多项式轨迹参数，可以将之设置为参数向量：
[0118]
p＝[p0,p1,
…
,p
n
]
τ
[0119]
进一步将上述轨迹表述成向量形式：
[0120]
p(t)＝[1,t,t2,
…
,t
n
]
·
p
[0121]
对于任意时刻t，可以根据参数计算出轨迹的位置、速度、加速度，jerk、snap等。
[0122]
v(t)＝p
′
(t)＝[0,1,2t,3t2,4t3,...,nt
n
‑1p
[0123]
a(t)＝p
″
(t)＝[0,0,2,6t,12t2,...,n(n
‑
1)t
n
‑2]
·
p
[0124][0125][0126]
若将相邻跳点连线为一段，采用jps跳点搜索求得的路径可以看成由多条线段连接构成，对每一段线段采用多项式作曲线拟合，形如：
[0127][0128]
其中：k为轨迹段数，p
i
＝[p
i0
,p
i1
,...,p
in
]
τ
表示第i段轨迹的参数向量。
[0129]
路径优化的目的是：求出多项式轨迹参数p1,p2,...,p
k
，采用minimum snap:mimf(p)＝mim(p
(4)
(t))2，最小化目标函数snap，构建优化函数如下：
[0130][0131][0132]
其中：r和c为矩阵从0开始的行索引和列索引。对进行化简，
[0133][0134]
则，转化为二次规划问题。为保证两段轨迹之间连续，轨
迹经过路径节点所在栅格，避免与障碍物碰撞，需满足以下约束条件：
[0135][0136]
其中：s表示约束放宽的长度。
[0137]
给定包含起始节点和目标节点在内的n 2个二维路径点b
s
,b1,...,b
n
,b
g
,b
s
(x
s
,y
s
),b
i
(x
i
,y
i
),b
g
(x
i
,y
i
)，求解拟合后的路径。
[0138]
结合具体实施例，对基于边界查找的双向加权jps跳点搜索方法作进一步说明，图3为基于边界查找的双向jps跳点搜索方法示意图，尺寸为18
×
14的栅格地图，在图中，采用字母a～t、a1～t1和数字1～16分别对栅格地图边界的横向和纵向单元格作标记，黑色单元格表示使用膨胀法处理后的障碍物栅格，s和g分别表示起始节点和目标节点，p1～p9表示跳点，每条横向和垂直方向的黑色带箭头虚线代表一次边界查找的跳点搜索，黑色带箭头实线为最终搜索到的路径。
[0139]
表1和表2为与图3相对应的正向和反向搜索边界查找表。
[0140]
表1正向搜索边界查找表
[0141][0142]
表2反向搜索边界查找表
[0143][0144]
表1和表2中的值记录了横向和垂直边界查找的单元格在阻塞和开放状态之间切换的边界，正向搜索边界查找的横向边界查找的正方向为自左向右，垂直边界查找的正方向为自上而下；反向搜索边界查找的横向边界查找的正方向为自右向左，垂直边界查找的正方向为自上而下。
[0145]
如表1中横向边界查找，第1行，在单元格a所在列为地图边界呈现阻塞状态,记录为cells 1:(a)；第2～7行，在单元格a所在列为地图边界呈现阻塞状态，在单元格b所在列重新打开呈现开放状态，在单元格l所在列为障碍物边界呈现阻塞状态，在单元格m所在列
重新打开呈现开放状态，再到地图边界t所在列，记录为cells 2to 7:(a,b,l,m,t)；第8行，在单元格a所在列为地图边界呈现阻塞状态，在单元格b所在列重新打开呈现开放状态，在单元格g所在列为障碍物边界呈现阻塞状态，在单元格i所在列重新打开呈现开放状态，在单元格l所在列为障碍物边界呈现阻塞状态，在单元格m所在列重新打开呈现开放状态，再到地图边界t所在列，记录为cells 8:(a,b,g,i,l,m,t)；第9～10行，在单元格a所在列为地图边界呈现阻塞状态，在单元格b所在列重新打开呈现开放状态，在单元格g所在列为障碍物边界呈现阻塞状态，在单元格i所在列重新打开呈现开放状态，在单元格l所在列为障碍物边界呈现阻塞状态，在单元格m所在列重新打开呈现开放状态，在单元格p所在列为障碍物边界呈现阻塞状态，在单元格q所在列重新打开呈现开放状态，再到地图边界t所在列，记录为cells 9to 10:(a,b,g,i,l,m,p,q,t)；第11～15行，在单元格a所在列为地图边界呈现阻塞状态，在单元格b所在列重新打开呈现开放状态，在单元格g所在列为障碍物边界呈现阻塞状态，在单元格i所在列重新打开呈现开放状态，在单元格p所在列为障碍物边界呈现阻塞状态，在单元格q所在列重新打开呈现开放状态，再到地图边界t所在列，记录为cells 11to 15:(a,b,g,i,p,q,t)。
[0146]
垂直边界查找原理与横向边界查找相同，此处不在作过多赘述。表中的每个奇数位都是障碍物或地图边界的位置，而每个偶数位都是边界重新打开可通行网格空间的位置，例如cells 9to 10:(a,b,g,i,l,m,p,q,t)中，奇数位有a、g、l、p和t均表示障碍物或地图边界的位置，偶数位有b、i、m、q均表示边界重新打开可通行的网格空间的位置。
[0147]
传统双向jps跳点搜索中跳点识别方法是从直线和对角线方向上迭代每个相邻单元格，例如，在图3中从s起始节点搜索到p1跳点，需要水平检查1 1 2 2 3次和垂直检查1 10 11 11 12次，对角线检查6次，共60次迭代检查。而使用本专利的基于边界查找的双向加权跳点搜索方法，对每个轴仅执行一次检查以确定节点是否为跳点，对于水平检查，每次检查涉及查询节点的上面一行、同一行和下面一行的边界距离，共检查19次，显然采用本专利的方法识别跳点速度更快且能保证路径最优。
[0148]
最后对本实施例搜索到的路径，采用微分平坦方法对生成的路径节点作曲线拟合，路径节点包括s(2,2)、p1(5,5)、p2(16,6)、p3(5,8)、p4(16,7)、p5(6,9)、p6(15,8)、p7(7,9)、p8(12,5)、p9(16,5)、g(18,4)，搜索到的路径总长度为25.8m，拟合后的路径如图4所示。
[0149]
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何熟悉本专业的技术人员在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。