CN112923946A - 一种基于Hybrid-astar的动态路径规划方法 - Google Patents

一种基于Hybrid-astar的动态路径规划方法 Download PDF

Info

Publication number
CN112923946A
CN112923946A CN202110216076.8A CN202110216076A CN112923946A CN 112923946 A CN112923946 A CN 112923946A CN 202110216076 A CN202110216076 A CN 202110216076A CN 112923946 A CN112923946 A CN 112923946A
Authority
CN
China
Prior art keywords
path
point
planning
hybrid
astar
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
CN202110216076.8A
Other languages
English (en)
Other versions
CN112923946B (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.)
Guangzhou Haige Tiancheng Technology Co ltd
Guangzhou Haige Communication Group Inc Co
Original Assignee
Guangzhou Haige Communication Group Inc Co
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 Guangzhou Haige Communication Group Inc Co filed Critical Guangzhou Haige Communication Group Inc Co
Priority to CN202110216076.8A priority Critical patent/CN112923946B/zh
Publication of CN112923946A publication Critical patent/CN112923946A/zh
Application granted granted Critical
Publication of CN112923946B publication Critical patent/CN112923946B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于Hybrid‑astar的动态路径规划方法，包括：S1，根据已知的全局路径、用户设置的最大路径规划长度dmax和可变规划距离dplan进行可变规划距离的粒度分割，生成全局路径；S2，对动态路径进行优化；其中，对动态路径进行优化的步骤包括：分段规划多点组合路径搜索、角度平滑和贝塞尔曲线差值平滑中的至少一种；S3，将优化后的路径周期性下发至无人车，无人车根据路径行驶。本发明按照可变规划距离进行粒度分割，生成全局路径，将全局路径分解为若干路径点子集；采用多点组合路径搜索方法进行路径搜索，提高路径搜索的成功概率。

Description

一种基于Hybrid-astar的动态路径规划方法
技术领域
本发明涉及车辆动态路径规划技术领域，具体涉及一种基于Hybrid-astar的动态路径规划方法。
背景技术
路径规划中常常使用混合A*算法,混合A*(Hybrid-astar)是一种适用于全局、静态的路径规划算法。通过连续坐标系下进行启发式搜索生成运动轨迹，并对运动轨迹进行车辆非完整性约束得到车辆的行驶最终运动轨迹。传统的A*算法使用2D欧式距离启发式搜索，混合A*使用车辆非完整性约束启发式搜索和完整性约束启发式搜索，同时使用Reeds-Shepp曲线加快搜索速度。参见图1，混合A*算法的实现步骤为：
1)把起点加入open list；
2)重复以下步骤遍历所有节点：
①遍历open list，查找F(Reed-Shepp距离/Dubins距离)值最小的节点，把它作为当前要处理的节点。
②把这个节点移到close list。
③对当前节点的8个相邻方向的节点进行遍历。
a.如果它是不可抵达的或者它在close list中，忽略它。否则，做如下处理。
b.把该节点加入open list，并且把当方格设置为它的父亲，记录该方格的F,G和H值。
c.G值作衡量该路径是否是最优路径，G值越小路径越优。如果G值最小，把它的父亲设置为当前方格，并重新计算它的G和F值。
④生成Reed-Shepp曲线，并进行碰撞检测，把终点加入到了open list中，停止路径遍历。
3)保存路径。从终点开始，每个方格沿着父节点移动直至起点，得到混合A*规划好的路径。
混合A*是一种适用于静态全局、由始点到终点的两点路径规划算法。其在做长距离或动态的行驶规划中耗时长、无法避开动态障碍物；并且混合A*算法只适用于近似最优路径规划，无法满足用户特性化需求，所以并不适用于动态的路径规划。
因此,行业内急需研发一种基于动态、多点路的路径规划算法。
发明内容
本发明的目的是为了克服以上现有技术存在的不足，提供了一种适用于动态、高速应用场景，且能实时避障能力的Hybrid-astar的动态路径规划方法。
本发明的目的通过以下的技术方案实现：
一种基于Hybrid-astar的动态路径规划方法，包括：
S1，根据已知的全局路径、用户设置的最大路径规划长度dmax和可变规划距离dplan进行可变规划距离的粒度分割，生成全局路径；
S2，对动态路径进行优化；其中，对动态路径进行优化的步骤包括：分段规划多点组合路径搜索、角度平滑和贝塞尔曲线差值平滑中的至少一种；
S3，将优化后的路径周期性下发至无人车，无人车根据路径行驶。
优选地，步骤S1包括：根据车辆当前位置，在全局路径集合Ψ＝{p1，p2，…，pn}中查找离车辆最近的点Pnearest，将离车辆最近的点Pnearest作为规划的起始点，并加入到路径子集
Figure BDA0002953843600000031
中；从起始点开始逐点遍历全局路径集合Ψ，计算全局路径集合Ψ中的其余点与起始点的累计欧式距离
Figure BDA0002953843600000032
当di大于dplan时，将当前点加入到路径子集中
Figure BDA0002953843600000033
循环依次遍历进行粒度分割，当路径子集累计总距离
Figure BDA0002953843600000034
大于dmax时，终止路径点的筛选，得到路径子集合
Figure BDA0002953843600000035
优选地，在步骤S2中，若在单次规划中起点p′i与终点p′j(i＜j)规划失败，则进行分段规划多点组合路径搜索，具体为：保持终点p′j不变，在p′i点之前规划成功的点集中由后至前依次重选起点，若在预设重选起点阈值范围内的点p′k(k＜i)到Pj规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集；若在预设重选起点阈值范围内的点到p′j均未规划成功，则进行终点重选，保持起点p′i不变，在终点p′j(i＜j)点之后规划成功的点集中由前至后依次重选终点，若起点p′i到在预设重选终点阈值范围内的点p′k(k＜i)规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集。
优选地，预设重选起点阈值和预设重选终点阈值均为3次。
优选地，角度平滑包括对路径子集
Figure BDA0002953843600000038
中相邻规划点的角度进行平滑处理，保持两个相邻的点在同一象限或者保证相邻的两个点的角度差值小于k，k>0；具体为：判断路径集合
Figure BDA0002953843600000036
中当前相邻两点的弧度αi-1和αi差值是否超过预设角度阈值，若超过预设角度阈值则按照
Figure BDA0002953843600000037
调整前一个点αi-1；
判断αi-2和αi-1是否超过预设角度阈值，若超过预设角度阈值则按照
Figure BDA0002953843600000041
调整；
判断αi-3和αi-2是否超过预设角度阈值，如果超过预设角度阈值则按照
Figure BDA0002953843600000042
调整；
其中，路径集合
Figure BDA0002953843600000043
的各点之间的角度αi＝arctan(Δy，Δx)，而Δy＝yi+1-yi，Δx＝xi+1-xi。
优选地，预设角度阈值为π/4。
优选地，使用贝塞尔曲线插值对路径轨迹进行平滑，贝塞尔曲线插值公式为：
Figure BDA0002953843600000044
其中，t∈[0，1]，p为锚点或者控制点的坐标，p0为始点坐标，pn为终点坐标，pi为控制点坐标。
优选地，步骤S3包括：将每次规划好的路线存储在链表中，并根据当前无人车的位置，将规划好的路径按5m/40ms的周期给下发至无人车，无人车根据路线行驶。
优选地，无人车根据路线行驶过程中进行动态避障，具体为：根据相关参数使用基于Hybrid-astar的动态路径规划方法周期性的计算地图的障碍，规划新的路径，相关参数为目标路径点、方向、弯道弧度、障碍物尺寸、车辆转弯半径。
优选地，假设Ti规划出来的路径为Li，Ti+1时刻为Li+1，当点pi与pk存在动态的障碍物时，更新pi与pk两点之间规划出来的路线就实现动态规划和避障功能。
本发明相对于现有技术具有如下优点：
本发明按照可变规划距离进行粒度分割，生成全局路径，将全局路径分解为若干路径点子集；采用多点组合路径搜索方法进行路径搜索，提高路径搜索的成功概率；对规划后的路径子集进行多次角度平滑，解决多段子集路径集合符合运动学的轨迹要求；另外，障碍物变化时，只需要重新规划障碍物所在的路径子集区域就更新局部路线，不需要重新对整个路径进行规划，从而实现动态的避障路径规划。至此解决了混合A*无法实现较长距离路径规划、动态路径规划、按用户下发的多点路径规划等问题。
附图说明
构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为传统的混合A*算法的流程示意图。
图2为本实施例的基于Hybrid-astar的动态路径规划方法的流程示意图。
图3为本实施例的多点组合路径搜索的流程示意图。
图4为本实施例的栅格的启发式路径搜索的示意图。
图5为本实施例的非完备式栅格的启发式路径搜索的示意图。
图6为本实施例的角度平滑的流程示意图。
图7的(a)为本实施例的一规划路径角度平滑处理前的方向变化图。
图7的(b)为本实施例的一规划路径经角度平滑处理后的方向变化图。
图7的(c)为本实施例的另一规划路径的角度平滑处理前的方向变化图。
图7的(d)为本实施例的另一规划路径的角度平滑处理后的方向变化图。
图8的(a)为图7的(a)中相应子图的放大图。
图8的(b)为图7的(b)中相应子图的放大图
图8的(c)为图7的(c)中相应子图的放大图
图8的(d)为图7的(d)中相应子图的放大图
图9的(a)为本实施例的规划出的一轨迹图。
图9的(b)为本实施例的规划出的另一轨迹图。
图10的(a)为本实施例的经平滑后的一轨迹图。
图10的(b)为本实施例的经平滑后的另一轨迹图。
图11的(a)为本实施例的另一规划路径经角度、差值平滑处理前的方向变化图。
图11的(b)为本发明的另一规划路径经差值平滑处理的方向变化图。
图11的(c)为本发明的另一规划路径经角度平滑处理的方向变化图。
图11的(d)为本发明的另一规划路径经角度、差值平滑处理后的方向变化图。
图11的(e)为本发明的一规划路径经角度、差值平滑处理前的方向变化图。
图11的(f)为本发明的一规划路径经差值平滑处理的方向变化图。
图11的(g)为本发明的一规划路径经角度平滑处理的方向变化图。
图11的(h)为本发明的一规划路径经角度、差值平滑处理后的方向变化图。
图12的(a)为本实施例的一规划路径的测试结果图。
图12的(b)为本发明的另一规划路径的测试结果图。
具体实施方式
下面结合附图和实施例对本发明作进一步说明。
参见图2、一种基于Hybrid-astar的动态路径规划方法，包括：
S1，根据已知的全局路径、用户设置的最大路径规划长度dmax和可变规划距离dplan进行可变规划距离的粒度分割，生成全局路径；分割粒度可以根据车辆机动性能、路况等实际需求进行灵活设置，以满足不同应用场景。具体地，步骤S1包括根据车辆当前位置，在全局路径集合Ψ＝{p1，p2，…，pn}中查找离车辆最近的点Pnearest，将离车辆最近的点Pnearest作为规划的起始点，并加入到路径子集
Figure BDA0002953843600000076
中；
从起始点开始逐点遍历全局路径集合Ψ，计算全局路径集合Ψ中的其余点与起始点的累计欧式距离
Figure BDA0002953843600000071
当di大于dplan时，将当前点加入到路径子集中
Figure BDA0002953843600000072
循环依次遍历进行粒度分割，当路径子集累计总距离
Figure BDA0002953843600000073
大于dmax时，终止路径点的筛选，得到路径子集合
Figure BDA0002953843600000074
S2，对动态路径进行优化；其中，对动态路径进行优化的步骤包括：分段规划多点组合路径搜索、角度平滑和贝塞尔曲线差值平滑中的至少一种；
S3，将优化后的路径周期性下发至无人车，无人车根据路径行驶。
在本实施例，在步骤S2中，若在单次规划中起点p′i与终点p′j(i＜j)规划失败，则进行分段规划多点组合路径搜索。分段规划多点组合路径搜索参见图3，本发明基于全局路径按照可变规划距离进行粒度分割，遍历路径子集
Figure BDA0002953843600000077
由近及远两两相邻路径点进行路径规划，在两点规划失败的情况下根据重选点策略变换起始点或者终点，这样可以增强数据的容错能力，允许部分点的规划失败，提高总体路径规划成功概率。
由可变规划距离的粒度分割得到新的路径子集合
Figure BDA0002953843600000075
重选点策略是在单次规划中起点p′i与终点p′j(i＜j)规划失败的情况下触发，包括起点重选和终点重选。具体为：保持终点p′j不变，在p′i点之前规划成功的点集中由后至前依次重选起点，若在预设重选起点阈值范围内的点p′k(k＜i)到Pj规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集；若在预设重选起点阈值范围内的点到p′j均未规划成功，则进行终点重选，保持起点p′i不变，在终点p′j(i＜j)点之后规划成功的点集中由前至后依次重选终点，若起点p′i到在预设重选终点阈值范围内的点p′k(k＜i)规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集。更进一步地，预设重选起点阈值和预设重选终点阈值均为3次。通过路径分割和重选点策略，极大增强数据的容错能力，并且明显提升规划成功概率。
多点组合路径搜索算法：
Figure BDA0002953843600000081
其中：fA为路径搜索算法。本方案的多点组合路径搜索算法提供了迭代循环退出机制，A*算法进行路径搜索时如果迭代次数超过5次仍然无法找到路径时，退出本次搜索，减少无效的搜索时间，图4是基于栅格地图路径规划的方向和代价图示。
本算法在实践中发现经过分段规划和多点组合的方式并不需要完备的启发式算法就可以得到预期的规划路径，再综合实际应用中并无倒车的需求，所以采用了非完备式栅格的启发式路径搜索方法，如图5所示。
在本实施例，根据可变规划距离粒度分割之后获得到的规划点的方向对路径规划的结果影响较大，混合A*算法使用Dubin曲线或者Reeds-Shepp曲线使得路径更加符合车辆运动学模型，如果路径子集
Figure BDA0002953843600000091
中相邻点之间角度相差较大，会导致路径规划失败或者规划出的路径出现扭结现象。所以，在路径规划前需要对路径子集
Figure BDA0002953843600000092
中相邻规划点的角度进行平滑处理(角度平滑)，保持两个相邻的点在同一象限或者保证相邻的两个点的角度差值比较小。根据路径集合
Figure BDA0002953843600000093
通过公式αi＝arctan(Δy，Δx)依次求得各点之间的角度，其中Δy＝yi+1-yi，Δx＝xi+1-xi。在全局路径中较远的点代表了路径的运动方向和趋势，所以在进行角度平滑的时候由远及近依次进行调整。每次最多调整前方四个点之间的角度差值。图3是角度平滑的流程图，通过多次的迭代逐渐的缩小两点间的角度差值、所有角度向终点靠拢。角度平滑包括对路径子集
Figure BDA0002953843600000099
中相邻规划点的角度进行平滑处理，保持两个相邻的点在同一象限或者保证相邻的两个点的角度差值小于k，k>0；具体为：判断路径集合
Figure BDA0002953843600000094
中当前相邻两点的弧度αi-1和αi差值是否超过π/4，若π/4则按照
Figure BDA0002953843600000095
调整前一个点αi-1；判断αi-2和αi-1是否超过π/4，若超过π/4则按照
Figure BDA0002953843600000096
调整；判断αi-3和αi-2是否超过π/4，如果超过π/4则按照
Figure BDA0002953843600000097
调整其中，路径集合
Figure BDA0002953843600000098
的各点之间的角度αi＝arctan(Δy，Δx)，而Δy＝yi+1-yi，Δx＝xi+1-xi。通过依次迭代向前调整，使得相邻两点间的角度平滑可导，大大提高路径规划的成功率和正确率。角度调整算法：
Figure BDA0002953843600000101
图6是角度平滑的流程图，通过多次的迭代逐渐的缩小两点间的角度差值、所有角度向终点靠拢。(注：fabs()函数是对浮点数取绝对值)。图8的(a)-(d)分别为图7的(a)-(d)相应子图的放大图，图7的(a)是进行角度平滑处理前点的方向变化，图7的(b)是经过角度平滑处理后的角度变化，明显可以看出图7的(b)始点的角度逐步的趋向终点的角度变化，始点到终点的角度差值减少。图7的(d)相较图7的(c)也具有角度趋向终点变化的效果。
在本实施例，在实际使用中由激光雷达扫描的世界坐标经过转换后生成新地图坐标，相当于对世界坐标做了缩放，再经过混合A*算法规划出来的曲线转为世界坐标坐标后会出现很多折线，曲线是经过上文多点组合和角度平滑规划的路线、带箭头的点为经过可变规划距离粒度分割后的路径子集点。从图9的(a)-(b)中可以看到规划出来的路径并没有达到理想的平滑可行的效果，为了进一步平滑路线，本算法使用贝塞尔曲线插值对路径轨迹进行平滑，贝塞尔曲线插值公式为：
Figure BDA0002953843600000111
其中，t∈[0，1]，p为锚点或者控制点的坐标，p0为始点坐标，pn为终点坐标，pi为控制点坐标。
经过贝塞尔曲线插值平滑后的效果如图10的(a)-(b)。经过角度平滑和贝塞尔曲线平滑后车辆轨迹逐渐变得平滑，后期经过车辆的相关参数和运动学轨迹修正后，能够满足车辆的正常行进和动态避障功能。
其中：空白处为可行区域，黑色为预先下发的路径点，带方向箭头的为筛选后的路径子集点，红色为规划后的路径轨迹；图11的(a)和(e)为未经过处理的轨迹图，图11的(b)和(f)为切线平滑未经角度平滑的轨迹图，图11的(c)和(g)经角度平滑的轨迹图，图11的(d)和(h)为经过角度平滑和切线平滑的轨迹图。
在本实施例，步骤S3包括：为了稳定的输出路径，将每次规划好的路线存储在链表中，并根据当前无人车的位置，将规划好的路径按5m/40ms的周期给下发至无人车，无人车根据路线行驶。
在本实施例，无人车根据路线行驶过程中进行动态避障，具体为：
根据相关参数使用基于Hybrid-astar的动态路径规划方法周期性的计算地图的障碍，规划新的路径，相关参数为目标路径点、方向、弯道弧度、障碍物尺寸、车辆转弯半径。假设Ti规划出来的路径为Li，Ti+1时刻为Li+1，采用“分段规划，多点组合路径搜索”的搜索方法进行路径搜索，当点pi与pk存在动态的障碍物时，更新pi与pk两点之间规划出来的路线就实现动态规划和避障功能。分段规划的方法可以①减少路径搜索时间，提高扫描的频率，②实时更新变更的路径信息，实现动态的避障路线规划。
在实际应用中考虑到计算资源有限，使用了以下方法对路径规划程序进行了计算优化。
1)三角函数查表实现：混合A*算法过程中涉及到较多的三角函数相关运算，相对比较消耗CPU资源，通过查表法可以减少CPU运算量。
2)采用OpenMP多线程并发处理，减少算法过程中两点之间路径搜索的时间，保证CPU负载计算均衡。
测试结果以及分析
混合A*是基于两点的路线规划，结合运动控制学的特点规划出符合车辆运动的轨迹，输入点和输出点的方向对车的轨迹有很重要的决定作用；两点规划的缺点在于无法进行角度和运动轨迹的微调，如果只给出两点距离就无法规划弧形或者弧度比较大的路线(比如在给定路线的弯道上行驶)，用分割微分的方式，按粒度规划很好解决了该问题。
测试结果如图12(a)和(b)所示，对给定的两组路径点按本算法进行路径规划，对应的全局路径长度(最大规划距离)分别为200米、120米，分割粒度为6米，箭头表示分割后的规划点以及调整后的点的角度方向，路线为本算法规划的路径轨迹，该轨迹在避障的基础上进行了平滑插值处理，在Xavier平台上平均分别耗时1571毫秒、1011毫秒，满足实时性无人车的动态规划需求。
与现有技术相比，本发明具有如下优点：
1)、基于用户特性化的始点到终点的全局路径，按照可变规划距离进行粒度分割，分解为若干路径点子集，减少搜索的区域，从而降低地图遍历的耗时，提高效率；
2)、采用多点组合路径搜索方法进行路径搜索，提高路径搜索的成功概率；对规划后的路径子集进行多次角度平滑，解决多段子集路径集合符合运动学的轨迹要求；
3)、障碍物变化时，只需要重新规划障碍物所在的路径子集区域就更新局部路线，不需要重新对整个路径进行规划。
4)、两点规划的缺点在于无法进行角度和运动轨迹的微调，如果只给出两点距离就无法规划弧形或者弧度比较大的路线(比如在给定路线的弯道上行驶)，用分割微分的方式，按粒度规划很好解决了该问题。
5)、使用贝塞尔插值算法，将平滑度的最小距离设置为0.1m，很好地解决了地图坐标和世界坐标转换过程中规划路径不平滑问题。
6)、实现动态路径规划和避障功能。
上述具体实施方式为本发明的优选实施例，并不能对本发明进行限定，其他的任何未背离本发明的技术方案而所做的改变或其它等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于Hybrid-astar的动态路径规划方法，其特征在于，包括：
S1，根据已知的全局路径、用户设置的最大路径规划长度dmax和可变规划距离dplan进行可变规划距离的粒度分割，生成全局路径；
S2，对动态路径进行优化；其中，对动态路径进行优化的步骤包括：分段规划多点组合路径搜索、角度平滑和贝塞尔曲线差值平滑中的至少一种；
S3，将优化后的路径周期性下发至无人车，无人车根据路径行驶。
2.根据权利要求1所述的基于Hybrid-astar的动态路径规划方法，其特征在于，步骤S1包括：
根据车辆当前位置，在全局路径集合Ψ＝{p1，p2，…，pn}中查找离车辆最近的点Pnearest，将离车辆最近的点Pnearest作为规划的起始点，并加入到路径子集
Figure FDA0002953843590000011
中；
从起始点开始逐点遍历全局路径集合Ψ，计算全局路径集合Ψ中的其余点与起始点的累计欧式距离
Figure FDA0002953843590000012
当di大于dplan时，将当前点加入到路径子集中
Figure FDA0002953843590000013
循环依次遍历进行粒度分割，当路径子集累计总距离
Figure FDA0002953843590000014
大于dmax时，终止路径点的筛选，得到路径子集合
Figure FDA0002953843590000015
3.根据权利要求2所述的基于Hybrid-astar的动态路径规划万法，其特征在于，在步骤S2中，若在单次规划中起点p′i与终点p′j(i＜j)规划失败，则进行分段规划多点组合路径搜索，具体为：
保持终点p′j不变，在p′i点之前规划成功的点集中由后至前依次重选起点，若在预设重选起点阈值范围内的点p′k(k＜i)到Pj规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集；
若在预设重选起点阈值范围内的点到p′j均未规划成功，则进行终点重选，保持起点p′i不变，在终点p′j(i＜j)点之后规划成功的点集中由前至后依次重选终点，若起点p′i到在预设重选终点阈值范围内的点p′k(k＜i)规划成功，则用新的路径替换原来失败的p′k到p′i的路径，并且更新成功的点集。
4.根据权利要求3所述的基于Hybrid-astar的动态路径规划方法，其特征在于，预设重选起点阈值和预设重选终点阈值均为3次。
5.根据权利要求3所述的基于Hybrid-astar的动态路径规划方法，其特征在于，角度平滑包括对路径子集
Figure FDA0002953843590000021
中相邻规划点的角度进行平滑处理，保持两个相邻的点在同一象限或者保证相邻的两个点的角度差值小于k，k＞0；具体为：
判断路径集合
Figure FDA0002953843590000022
中当前相邻两点的弧度αi-1和αi差值是否超过预设角度阈值，若超过预设角度阈值则按照
Figure FDA0002953843590000023
调整前一个点αi-1；
判断αi-2和αi-1是否超过预设角度阈值，若超过预设角度阈值则按照
Figure FDA0002953843590000024
调整；
判断αi-3和αi-2是否超过预设角度阈值，如果超过预设角度阈值则按照
Figure FDA0002953843590000025
调整；
其中，路径集台
Figure FDA0002953843590000026
的各点之间的角度αi＝arctan(Δy，Δx)，而Δy＝yi+1-yi，Δx＝xi+1-xi。
6.根据权利要求5所述的基于Hybrid-astar的动态路径规划方法，其特征在于，预设角度阈值为π/4。
7.根据权利要求1所述的基于Hybrid-astar的动态路径规划方法，其特征在于，使用贝塞尔曲线插值对路径轨迹进行平滑，贝塞尔曲线插值公式为：
Figure FDA0002953843590000031
其中，t∈[0，1]，p为锚点或者控制点的坐标，p0为始点坐标，pn为终点坐标，pi为控制点坐标。
8.根据权利要求1所述的基于HVbrid-astar的动态路径规划方法，其特征在于，步骤S3包括：将每次规划好的路线存储在链表中，并根据当前无人车的位置，将规划好的路径按5m/40ms的周期给下发至无人车，无人车根据路线行驶。
9.根据权利要求1所述的基于Hybrid-astar的动态路径规划方法，其特征在于，无人车根据路线行驶过程中进行动态避障，具体为：
根据相关参数使用基于Hybrid-astar的动态路径规划方法周期性的计算地图的障碍，规划新的路径，相关参数为目标路径点、方向、弯道弧度、障碍物尺寸、车辆转弯半径。
10.根据权利要求9所述的基于Hybrid-astar的动态路径规划方法，其特征在于，假设Ti规划出来的路径为Li，Ti+1时刻为Li+1，当点pi与pk存在动态的障碍物时，更新pi与pk两点之间规划出来的路线就实现动态规划和避障功能。
CN202110216076.8A 2021-02-26 2021-02-26 一种基于Hybrid-astar的动态路径规划方法 Active CN112923946B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110216076.8A CN112923946B (zh) 2021-02-26 2021-02-26 一种基于Hybrid-astar的动态路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110216076.8A CN112923946B (zh) 2021-02-26 2021-02-26 一种基于Hybrid-astar的动态路径规划方法

Publications (2)

Publication Number Publication Date
CN112923946A true CN112923946A (zh) 2021-06-08
CN112923946B CN112923946B (zh) 2024-03-12

Family

ID=76172137

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110216076.8A Active CN112923946B (zh) 2021-02-26 2021-02-26 一种基于Hybrid-astar的动态路径规划方法

Country Status (1)

Country Link
CN (1) CN112923946B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113485430A (zh) * 2021-07-23 2021-10-08 中国科学院地理科学与资源研究所 一种无人机的路径规划方法和系统
CN113984062A (zh) * 2021-10-26 2022-01-28 中国科学院合肥物质科学研究院 一种基于机动性评估的地面车辆路径规划方法
CN116839616A (zh) * 2023-07-04 2023-10-03 深圳源谷科技有限公司 一种应用北斗定位技术的智能化管理系统及方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643721A (zh) * 2016-10-11 2017-05-10 北京工业大学 一种环境拓扑地图的构建方法
US20170192437A1 (en) * 2016-01-04 2017-07-06 Cruise Automation, Inc. System and method for autonomous vehicle fleet routing
CN107479572A (zh) * 2017-08-02 2017-12-15 南京理工大学 基于仿生的无人机群组实时路径规划方法
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN108520044A (zh) * 2018-04-03 2018-09-11 西安交通大学 一种基于公度性思想的最小二乘法路径预测算法
CN110095127A (zh) * 2019-04-08 2019-08-06 西北大学 一种基于分段的隐马尔可夫模型地图匹配方法
CN110220514A (zh) * 2019-05-27 2019-09-10 中国电子科技集团公司第二十八研究所 一种基于海图数据的动态航线生成方法
US10545026B1 (en) * 2017-06-05 2020-01-28 Philip Raymond Schaefer System and method for tracking the position of a person and providing navigation assistance
CN111158365A (zh) * 2019-12-31 2020-05-15 深圳优地科技有限公司 一种路径规划方法、装置、机器人及存储介质
EP3715788A1 (en) * 2019-03-29 2020-09-30 Baidu Online Network Technology (Beijing) Co., Ltd. Method, device, server and medium for determining quality of trajectory-matching data
CN112066991A (zh) * 2020-09-07 2020-12-11 常州微亿智造科技有限公司 多约束条件下智能飞行器航迹快速规划方法、装置
US20210020045A1 (en) * 2019-07-19 2021-01-21 Zoox, Inc. Unstructured vehicle path planner

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170192437A1 (en) * 2016-01-04 2017-07-06 Cruise Automation, Inc. System and method for autonomous vehicle fleet routing
CN106643721A (zh) * 2016-10-11 2017-05-10 北京工业大学 一种环境拓扑地图的构建方法
US10545026B1 (en) * 2017-06-05 2020-01-28 Philip Raymond Schaefer System and method for tracking the position of a person and providing navigation assistance
CN107479572A (zh) * 2017-08-02 2017-12-15 南京理工大学 基于仿生的无人机群组实时路径规划方法
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN108520044A (zh) * 2018-04-03 2018-09-11 西安交通大学 一种基于公度性思想的最小二乘法路径预测算法
EP3715788A1 (en) * 2019-03-29 2020-09-30 Baidu Online Network Technology (Beijing) Co., Ltd. Method, device, server and medium for determining quality of trajectory-matching data
CN110095127A (zh) * 2019-04-08 2019-08-06 西北大学 一种基于分段的隐马尔可夫模型地图匹配方法
CN110220514A (zh) * 2019-05-27 2019-09-10 中国电子科技集团公司第二十八研究所 一种基于海图数据的动态航线生成方法
US20210020045A1 (en) * 2019-07-19 2021-01-21 Zoox, Inc. Unstructured vehicle path planner
CN111158365A (zh) * 2019-12-31 2020-05-15 深圳优地科技有限公司 一种路径规划方法、装置、机器人及存储介质
CN112066991A (zh) * 2020-09-07 2020-12-11 常州微亿智造科技有限公司 多约束条件下智能飞行器航迹快速规划方法、装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
柏硌;赵刚要;: "基于MapReduce与蚁群优化的航路规划算法", 计算机工程, no. 05, pages 38 - 55 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113485430A (zh) * 2021-07-23 2021-10-08 中国科学院地理科学与资源研究所 一种无人机的路径规划方法和系统
CN113984062A (zh) * 2021-10-26 2022-01-28 中国科学院合肥物质科学研究院 一种基于机动性评估的地面车辆路径规划方法
CN113984062B (zh) * 2021-10-26 2023-11-07 中国科学院合肥物质科学研究院 一种基于机动性评估的地面车辆路径规划方法
CN116839616A (zh) * 2023-07-04 2023-10-03 深圳源谷科技有限公司 一种应用北斗定位技术的智能化管理系统及方法
CN116839616B (zh) * 2023-07-04 2024-05-07 深圳源谷科技有限公司 一种应用北斗定位技术的智能化管理系统及方法

Also Published As

Publication number Publication date
CN112923946B (zh) 2024-03-12

Similar Documents

Publication Publication Date Title
CN112923946A (zh) 一种基于Hybrid-astar的动态路径规划方法
Deo et al. Multimodal trajectory prediction conditioned on lane-graph traversals
CN113204236B (zh) 一种智能体路径跟踪控制方法
CN107390698B (zh) 扫地机器人的补扫方法及芯片
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN106647769B (zh) 基于a*提取引导点的agv路径跟踪与避障协调方法
CN110487279B (zh) 一种基于改进a＊算法的路径规划方法
CN111006667B (zh) 高速场景下的自动驾驶轨迹生成系统
CN107085437A (zh) 一种基于eb‑rrt的无人机航迹规划方法
CN109764886A (zh) 一种路径规划方法
CN111896004A (zh) 一种狭窄通道车辆轨迹规划方法及系统
CN113886764B (zh) 一种基于Frenet坐标系的智能车辆多场景轨迹规划方法
CN110967032A (zh) 一种野外环境下无人车局部行驶路线实时规划方法
CN112486178A (zh) 一种基于有向d＊算法的动态路径规划方法
CN116185014A (zh) 一种基于动态规划的智能车全局最优轨迹规划方法与系统
CN116360457A (zh) 一种基于自适应栅格和改进a*-dwa融合算法的路径规划方法
Huang et al. An online multi-lidar dynamic occupancy mapping method
CN114715192A (zh) 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统
CN113589809B (zh) 可避障的挖掘机工作装置作业轨迹规划方法及装置
CN114442618A (zh) 一种基于aco-pso-vfh的室内移动机器人自主动态路径规划方法
CN117249842A (zh) 一种基于轨迹平滑优化的无人车混合轨迹规划方法
Ma et al. An improved jump point search algorithm for home service robot path planning
CN117109620A (zh) 基于采样的车辆行为与环境交互的自动驾驶路径规划方法
CN116839609A (zh) 全覆盖路径规划方法、装置及计算机可读存储介质

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
TA01 Transfer of patent application right

Effective date of registration: 20230818

Address after: No. 88 Haiyun Road, Science City, High tech Industrial Development Zone, Guangzhou, Guangdong Province, 510145

Applicant after: Guangzhou Haige Communications Group Incorporated Company

Applicant after: Guangzhou Haige Tiancheng Technology Co.,Ltd.

Address before: 510663 No. 88 Haiyun Road, Science City, Guangzhou High-tech Industrial Development Zone, Guangdong Province

Applicant before: Guangzhou Haige Communications Group Incorporated Company

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant