CN113515129A - 一种基于边界查找的双向跳点搜索无人车路径规划方法 - Google Patents

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

Info

Publication number
CN113515129A
CN113515129A CN202110965209.1A CN202110965209A CN113515129A CN 113515129 A CN113515129 A CN 113515129A CN 202110965209 A CN202110965209 A CN 202110965209A CN 113515129 A CN113515129 A CN 113515129A
Authority
CN
China
Prior art keywords
node
search
boundary
reverse
jumping
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110965209.1A
Other languages
English (en)
Other versions
CN113515129B (zh
Inventor
栾添添
胡占永
孙明晓
王皓
原张杰
付强
王万鹏
谢春旺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Wanzhida Technology Co ltd
Original Assignee
Harbin University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202110965209.1A priority Critical patent/CN113515129B/zh
Publication of CN113515129A publication Critical patent/CN113515129A/zh
Application granted granted Critical
Publication of CN113515129B publication Critical patent/CN113515129B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明涉及一种基于边界查找的双向跳点搜索无人车路径规划方法。所述方法包括：使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域，将正向搜索和反向搜索的起始节点分别放入OpenList1和OpenList2中，从正向和反向进行跳点交替迭代搜索，利用边界查找优化水平和垂直方向的节点搜索和跳点识别，且将跳点分别添加到OpenList1和OpenList2中，采用改进的正向和反向启发式估价函数分别计算OpenList1和OpenList2中代价最小的跳点，直到正向和反向搜索的当前节点重合，搜索成功并保存正反方向搜索的路径节点即跳点坐标数据，采用微分平坦方法对生成的路径节点作曲线拟合。本发明可有效提高节点搜索和跳点识别效率，减少路径寻优时间，保证路径最优性，避免拟合后的路径与障碍物碰撞。

Description

一种基于边界查找的双向跳点搜索无人车路径规划方法
技术领域
本发明涉及人工智能与无人系统领域，具体涉及自动驾驶路径规划技术领域，尤其涉及一种基于边界查找的双向跳点搜索无人车路径规划方法。
背景技术
在无人系统领域无人车的自动驾驶中，路径规划是其关键技术之一，针对这一技术目前已经开展了大量的研究工作。无人车路径规划可以分为静态路径规划和动态路径规划。其中A*算法、Dijkstra算法等目前主要应用于已知的静态场景中，而人工势场法、D*算法等则大多应用于环境未知动态场景中。其中跳点搜索(Jump Point Search,JPS)是对A*搜索算法的优化，通过剪枝规则，打破栅格的对称性，减少无效节点的访问，保留关键“跳点”，能让搜索在栅格上直线长“跳跃”，具有较好的路径寻优性能。但是，对于大规模和复杂化的栅格地图，JPS跳点搜索算法在节点搜索和跳点识别阶段会消耗大量时间，导致路径寻优速度降低，且无法保证路径最优性，所得路径不符合无人车运动学约束，需进一步优化处理。
针对上述问题，文献《一种面向非结构化环境的改进跳点搜索路径规划算法》提出加权JPS跳点搜索算法，存在如下问题：
(1)加权启发式估价函数只考虑了目标节点信息，未考虑起始节点信息，虽然提高了路径的搜索速度，但是不能保证路径最优性；
(2)采用微分平坦法对生成的路径节点作曲线拟合，未考虑安全距离，导致优化后的路径可能碰到障碍物。
文献《双向跳点搜索算法的移动机器人全局路径规划研究》提出双向JPS跳点搜索算法，存在如下问题：
(1)在跳点识别阶段，从直线和对角线方向上迭代每个相邻单元格，会消耗大量时间，导致路径寻优速度降低；
(2)采用JPS跳点搜索基本的启发式估价函数f(n)＝g(n)+h(n)，可以搜索到路径，但是计算时间长。
发明内容
本发明提供一种基于边界查找的双向跳点搜索无人车路径规划方法，从正向和反向进行跳点交替迭代搜索，通过边界查找提高水平和垂直方向上跳点的搜索速度，减少搜索跳点所涉及的大量迭代，设计改进的启发式估价函数提高路径寻优速度，保证路径最优性，采用微分平坦方法对生成的路径节点作曲线拟合，保证路径的连续性、平滑性和安全性。
为达到上述目的，本发明提供一种基于边界查找的双向跳点搜索无人车路径规划方法，包括以下步骤：
S1、使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域；
S2、将正向搜索和反向搜索的起始节点分别放入OpenList1和OpenList2中；
S3、正向搜索；
S4、反向搜索；
S5、从正向和反向进行跳点交替迭代搜索；
S6、搜索成功并保存正反方向搜索的路径节点即跳点坐标数据；
S7、采用微分平坦方法对生成的路径节点作曲线拟合；
所述的S1中，使用膨胀法对障碍物进行处理：
本专利中无人车和障碍物都可视为凸多边形，故采用Minkowski和的方法求解临界多边形，以无人车宽度一半为半径的圆B0，作B0与障碍物M的Minkowski和，以和的边界为障碍物拓展的边界。计算公式如下：
Figure BDA0003223627800000021
其中：
Figure BDA0003223627800000022
表示两个向量
Figure BDA0003223627800000023
和
Figure BDA0003223627800000024
的向量和。
所述的S3中，正向搜索具体包括以下子步骤：
S31、正向扩展子节点，判断当前节点是否为反向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S32、依据剪枝规则去掉冗余的对称节点，确定强制邻节点和跳点：
无人车在栅格地图上扩展子节点分为直线方向和对角线方向，对于直线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)≤len(＜p(a),a,n＞)
其中：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的父节点；
对于对角线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)＜len(＜p(a),a,n＞)
对于强制邻节点，根据以下规则进行筛选：
len(＜p(a),a,n＞)＜len(＜p(a),…,n＞\a)
S33、利用边界查找优化水平和垂直方向节点搜索和跳点识别，且将跳点添加到OpenList1中：
边界查找：
从正向搜索和反向搜索两个方面记录障碍边界的位置，障碍物由其边界的位置来定义，通过对网格进行预处理并记录这些边界的位置，边界查找不会在垂直或水平方向上迭代每个相邻单元格，相反，它会查找这些方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，并通过直接查找来减少在算法中搜索跳点所涉及的大量迭代，当在水平方向上的边界比其上方和下方所在行的重新打开值更远时，会识别左、右两边的跳点，垂直方向的工作原理相同，只是检查相邻列而不是行，东西方向使用水平边界查找表，南北方向使用垂直边界查找表；
跳点识别规则可表示为：b＝a+kd，从a节点出发，通过在d方向移动k步到达b，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
(1)节点b为目标节点；
(2)节点b含有至少一个强制邻节点；
(3)若d为对角线移动，存在c＝b+kidi，其中ki∈N，c是b的跳点，则b也是a的跳点；
S34、采用改进的正向启发式估价函数计算OpenList1中代价最小的跳点；
正向启发式估价函数为：
f(n)＝gi(n)+hi(n)+cross×0.001
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数，gi(n)表示起始节点到当前节点的累计实际代价，hi(n)表示当前节点到目标节点的估计代价，
Figure BDA0003223627800000041
cross表示原起始节点指向原目标节点的向量与当前节点指向原目标节点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达；
S35、将代价最小的跳点添加到CloseList1中。
所述的S4中，反向搜索具体包括以下子步骤：
S41、反向扩展子节点，判断当前节点是否为正向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S42、依据剪枝规则去掉冗余的对称节点，规则与正向搜索相同，确定强制邻节点和跳点：
S43、利用边界查找优化节点搜索和跳点识别，边界查找优化原理和跳点识别规则与正向搜索相同，且将跳点添加到OpenList2中：
S44、采用改进的反向启发式估价函数计算OpenList2中代价最小的跳点；
反向启发式估价函数为：
f′(n)＝gj(n)+hj(n)+cross×0.001
其中：f′(n)表示反向搜索从起始节点到当前节点到目标节点的代价函数，gj(n)表示起始节点到当前节点的累计实际代价，hj(n)表示当前节点到目标节点的估计代价，且采用欧式距离作距离表达；
S45、将代价最小的跳点添加到CloseList2中。
所述的S5中，从正向和反向进行跳点交替迭代搜索的过程如下：
Bs表示正向搜索的起始节点，Bg表示反向搜索的起始节点，首先进行正向搜索，以Bs为正向搜索的起始节点，Bg为正向搜索的目标节点进行子节点扩展搜索，直到搜索到代价最小的跳点B1；切换为反向搜索，以Bg为反向搜索的起始节点，B1为反向搜索的目标节点进行搜索，搜索到代价最小跳点Bn；切换为正向搜索，以Bs为正向搜索的起始节点，Bn为正向搜索的目标节点进行搜索，以此进行交替迭代搜索，直到正向和反向搜索的当前节点重合，则说明已经发现了一条完整路径。
算法交替迭代方法如下所示：
Figure BDA0003223627800000051
Figure BDA0003223627800000052
其中：Bs(xs,ys)表示原起始节点Bs的横纵坐标，Bg(xg,yg)表示原目标节点Bg的横纵坐标，B1(x1,y1)表示正向搜索代价最小的跳点B1的横纵坐标，Bn(xn,yn)表示反向搜索代价最小的跳点Bn的横纵坐标。
本发明具有如下有益效果：
1.通过边界查找水平和垂直方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，以提高水平和垂直方向上跳点的搜索速度，减少搜索跳点所涉及的大量迭代；
2.设计双向改进的启发式估价函数，从正向和反向进行交替迭代路径搜索，使得路径搜索时间和扩展节点大大减少，提高路径寻优速度，保证路径最优性；
3.采用微分平坦方法对生成的路径节点作曲线拟合，避免与障碍物碰撞，保证路径的连续性、平滑性和安全性。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本专利的其它特征、目的和优点将会变得更明显：
图1基于边界查找的双向跳点搜索算法的流程框图；
图2正向和反向交替迭代搜索的示意图；
图3基于边界查找的双向JPS跳点搜索算法示意图；
图4采用微分平坦方法对实施例中生成的路径节点作曲线拟合的示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。
如图1所示，本发明提供了一种基于边界查找的双向跳点搜索无人车路径规划方法，具体包括以下步骤：
S1、使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域，具体包括以下子步骤；
S11、本专利中无人车和障碍物都可视为凸多边形，故采用Minkowski和的方法求解临界多边形，以无人车宽度一半为半径的圆B0，作B0与障碍物M的Minkowski和，以和的边界为障碍物拓展的边界。计算公式如下：
Figure BDA0003223627800000061
其中：
Figure BDA0003223627800000062
表示两个向量
Figure BDA0003223627800000063
和
Figure BDA0003223627800000064
的向量和。
S12、以一定的长度C将无人车移动平面离散化，划分为若干等分的栅格，对每个栅格进行编号，并将存在障碍物的栅格标记为黑色，表示不可通行，不存在障碍物的栅格标记为白色，表示可通行。
S2、将正向搜索和反向搜索的起始节点分别放入OpenList1和OpenList2中；
S3、正向搜索，具体包括以下子步骤：
S31、正向扩展子节点，判断当前节点是否为反向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S32、依据剪枝规则去掉冗余的对称节点，确定强制邻节点和跳点：
无人车在栅格地图上扩展子节点分为直线方向和对角线方向，对于直线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)≤len(＜p(a),a,n＞)
其中：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的父节点；
对于对角线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)＜len(＜p(a),a,n＞)
对于强制邻节点，根据以下规则进行筛选：
len(＜p(a),a,n＞)＜len(＜p(a),…,n＞\a)
S33、利用边界查找优化水平和垂直方向节点搜索和跳点识别，且将跳点添加到OpenList1中：
边界查找：
从正向搜索和反向搜索两个方面记录障碍边界的位置，障碍物由其边界的位置来定义，通过对网格进行预处理并记录这些边界的位置，边界查找不会在垂直或水平方向上迭代每个相邻单元格，相反，它会查找这些方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，并通过直接查找来减少在算法中搜索跳点所涉及的大量迭代，当在水平方向上的边界比其上方和下方所在行的重新打开值更远时，会识别左、右两边的跳点，垂直方向的工作原理相同，只是检查相邻列而不是行，东西方向使用水平边界查找表，南北方向使用垂直边界查找表；
跳点识别规则可表示为：b＝a+kd，从a节点出发，通过在d方向移动k步到达b节点，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
(1)节点b为目标点；
(2)节点b含有至少一个强制邻节点；
(3)若d为对角线移动，存在c＝b+kidi，其中ki∈N，c是b的跳点，则b也是a的跳点；
S34、采用改进的正向启发式估价函数计算OpenList1中代价最小的跳点；
正向启发式估价函数为：
f(n)＝gi(n)+hi(n)+cross×0.001
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数，gi(n)表示起始节点到当前节点的累计实际代价，hi(n)表示当前节点到目标节点的估计代价，
Figure BDA0003223627800000071
cross表示原起始节点指向原目标节点的向量与当前节点指向原目标节点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达；
S35、将代价最小的跳点添加到CloseList1中。
S4、反向搜索，具体包括以下子步骤：
S41、反向扩展子节点，判断当前节点是否为正向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S42、依据剪枝规则去掉冗余的对称节点，规则与正向搜索相同，确定强制邻节点和跳点：
S43、利用边界查找优化节点搜索和跳点识别，边界查找优化原理和跳点识别规则与正向搜索相同，且将跳点添加到OpenList2中：
S44、采用改进的反向启发式估价函数计算OpenList2中代价最小的跳点；
反向启发式估价函数为：
f′(n)＝gj(n)+hj(n)+cross×0.001
其中：f′(n)表示反向搜索从起始节点到当前节点到目标节点的代价函数，gj(n)表示起始节点到当前节点的累计实际代价，hj(n)表示当前节点到目标节点的估计代价，且采用欧式距离作距离表达。
S45、将代价最小的跳点添加到CloseList2中。
S5、从正向和反向进行跳点交替迭代搜索，如图2所示：
Bs表示正向搜索的起始节点，Bg表示反向搜索的起始节点，首先进行正向搜索，以Bs为正向搜索的起始节点，Bg为正向搜索的目标节点进行子节点扩展搜索，直到搜索到代价最小的跳点B1；切换为反向搜索，以Bg为反向搜索的起始节点，B1为反向搜索的目标节点进行搜索，搜索到代价最小跳点Bn；切换为正向搜索，以Bs为正向搜索的起始节点，Bn为正向搜索的目标节点进行搜索，以此进行交替迭代搜索，直到正向和反向搜索的当前节点重合，则说明已经发现了一条完整路径。
算法交替迭代方法如下所示：
Figure BDA0003223627800000081
Figure BDA0003223627800000082
其中：Bs(xs,ys)表示原起始节点Bs的横纵坐标，Bg(xg,yg)表示原目标节点Bg的横纵坐标，B1(x1,y1)表示正向搜索代价最小的跳点B1的横纵坐标，Bn(xn,yn)表示反向搜索代价最小的跳点Bn的横纵坐标。
S6、搜索成功并保存正反方向搜索的路径节点即跳点坐标数据，即从起始节点到目标节点所经过的路径节点B1～Bn；
S7、采用微分平坦方法对生成的路径节点作曲线拟合；
首先将路径建模为多项式方程模型，具体为：
Figure BDA0003223627800000091
其中：p为多项式轨迹参数，可以将之设置为参数向量：
p＝[p0,p1,…,pn]Τ
进一步将上述轨迹表述成向量形式：
p(t)＝[1,t,t2,…,tn]·p
对于任意时刻t，可以根据参数计算出轨迹的位置、速度、加速度，jerk、snap等。
v(t)＝p′(t)＝[0,1,2t,3t2,4t3,...,ntn-1p
a(t)＝p″(t)＝[0,0,2,6t,12t2,...,n(n-1)tn-2]·p
Figure BDA0003223627800000092
Figure BDA0003223627800000093
若将相邻跳点连线为一段，采用JPS跳点搜索求得的路径可以看成由多条线段连接构成，对每一段线段采用多项式作曲线拟合，形如：
Figure BDA0003223627800000094
其中：k为轨迹段数，pi＝[pi0,pi1,...,pin]Τ表示第i段轨迹的参数向量。
路径优化的目的是：求出多项式轨迹参数p1,p2,...,pk，采用minimum snap:mimf(p)＝mim(p(4)(t))2，最小化目标函数snap，构建优化函数如下：
Figure BDA0003223627800000101
Figure BDA0003223627800000102
其中：r和c为矩阵从0开始的行索引和列索引。对
Figure BDA0003223627800000103
进行化简，
Figure BDA0003223627800000104
则，
Figure BDA0003223627800000105
转化为二次规划问题。为保证两段轨迹之间连续，轨迹经过路径节点所在栅格，避免与障碍物碰撞，需满足以下约束条件：
Figure BDA0003223627800000106
其中：S表示约束放宽的长度。
给定包含起始节点和目标节点在内的n+2个二维路径点Bs,B1,...,Bn,Bg,Bs(xs,ys),Bi(xi,yi),Bg(xi,yi)，求解拟合后的路径。
结合具体实施例，对基于边界查找的双向加权JPS跳点搜索方法作进一步说明，图3为基于边界查找的双向JPS跳点搜索方法示意图，尺寸为18×14的栅格地图，在图中，采用字母A～T、A1～T1和数字1～16分别对栅格地图边界的横向和纵向单元格作标记，黑色单元格表示使用膨胀法处理后的障碍物栅格，s和g分别表示起始节点和目标节点，p1～p9表示跳点，每条横向和垂直方向的黑色带箭头虚线代表一次边界查找的跳点搜索，黑色带箭头实线为最终搜索到的路径。
表1和表2为与图3相对应的正向和反向搜索边界查找表。
表1正向搜索边界查找表
Figure BDA0003223627800000111
表2反向搜索边界查找表
Figure BDA0003223627800000112
表1和表2中的值记录了横向和垂直边界查找的单元格在阻塞和开放状态之间切换的边界，正向搜索边界查找的横向边界查找的正方向为自左向右，垂直边界查找的正方向为自上而下；反向搜索边界查找的横向边界查找的正方向为自右向左，垂直边界查找的正方向为自上而下。
如表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所在列，记录为Cells11to 15:(A,B,G,I,P,Q,T)。
垂直边界查找原理与横向边界查找相同，此处不在作过多赘述。表中的每个奇数位都是障碍物或地图边界的位置，而每个偶数位都是边界重新打开可通行网格空间的位置，例如Cells 9to 10:(A,B,G,I,L,M,P,Q,T)中，奇数位有A、G、L、P和T均表示障碍物或地图边界的位置，偶数位有B、I、M、Q均表示边界重新打开可通行的网格空间的位置。
传统双向JPS跳点搜索中跳点识别方法是从直线和对角线方向上迭代每个相邻单元格，例如，在图3中从s起始节点搜索到p1跳点，需要水平检查1+1+2+2+3次和垂直检查1+10+11+11+12次，对角线检查6次，共60次迭代检查。而使用本专利的基于边界查找的双向加权跳点搜索方法，对每个轴仅执行一次检查以确定节点是否为跳点，对于水平检查，每次检查涉及查询节点的上面一行、同一行和下面一行的边界距离，共检查19次，显然采用本专利的方法识别跳点速度更快且能保证路径最优。
最后对本实施例搜索到的路径，采用微分平坦方法对生成的路径节点作曲线拟合，路径节点包括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所示。
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何熟悉本专业的技术人员在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (1)

1.一种基于边界查找的双向跳点搜索无人车路径规划方法，其特征在于，包括以下步骤：
S1、使用膨胀法对障碍物进行处理，采用栅格法划分搜索区域；
S2、将正向搜索和反向搜索的起始节点分别放入OpenList1和OpenList2中；
S3、正向搜索；
S4、反向搜索；
S5、从正向和反向进行跳点交替迭代搜索；
S6、搜索成功并保存正反方向搜索的路径节点即跳点坐标数据；
S7、采用微分平坦方法对生成的路径节点作曲线拟合；
所述的S3中，正向搜索，具体包括以下子步骤：
S31、正向扩展子节点，判断当前节点是否为反向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S32、依据剪枝规则去掉冗余的对称节点，确定强制邻节点和跳点：
无人车在栅格地图上扩展子节点分为直线方向和对角线方向，对于直线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)≤len(＜p(a),a,n＞)
其中：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的父节点；
对于对角线方向，裁剪掉所有符合以下条件的节点n：
len(＜p(a),…,n＞\a)＜len(＜p(a),a,n＞)
对于强制邻节点，根据以下规则进行筛选：
len(＜p(a),a,n＞)＜len(＜p(a),…,n＞\a)
S33、利用边界查找优化水平和垂直方向节点搜索和跳点识别，且将跳点添加到OpenList1中；
边界查找：
从正向搜索和反向搜索两个方面记录障碍边界的位置，障碍物由其边界的位置来定义，通过对网格进行预处理并记录这些边界的位置，边界查找不会在垂直或水平方向上迭代每个相邻单元格，相反，它会查找这些方向上是否存在边界或障碍物，并立即评估当前节点是否为跳点，并通过直接查找来减少在算法中搜索跳点所涉及的大量迭代，当在水平方向上的边界比其上方和下方所在行的重新打开值更远时，会识别左、右两边的跳点，垂直方向的工作原理相同，只是检查相邻列而不是行，东西方向使用水平边界查找表，南北方向使用垂直边界查找表；
跳点识别规则可表示为：b＝a+kd，从a节点出发，通过在d方向移动k步到达b节点，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
(1)节点b为目标节点；
(2)节点b含有至少一个强制邻节点；
(3)若d为对角线移动，存在c＝b+kidi，其中ki∈N，c是b的跳点，则b也是a的跳点；
S34、采用改进的正向启发式估价函数计算OpenList1中代价最小的跳点；
正向启发式估价函数为：
f(n)＝gi(n)+hi(n)+cross×0.001
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数，gi(n)表示起始节点到当前节点的累计实际代价，hi(n)表示当前节点到目标节点的估计代价，
Figure FDA0003223627790000021
cross表示原起始节点指向原目标节点的向量与当前节点指向原目标节点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达；
S35、将代价最小的跳点添加到CloseList1中；
所述的S4中，反向搜索，具体包括以下子步骤：
S41、反向扩展子节点，判断当前节点是否为正向搜索的当前节点，若是则跳转到S6，否则继续执行以下步骤；
S42、依据剪枝规则去掉冗余的对称节点，规则与正向搜索相同，确定强制邻节点和跳点；
S43、利用边界查找优化节点搜索和跳点识别，边界查找优化原理和跳点识别规则与正向搜索相同，且将跳点添加到OpenList2中；
S44、采用改进的反向启发式估价函数计算OpenList2中代价最小的跳点；
反向启发式估价函数为：
f′(n)＝gj(n)+hj(n)+cross×0.001
其中：f′(n)表示反向搜索从起始节点到当前节点到目标节点的代价函数，gj(n)表示起始节点到当前节点的累计实际代价，hj(n)表示当前节点到目标节点的估计代价，且采用欧式距离作距离表达；
S45、将代价最小的跳点添加到CloseList2中；
所述的S5中，从正向和反向进行跳点交替迭代搜索的具体过程如下：
Bs表示正向搜索的起始节点，Bg表示反向搜索的起始节点，首先进行正向搜索，以Bs为正向搜索的起始节点，Bg为正向搜索的目标节点进行子节点扩展搜索，直到搜索到代价最小的跳点B1；切换为反向搜索，以Bg为反向搜索的起始节点，B1为反向搜索的目标节点进行搜索，直到搜索到代价最小跳点Bn；切换为正向搜索，以Bs为正向搜索的起始节点，Bn为正向搜索的目标节点进行搜索，以此进行交替迭代搜索，直到正向和反向搜索的当前节点重合，则说明已经发现了一条完整路径；
算法交替迭代方法如下所示：
Figure FDA0003223627790000031
Figure FDA0003223627790000032
其中：Bs(xs,ys)表示原起始节点Bs的横纵坐标，Bg(xg,yg)表示原目标节点Bg的横纵坐标，B1(x1,y1)表示正向搜索代价最小的跳点B1的横纵坐标，Bn(xn,yn)表示反向搜索代价最小的跳点Bn的横纵坐标。
CN202110965209.1A 2021-08-23 2021-08-23 一种基于边界查找的双向跳点搜索无人车路径规划方法 Active CN113515129B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110965209.1A CN113515129B (zh) 2021-08-23 2021-08-23 一种基于边界查找的双向跳点搜索无人车路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110965209.1A CN113515129B (zh) 2021-08-23 2021-08-23 一种基于边界查找的双向跳点搜索无人车路径规划方法

Publications (2)

Publication Number Publication Date
CN113515129A true CN113515129A (zh) 2021-10-19
CN113515129B CN113515129B (zh) 2022-02-11

Family

ID=78069119

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110965209.1A Active CN113515129B (zh) 2021-08-23 2021-08-23 一种基于边界查找的双向跳点搜索无人车路径规划方法

Country Status (1)

Country Link
CN (1) CN113515129B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114431771A (zh) * 2021-12-31 2022-05-06 浙江大华技术股份有限公司 一种扫地机器人清扫方法及相关装置
CN114510056A (zh) * 2022-02-21 2022-05-17 上海机器人产业技术研究院有限公司 室内移动机器人的平稳移动全局路径规划方法
CN114564023A (zh) * 2022-03-11 2022-05-31 哈尔滨理工大学 一种动态场景下的跳点搜索路径规划方法
CN115118326A (zh) * 2022-06-17 2022-09-27 凯睿星通信息科技(南京)股份有限公司 基于双向跳跃式搜索的卫星通信路由选择方法、系统及装置
CN116301026A (zh) * 2023-01-13 2023-06-23 中国建筑一局(集团)有限公司 一种复杂环境下四旋翼无人机大机动敏捷飞行方法
CN117451057A (zh) * 2023-12-25 2024-01-26 长春理工大学 基于改进a*算法无人机三维路径规划方法、设备和介质
CN117647706A (zh) * 2024-01-30 2024-03-05 山东昊能电力建设有限公司 一种基于大数据的智慧电网运行故障诊断系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005032196A (ja) * 2003-07-11 2005-02-03 Japan Science & Technology Agency 移動ロボット用経路計画システム
CN105955280A (zh) * 2016-07-19 2016-09-21 Tcl集团股份有限公司 移动机器人路径规划和避障方法及系统
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
CN111309004A (zh) * 2019-12-06 2020-06-19 江苏南大电子信息技术股份有限公司 一种基于改进的跳点搜索算法移动机器人路径规划方法
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN112915541A (zh) * 2021-04-07 2021-06-08 腾讯科技（深圳）有限公司 跳点搜索方法、装置、设备及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005032196A (ja) * 2003-07-11 2005-02-03 Japan Science & Technology Agency 移動ロボット用経路計画システム
CN105955280A (zh) * 2016-07-19 2016-09-21 Tcl集团股份有限公司 移动机器人路径规划和避障方法及系统
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
CN111309004A (zh) * 2019-12-06 2020-06-19 江苏南大电子信息技术股份有限公司 一种基于改进的跳点搜索算法移动机器人路径规划方法
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN112915541A (zh) * 2021-04-07 2021-06-08 腾讯科技（深圳）有限公司 跳点搜索方法、装置、设备及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
马小陆 等: "双向跳点搜索算法的移动机器人全局路径规划研究", 《机械科学与技术》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114431771A (zh) * 2021-12-31 2022-05-06 浙江大华技术股份有限公司 一种扫地机器人清扫方法及相关装置
CN114510056A (zh) * 2022-02-21 2022-05-17 上海机器人产业技术研究院有限公司 室内移动机器人的平稳移动全局路径规划方法
CN114564023A (zh) * 2022-03-11 2022-05-31 哈尔滨理工大学 一种动态场景下的跳点搜索路径规划方法
CN114564023B (zh) * 2022-03-11 2022-11-08 哈尔滨理工大学 一种动态场景下的跳点搜索路径规划方法
CN115118326A (zh) * 2022-06-17 2022-09-27 凯睿星通信息科技(南京)股份有限公司 基于双向跳跃式搜索的卫星通信路由选择方法、系统及装置
CN115118326B (zh) * 2022-06-17 2024-05-17 凯睿星通信息科技(南京)股份有限公司 基于双向跳跃式搜索的卫星通信路由选择方法、系统及装置
CN116301026A (zh) * 2023-01-13 2023-06-23 中国建筑一局(集团)有限公司 一种复杂环境下四旋翼无人机大机动敏捷飞行方法
CN117451057A (zh) * 2023-12-25 2024-01-26 长春理工大学 基于改进a*算法无人机三维路径规划方法、设备和介质
CN117451057B (zh) * 2023-12-25 2024-03-12 长春理工大学 基于改进a*算法无人机三维路径规划方法、设备和介质
CN117647706A (zh) * 2024-01-30 2024-03-05 山东昊能电力建设有限公司 一种基于大数据的智慧电网运行故障诊断系统及方法
CN117647706B (zh) * 2024-01-30 2024-04-05 山东昊能电力建设有限公司 一种基于大数据的智慧电网运行故障诊断系统及方法

Also Published As

Publication number Publication date
CN113515129B (zh) 2022-02-11

Similar Documents

Publication Publication Date Title
CN113515129B (zh) 一种基于边界查找的双向跳点搜索无人车路径规划方法
CN113485369B (zh) 改进a*算法的室内移动机器人路径规划和路径优化方法
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
Deng et al. Multi-obstacle path planning and optimization for mobile robot
CN113110520B (zh) 一种多智能优化并行算法的机器人路径规划方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN106647769A (zh) 基于a*提取引导点的agv路径跟踪与避障协调方法
CN111811514A (zh) 一种基于正六边形栅格跳点搜索算法的路径规划方法
CN109163722A (zh) 一种仿人机器人路径规划方法及装置
Choi et al. Any-angle path planning on non-uniform costmaps
CN114089765A (zh) 一种面向城市绿地的割草机器人遍历路径规划方法
CN115407784B (zh) 一种基于空地信息互补的无人车路径规划方法
CN111721296B (zh) 一种水下无人航行器数据驱动路径规划方法
CN112612267B (zh) 自动驾驶的路径规划方法和装置
CN115167474A (zh) 一种移动机器人路径规划优化方法
Gu et al. Path planning for mobile robot in a 2.5‐dimensional grid‐based map
CN114740898A (zh) 一种基于自由空间与a*算法的三维航迹规划方法
CN113325834A (zh) 一种基于图形预处理的改进a*算法的路径规划方法
CN113848911A (zh) 基于Q-learning和RRT*的移动机器人全局路径规划方法
Fan et al. Research and implementation of multi-robot path planning based on genetic algorithm
CN116560360A (zh) 面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统
CN116698065A (zh) 融合运动约束和安全约束的自动驾驶车辆路径规划方法及介质
CN114564023B (zh) 一种动态场景下的跳点搜索路径规划方法
CN115586769A (zh) 移动机器人路径规划方法和系统
CN115016461A (zh) 一种基于动态终点策略的IA-Star算法的机器人路径规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20240906

Address after: 518000 1002, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Patentee after: Shenzhen Wanzhida Technology Co.,Ltd.

Country or region after: China

Address before: 150080 Harbin University of Science and Technology, Harbin Road, Nangang District, Heilongjiang, 52

Patentee before: HARBIN University OF SCIENCE AND TECHNOLOGY

Country or region before: China

TR01 Transfer of patent right