CN114859917A - 非结构化道路自动驾驶路径规划方法、系统及车辆 - Google Patents

非结构化道路自动驾驶路径规划方法、系统及车辆 Download PDF

Info

Publication number
CN114859917A
CN114859917A CN202210505059.0A CN202210505059A CN114859917A CN 114859917 A CN114859917 A CN 114859917A CN 202210505059 A CN202210505059 A CN 202210505059A CN 114859917 A CN114859917 A CN 114859917A
Authority
CN
China
Prior art keywords
path
node
automatic driving
sampling
planning
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.)
Pending
Application number
CN202210505059.0A
Other languages
English (en)
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.)
Jiaxing University
Original Assignee
Jiaxing University
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 Jiaxing University filed Critical Jiaxing University
Priority to CN202210505059.0A priority Critical patent/CN114859917A/zh
Publication of CN114859917A publication Critical patent/CN114859917A/zh
Pending legal-status Critical Current

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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明涉及一种非结构化道路自动驾驶路径规划方法，包括如下步骤：A)接收路径规划输入信息；B)获取障碍物信息，利用动态步长模块提取障碍物之间空隙的中心线并处理得到离散化路径，判断处理用时是否超出许用系统时间；C)若处理用时未超出设定许用系统时间，则将离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径；D)若动态步长模块为超时退出，则判断是否存在待行驶路径以及其安全性，以决定执行待行驶路径或进行刹车；若动态步长模块为路径规划成功退出，则执行自动驾驶路径。本发明的非结构化道路自动驾驶路径规划方法规划能力强，响应速度快。此外本发明还公开了一种非结构化道路自动驾驶路径规划系统及车辆。

Description

非结构化道路自动驾驶路径规划方法、系统及车辆
技术领域
本发明涉及自动驾驶领域，具体涉及一种非结构化道路自动驾驶路径规划方法，此外还涉及一种非结构化道路自动驾驶路径规划系统以及车辆。
背景技术
自动驾驶场景分为结构化道路和非结构化道路(包括完全非结构和半结构化)。
对于结构化道路，如具有车道边线和车道中心线的高速公路，已有很多成熟的解决方法，但是在非结构化道路，如何保证方法的完备性，并快速规划出满足车辆约束的可行路径仍然存在问题。
传统路径规划有基于曲线的方法，基于采样和搜索的方法，以及基于优化的方法。曲线法有Dubins曲线、Reeds-Shepp曲线、回旋线、样条曲线等，以上算法利用曲线的几何规则，在非结构化场景下能很快找到无碰撞的可行解，但是无法保证解的完备性，导致其在环境复杂时算法的性能下降；采样法(如动态步长算法)，虽然概率完备，但其会对构型空间进行大量的无效探索，导致降低了计算效率；搜索法(如A*搜索算法及其变形算法)虽然分辨力完备，但其离散大小需要经验选取，且算法严重依赖启发项，或者是需要大量内存保存离线启发，因此，以上的自动驾驶路径规划方法均难以做兼顾计算效率和完备性。
有鉴于此，需要提供一种非结构化道路自动驾驶路径规划方法。
发明内容
本发明的第一方面是要提供一种非结构化道路自动驾驶路径规划方法，其规划能力强，响应速度快。
本发明的第二方面是要提供一种非结构化道路自动驾驶路径规划系统，其规划能力强，响应速度快。
本发明的第三方面是要提供一种车辆，其在非结构化道路上的道路规划能力强，且响应速度快，安全性高。
为实现以上发明目的，本发明第一方面提供一种非结构化道路自动驾驶路径规划方法，包括如下步骤：A)接收路径规划输入信息，所述路径规划输入信息包括当前位姿以及目标位姿的坐标信息；
B)获取障碍物信息，利用动态步长模块根据所述障碍物信息提取障碍物之间空隙的中心线并对所述中心线进行后处理，以得到离散化路径，并判断所述离散化路径的处理用时是否超出设定许用系统时间；
C)若所述处理用时未超出所述规定许用系统时间，则将所述离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径；
D)若所述动态步长模块为超时退出，则判断是否存在待行驶路径，且所述待行驶路径的长度是否超过两倍制动所需的路径长度，若不存在所述待行驶路径，或所述待行驶路径的长度不超过两倍制动所需的路径长度，则输出刹车标志位并进行刹车，若存在所述待行驶路径，且所述待行驶路径的长度超过两倍制动所需的路径长度，则执行所述待行驶路径；若所述动态步长模块为路径规划成功后退出，则执行所述自动驾驶路径。
具体地，步骤A)中的所述坐标信息包括车辆坐标x、y以及车辆航向角θ。
进一步具体地，步骤B)中所述中心线的提取以及离散化的步骤包括：
B1)将所述障碍物信息以及所述坐标信息统一坐标系，并判断是否为首次规划自动驾驶路径；
B2)若是首次规划自动驾驶路径，则根据所述障碍物信息对障碍物进行多边形图形提取，以得到所述障碍物的主要边和有限边，将所述主要边和所述有限边中的端点绑定对应的所述车辆航向角θ，对所述端点对应的车辆位姿进行碰撞检测，以删除发生碰撞的边，并根据所述当前位姿获得起始节点，根据所述目标位姿获取目标节点，再基于深度优先法对所述起始节点与所述目标节点相连接的图片进行搜索，获取完整的中心线，并将所述中心线进行离散化以得到离散化路径；
B3)若不是首次规划自动驾驶路径，则调取已有的待行驶路径，对所述待行驶路径按照设定的路径间隔，检测所述待行驶路径是否会与所述障碍物发生碰撞，若会发生碰撞，则删除发生碰撞处所对应的路径点及其后续的所述待行驶路径，并基于剩余的安全路径初始化动态步长搜索树，若不会发生碰撞，则对所述待行驶路径初始化动态步长搜索树，并对所述动态步长搜索树进行离散化。
进一步具体地，所述中心线的提取步骤包括：提取所述障碍物的角点，将临近的所述角点构成所述障碍物的障碍物线段集合，对所述线段集合进行所述多边形图形提取，以筛选出所述主要边和所述有限边，对所述主要边和所述有限边中的端点绑定对应的所述车辆航向角θ：
Figure BDA0003637139000000031
其中，(x1，y1)为所述主要边或所述有限边中的末点坐标，(x0，y0)为所述主要边或所述有限边中的起点坐标；对所述端点对应的所述车辆位姿进行碰撞检测，以删除该端点所对应的发生碰撞的边，并以距离所述当前位姿最近的所述端点作为初始节点，距离所述目标位姿最近的所述端点作为目标节点，以先进先出的深度优先原则对所述初始节点与所述目标节点相连接的图片进行搜索，得到从所述起始节点到所述目标节点的所述中心线。
进一步地，所述中心线的提取以及离散化的过程中，所述动态步长模块每经过设定的时间间隔执行一次所述步骤B2)。
进一步具体地，步骤C)中所述自动驾驶路径的获取步骤包括：
C1)对所述离散化路径基于设定的偏置采样策略进行非均匀采样，得出采样节点；
C2)筛选出到所述采样节点的路径代价最小的多个节点作为附近节点，计算所述附近节点的代价值，选取代价值最小的所述附近节点接入到动态步长搜索树，并将相应的所述附近节点到目标节点的路径加入动态步长搜索树的路径；
C3)在所述初始节点到所述附近节点的路径上，随机选取任一节点连接到所述采样节点，计算选取的节点到所述采样节点且无碰撞的节点-采样点全代价值以及选取的节点到所述采样节点再到所述附近节点且无碰撞的节点-采样点-附近节点全代价值，并将所述节点-采样点全代价值以及所述节点-采样点-附近节点全代价值与所述附近节点的代价值对比，以在小于所述附近节点的代价值时对应进行重新连接最小代价的路径，并改变相应节点的对应父子关系。
进一步具体地，所述偏置采样策略的步骤包括：从多段所述中心线中均匀概率采样随机选择一条中心线段，在所述中心线段中间均匀采样一个点：
xm＝(1-u)x0+x1
ym＝(1-u)y0+y1
其中，u在0到1之间均匀采样，随后对偏置距离和航向角偏置量利用高斯分布采样：
Figure BDA0003637139000000041
其中σ是方差参数；μ是固定为0的均值参数，所述偏置距离经所述高斯分布采样得出所述采样节点，所述航向角偏置量经所述高斯分布采样得出采样位姿。
进一步具体地，所述代价值的计算函数为：
Cost(x,y)＝Cost(σx->y)＝w1Clength+w2Ccusp+w3Ccurv
其中，w1、w2、w3是权重系数，Clength为路径长度，Ccusp为路径尖角个数，Ccurv为按照Δ路径长度间隔计算的路径曲率的平方值之和，σx->y表示从节点x到节点y的Reeds-Shepp曲线段路径。
进一步地，本发明第二方面提供一种非结构化道路自动驾驶路径规划系统，包括GPS定位单元、障碍物检测单元、数据处理单元和路径存储单元，所述GPS定位单元用于接收GPS信息，所述障碍物检测单元用于收集障碍物信息，所述路径存储单元中存储有先前规划出的自动驾驶路径，以能够将所述自动驾驶路径作为待行驶路径，所述数据处理单元中预制有上述技术方案中任一项所述的非结构化道路自动驾驶路径规划方法。
进一步地，本发明第三方面提供一种车辆，该车辆包括上述技术方案中的道路自动驾驶路径规划系统。
本发明第一方面所提供的非结构化道路自动驾驶路径规划方法，其通过判断在处理得到中心线以及离散化路径的过程所用的时间是否超出设定的需用系统时间，以能够在未超过许用系统时间时，将离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径，也即是相对应当前的路况所做出的优选自动驾驶路径；若用时超过了许用系统时间，则当动态步长模块为超时退出时，先判断是否存在先前已经规划好的待行驶路径，若有，则对待行驶路径的安全性进行评估，若是待行驶路径的长度不超过两倍制动所需的路径长度，则判定待行驶路径的不安全，则输出刹车标志位并进行刹车，若待行驶路径的长度超过两倍制动所需的路径长度，则判定待行驶路径安全，则执行待行驶路径，若不存在先前已经规划好的待行驶路径，则输出刹车标志位并进行刹车；若动态步长模块为路径规划成功后退出，则执行自动驾驶路径，该设计使得在规划路径时能够在规划时间和路径的优选解之间做出良好的平衡，从而既能够保证自动驾驶路径规划方法能够快速响应，也能够保证自动驾驶路径规划方法具有良好的规划能力，且能够保证自动驾驶的安全性。
本发明第二方面所提供的非结构化道路自动驾驶路径规划系统，由于其数据处理单元中预制有上述的非结构化道路自动驾驶路径规划方法，因此，其同样具有良好的快速响应能力以及路径规划能力。
本发明第三方面所提供的车辆，由于其具有上述的非结构化道路自动驾驶路径规划系统，因此，其同样具有良好的快速响应能力以及路径规划能力，从而在非结构化道路上行驶的安全性高。
本发明实施例的其它特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
图1是本发明的非结构化道路自动驾驶路径规划方法的原理示意图；
图2是本发明的非结构化道路自动驾驶路径规划方法的流程示意图；
图3是本发明的非结构化道路自动驾驶路径规划方法中车辆坐标以及车辆航向角的示意图；
图4是本发明的非结构化道路自动驾驶路径规划方法中对障碍物进行多边形图形提取的示意图；
图5是本发明的非结构化道路自动驾驶路径规划方法中基于障碍物多边形图形进行线路规划的示意图；
图6是本发明的非结构化道路自动驾驶路径规划方法中障碍物的主要边和有限边获取的示意图；
图7是本发明的非结构化道路自动驾驶路径规划方法中偏置采样的示意图。
具体实施方式
以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。
在本发明第一方面所提供的非结构化道路自动驾驶路径规划方法的一种实施例中，如图1所示，该非结构化道路自动驾驶路径规划方法包括如下步骤：
A)接收路径规划输入信息，具体地，路径规划输入信息包括当前位姿以及目标位姿的坐标信息；
B)获取障碍物信息，利用动态步长模块根据障碍物信息提取障碍物之间空隙的中心线并对该中心线进行后处理，得到离散化路径，并判断离散化路径的处理用时是否超出设定许用系统时间；
C)若处理用时未超出规定许用系统时间，则将离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径；
D)若动态步长模块为超时退出，则判断是否存在待行驶路径，且待行驶路径的长度是否超过两倍制动所需的路径长度，若不存在待行驶路径，或待行驶路径的长度不超过两倍制动所需的路径长度，则判定无法进行现实路径的规划，或者道路的长度不足以使得车辆进行制动减速到安全速度，安全性不足，因此，输出刹车标志位并进行刹车，若存在待行驶路径，且待行驶路径的长度超过两倍制动所需的路径长度，则判定待行驶路径具有足够的安全性，则执行该待行驶路径；若动态步长模块为路径规划成功且收敛后退出，则执行自动驾驶路径。
本发明第一方面所提供的非结构化道路自动驾驶路径规划方法，其通过判断在处理得到中心线以及离散化路径的过程所用的时间是否超出设定的需用系统时间，以能够在未超过许用系统时间时，将离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径，也即是相对应当前的路况所做出的优选自动驾驶路径；若用时超过了许用系统时间，则当动态步长模块为超时退出时，先判断是否存在先前已经规划好的待行驶路径，若有，则对待行驶路径的安全性进行评估，若是待行驶路径的长度不超过两倍制动所需的路径长度，则判定待行驶路径的不安全，则输出刹车标志位并进行刹车，若待行驶路径的长度超过两倍制动所需的路径长度，则判定待行驶路径安全，则执行待行驶路径，若不存在先前已经规划好的待行驶路径，则输出刹车标志位并进行刹车；若动态步长模块为路径规划成功后退出，则执行自动驾驶路径，该设计使得在规划路径时能够在规划时间和路径的优选解之间做出良好的平衡，从而既能够保证自动驾驶路径规划方法能够快速响应，也能够保证自动驾驶路径规划方法具有良好的规划能力，且能够保证自动驾驶的安全性。
具体地，本发明第一方面所提供的非结构化道路自动驾驶路径规划方法的一种实施例中，如图3所示，坐标信息包括车辆坐标x、y以及车辆航向角θ，以能够通过车辆坐标x、y以及车辆航向角θ再结合车辆的外轮廓尺寸，实现车辆在行驶过程中是否会与障碍物发生碰撞的判断，从而能够判断出规划的路径的安全性；此外，前位姿以及目标位姿的坐标信息可以由车辆上的GPS定位单元接收GPS信号给出。
具体地，本发明第一方面所提供的非结构化道路自动驾驶路径规划方法的一种实施例中，如图2所示，步骤B)中的中心线的提取以及离散化的步骤包括：
B1)将障碍物信息以及坐标信息统一坐标系，并判断是否为首次规划自动驾驶路径；其中，障碍物信息可以由车辆的障碍物检测单元获取，具体地，障碍物检测单元可以是设置在车辆前端或车顶的激光雷达或图片采集装置，以激光雷达为例，可将通过激光雷达获取得到的3D点云信息投影为以地面为平面的2D信息，然后如图4和图5所示，采用拟合算法拟合出障碍物的多边形外形轮廓(即障碍物边界)作为车辆路径规划的无碰撞约束输入；将障碍物信息以及坐标信息统一坐标系，具体，由于障碍物检测单元获取的障碍物的坐标是以车辆作为基准点的，因此，可将由GPS给出的坐标信息，转换为以车辆作为基准点，以使得目标位姿的坐标以及障碍物的坐标都是处在以车辆为坐标原点的坐标系上，从而能够方便后续的路径规划；
B2)若是首次规划自动驾驶路径，则根据障碍物信息对障碍物进行多边形图形提取，以得到如图6所示的障碍物的主要边和有限边，将主要边和有限边中的端点绑定对应的车辆航向角θ，对端点对应的车辆位姿进行碰撞检测，以删除发生碰撞的边，并根据当前位姿获得起始节点，根据目标位姿获取目标节点，再基于深度优先法对初始节点与目标节点相连接的图片进行搜索，获取完整的中心线，并将中心线进行离散化以得到离散化路径S0，其中，中心线的提取步骤具体包括：提取障碍物的角点，将临近的角点构成障碍物的障碍物线段集合，对线段集合进行多边形图形提取，以筛选出主要边和有限边，对主要边和有限边中的端点绑定对应的车辆航向角θ：
Figure BDA0003637139000000091
其中，(x1，y1)为主要边或有限边中的末点坐标，(x0，y0)为主要边或有限边中的起点坐标，对主要边进行筛选的目的，是确保边没有穿过障碍物，提取出的通道是有意义的；再对端点对应的车辆位姿进行碰撞检测，以删除该端点所对应的发生碰撞的边，并以距离当前位姿最近的端点作为起始节点，距离目标位姿最近的端点作为目标节点，以先进先出的深度优先原则对起始节点与目标节点相连接的图片进行搜索，得到从起始节点到目标节点的中心线。其中，筛选主要边，是为了确保障碍物的边没有穿过障碍物，以保证能够准确地提取出可供车辆通行的通道的边界，筛选有限边是为了保证障碍物的边上的端点不在无穷远处，以能够使得筛选出的障碍物的边是距离车辆自身较近的，也即是说，是要能够邮箱提取距离车辆比较近的通道边界信息，从而能够减少不必要的计算量，使得路径的规划更加迅速；
B3)若不是首次规划自动驾驶路径，则说明存储自动驾驶路径的路径存储单元中存储有先前规划好的自动驾驶路径，从而能够将其作为当前的待行驶路径，因此，可优选调取已有的待行驶路径，并对待行驶路径按照设定的路径间隔，检测待行驶路径是否会与障碍物发生碰撞，若会发生碰撞，则删除发生碰撞处所对应的路径点及其后续的待行驶路径，并将剩余的安全路径初始化动态步长搜索树，若不会发生碰撞，则对待行驶路径初始化动态步长搜索树，并对动态步长搜索树进行离散化，以得到离散化路径S0；需要注意的是，在中心线的提取以及离散化的过程中，可以根据数据处理单元的计算能力，开辟新的线程，在步骤B3)的运算处理过程中，每间隔设定的时间间隔，新的线程机会执行一次步骤B2)，也即是说，在基于已有的待行驶路径来规划自动驾驶路径时，已有的待驾驶路径可能与实际上需要规划出的自动驾驶路径根本不存在相近的轨迹，从而使得在基于已有的待行驶路径来规划自动驾驶路径时，计算量大，耗时长，因此，需要在经过一定的设定时间后，进行步骤步骤B2)，即不考虑已有的待行驶路径直接进行新的自动驾驶路径的规划，且步骤B3)与步骤B2)两者同步进行，从而取两者中最快的出离散化路径S0的一者进行下一步骤，从而能够更加快速的获得离散化路径S0，以提高自动驾驶路径的规划速度。
具体地，本发明第一方面所提供的非结构化道路自动驾驶路径规划方法的一种实施例中，如图2所示，步骤C)中自动驾驶路径的获取步骤包括：
C1)对离散化路径S0基于设定的偏置采样策略在车辆配置空间(x，y，θ)中进行非均匀采样：
p(x,y,θ)＝(1-ε)p0+εU
其中，p是非均匀动态步长节点采样的概率，U是在所有的配置空间均匀采样，p0是设定的偏置采样策略，ε是控制均匀采样强度大小的常数，偏置采样策略具体是从多段中心线中均匀概率采样随机选择一条中心线段，在中心线段中间均匀采样一个点：
xm＝(1-u)x0+x1
ym＝(1-u)y0+y1
其中，u在0到1之间均匀采样，随后对偏置距离r和航向角偏置量δ利用高斯分布采样：
Figure BDA0003637139000000111
其中σ是方差参数；μ是固定为0的均值参数，采样得到偏置距离r后即可得到(x，y)。而航向角相对线段(x0，y0)(x1，y1)的角度旋转量δ同样按照上述的公式随机选取，θ′＝θ+δ，得到如图7所示的采样位姿(x,y,θ′)；
C2)筛选出设定半径内到采样节点xrand的路径代价最小的多个节点作为附近节点xnear，对应的路径代价由函数Cost(σnear)计算，其中，σnear是采样节点xrand和附近节点xnear之间的Reeds-Shepp路径，随后，将计算的路径代价值和已有树节点的代价值相加，以得到附近节点xnear的代价cnear＝Cost(xnear)+Cost(σnear)，随后即可得到一组cnear、xnear、σnear构成的集合L，然后将集合按照cnear从小到大到排序，选取其中σnear无碰撞且全代价值cnear+CostToGo(xnear)最小的附近节点xnear加入动态步长搜索树，其中，CostToGo为：CostToGo(x)＝Cost(x，xgoal)，即是计算节点x到目标节点xgoal的代价值，节点到目标节点的路径加入到动态步长搜索树的路径，以能够选择出一条从附近节点xnear到采样节点xrand的所用的代价最小路径；
C3)步骤C2)已经对采样节点xrand尝试了选择附近节点xnear，确保了起始节点到附近节点xnear再到采样节点xrand的代价最小，但是无法保证起始节点到采样节点xrand再到附近节点xnear的代价不会更低，因此，在起始节点到附近节点xnear的路径上，随机选取节点x连接到采样节点xrand再计算CostToGo(xrand)得到Cost(x)+Cost(x，xrand)+CostToGo(xrand)以及Cost(x)+Cost(x，xrand)+Cost(xrand，xnear)+CostToGo(xnear)，其中Cost(x，y)为节点x和y之间的Reeds-Shepp距离，若新计算的两个代价值均小于σnear无碰撞的全代价值，则对应进行重新连接最小代价的路径，并改变相应节点的对应父子关系，若能找到节点x使Cost(x)+Cost(x，xrand)+CostToGo(xrand)更小，则删除采样节点xrand的原父节点关系，并将x作为采样节点xrand新的父节点；若能找到x使Cost(x)+Cost(x，xrand)+Cost(xrand，xnear)+CostToGo(xnear)更小，则将x作为采样节点xrand新的父节点，并将采样节点xrand作为附近节点xnear的父节点；其中，采用的代价函数为：
Cost(x，y)＝Cost(σx->y)＝w1Clength+w2Ccusp+w3Ccurv
其中，w1、w2、w3是权重系数，Clength为路径长度，Ccusp为路径尖角个数，Ccurv为按照Δ路径长度间隔计算的路径曲率的平方值之和，σx->y表示从节点x到节点y的Reeds-Shepp曲线段路径，经过代价值的计算与比对，最终筛选出的自动驾驶路径能够兼顾路径的行驶效率和乘客的乘坐舒适性；优选地Δ为0.5，w1＝1，w2＝4.5，w3＝0.2。
在上述的步骤C3)，实际上是进行了双向版本的树搜索，具体而言，正向的搜索树是将当前位姿作为起始点，将目标位姿作为目标点，对应的，则以目标位姿作为起始点，以当前位姿作为目标点，构建反向的搜索树，两颗搜索树交替进行生长，直到连接到一起，在连接成功过后，记录当前完整路径的代价值，作为判断代价是否收敛的依据；若后续成功采样n次且每次最小代价的变化值都小于阈值k，则判定收敛，优选地，n＝5，k＝0.3。
进一步地，本发明第二方面所提供的非结构化道路自动驾驶路径规划系统的一种实施例中，该系统包括GPS定位单元、障碍物检测单元、数据处理单元和路径存储单元，其中，GPS定位单元用于接收GPS信息，以能够获取车辆当前位姿的坐标信息以及车辆目标位姿的坐标信息；障碍物检测单元用于收集障碍物信息，具体地，障碍物检测单元可以是设置在车辆前端或车顶的激光雷达或图片采集装置，以激光雷达为例，可将通过激光雷达获取得到的3D点云信息投影为以地面为平面的2D信息，然后采用拟合算法拟合出障碍物的多边形外形轮廓作为车辆路径规划的无碰撞约束输入；路径存储单元中存储有先前规划出的自动驾驶路径，以能够将自动驾驶路径作为待行驶路径，从而在需要时能够调取出与当前的路况匹配度最高的待行驶路径作为重新规划的自动驾驶路径的基础，从而能够极大地降低自动驾驶路径的规划时间；在数据处理单元中预制有上述技术方案中的非结构化道路自动驾驶路径规划方法。
本发明第二方面所提供的非结构化道路自动驾驶路径规划系统，由于具有上述技术方案中的非结构化道路自动驾驶路径规划方法，因此，在自动驾驶路径规划时能够具有更快的响应速度，能够快速的判断出是进行刹车制动还是基于待行驶路径或是按照自动驾驶路径进行行驶，而将数据处理单元分为多个线程，并同时进行对已有的待行驶路径的分析以及对新的自动驾驶路径的规划，能够使得获得驾驶路径的用时更短，极大地提高了路径规划能力以及效率，此外，还能够规划出代价更小的路径，能够兼顾路径规划的用时以及规划出的路径的驾驶舒适度。
进一步地，本发明第三方面所提供的车辆的一种实施例中，由于该车辆包括上述技术方案中的非结构化道路自动驾驶路径规划系统，因此，同样具有上述非结构化道路自动驾驶路径规划系统的技术方案所具备的技术效果，且能够使得车辆具有更高的安全性。
以上结合附图详细描述了本发明实施例的可选实施方式，但是，本发明实施例并不限于上述实施方式中的具体细节，在本发明实施例的技术构思范围内，可以对本发明实施例的技术方案进行多种简单变型，这些简单变型均属于本发明实施例的保护范围。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施例对各种可能的组合方式不再另行说明。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (10)

1.一种非结构化道路自动驾驶路径规划方法，其特征在于，包括如下步骤：
A)接收路径规划输入信息，所述路径规划输入信息包括当前位姿以及目标位姿的坐标信息；
B)获取障碍物信息，利用动态步长模块根据所述障碍物信息提取障碍物之间空隙的中心线并对所述中心线进行后处理，以得到离散化路径，并判断所述离散化路径的处理用时是否超出设定许用系统时间；
C)若所述处理用时未超出所述规定许用系统时间，则将所述离散化路径基于动态步长进行非均匀采样路径规划，得到自动驾驶路径；
D)若所述动态步长模块为超时退出，则判断是否存在待行驶路径，且所述待行驶路径的长度是否超过两倍制动所需的路径长度，若不存在所述待行驶路径，或所述待行驶路径的长度不超过两倍制动所需的路径长度，则输出刹车标志位并进行刹车，若存在所述待行驶路径，且所述待行驶路径的长度超过两倍制动所需的路径长度，则执行所述待行驶路径；若所述动态步长模块为路径规划成功后退出，则执行所述自动驾驶路径。
2.根据权利要求1所述的非结构化道路自动驾驶路径规划方法，其特征在于，步骤A)中的所述坐标信息包括车辆坐标x、y以及车辆航向角θ。
3.根据权利要求2所述的非结构化道路自动驾驶路径规划方法，其特征在于，步骤B)中所述中心线的提取以及离散化的步骤包括：
B1)将所述障碍物信息以及所述坐标信息统一坐标系，并判断是否为首次规划自动驾驶路径；
B2)若是首次规划自动驾驶路径，则根据所述障碍物信息对障碍物进行多边形图形提取，以得到所述障碍物的主要边和有限边，将所述主要边和所述有限边中的端点绑定对应的所述车辆航向角θ，对所述端点对应的车辆位姿进行碰撞检测，以删除发生碰撞的边，并根据所述当前位姿获得初始节点，根据所述目标位姿获取目标节点，再基于深度优先法对所述初始节点与所述目标节点相连接的图片进行搜索，获取完整的中心线，并将所述中心线进行离散化以得到离散化路径；
B3)若不是首次规划自动驾驶路径，则调取已有的待行驶路径，对所述待行驶路径按照设定的路径间隔，检测所述待行驶路径是否会与所述障碍物发生碰撞，若会发生碰撞，则删除发生碰撞处所对应的路径点及其后续的所述待行驶路径，并基于剩余的安全路径初始化动态步长搜索树，若不会发生碰撞，则对所述待行驶路径初始化动态步长搜索树，并对所述动态步长搜索树进行离散化。
4.根据权利要求3所述的非结构化道路自动驾驶路径规划方法，其特征在于，所述中心线的提取步骤包括：提取所述障碍物的角点，将临近的所述角点构成所述障碍物的障碍物线段集合，对所述线段集合进行所述多边形图形提取，以筛选出所述主要边和所述有限边，对所述主要边和所述有限边中的端点绑定对应的所述车辆航向角θ：
Figure FDA0003637138990000021
其中，(x1，y1)为所述主要边或所述有限边中的末点坐标，(x0，y0)为所述主要边或所述有限边中的起点坐标；对所述端点对应的所述车辆位姿进行碰撞检测，以删除该端点所对应的发生碰撞的边，并以距离所述当前位姿最近的所述端点作为起始节点，距离所述目标位姿最近的所述端点作为目标节点，以先进先出的深度优先原则对所述起始节点与所述目标节点相连接的图片进行搜索，得到从所述起始节点到所述目标节点的所述中心线。
5.根据权利要求3所述的非结构化道路自动驾驶路径规划方法，其特征在于，所述中心线的提取以及离散化的过程中，所述动态步长模块每经过设定的时间间隔执行一次所述步骤B2)。
6.根据权利要求3至5中任一项所述的非结构化道路自动驾驶路径规划方法，其特征在于，步骤C)中所述自动驾驶路径的获取步骤包括：
C1)对所述离散化路径基于设定的偏置采样策略进行非均匀采样，得出采样节点；
C2)筛选出到所述采样节点的路径代价最小的多个节点作为附近节点，计算所述附近节点的代价值，选取代价值最小的所述附近节点接入到动态步长搜索树，并将相应的所述附近节点到目标节点的路径加入动态步长搜索树的路径；
C3)在所述初始节点到所述附近节点的路径上，随机选取任一节点连接到所述采样节点，计算选取的节点到所述采样节点且无碰撞的节点-采样点全代价值以及选取的节点到所述采样节点再到所述附近节点且无碰撞的节点-采样点-附近节点全代价值，并将所述节点-采样点全代价值以及所述节点-采样点-附近节点全代价值与所述附近节点的代价值对比，以在小于所述附近节点的代价值时对应进行重新连接最小代价的路径，并改变相应节点的对应父子关系。
7.根据权利要求6所述的非结构化道路自动驾驶路径规划方法，其特征在于，所述偏置采样策略的步骤包括：从多段所述中心线中均匀概率采样随机选择一条中心线段，在所述中心线段中间均匀采样一个点：
xm＝(1-u)x0+x1
ym＝(1-u)y0+y1
其中，u在0到1之间均匀采样，随后对偏置距离和航向角偏置量利用高斯分布采样：
Figure FDA0003637138990000041
其中σ是方差参数；μ是固定为0的均值参数，所述偏置距离经所述高斯分布采样得出所述采样节点，所述航向角偏置量经所述高斯分布采样得出采样位姿。
8.根据权利要求6所述的非结构化道路自动驾驶路径规划方法，其特征在于，所述代价值的计算函数为：
Cost(x,y)＝Cost(σx->y)＝w1Clength+w2Ccusp+w3Ccurv
其中，w1、w2、w3是权重系数，Clength为路径长度，Ccusp为路径尖角个数，Ccurv为按照Δ路径长度间隔计算的路径曲率的平方值之和，σx->y表示从节点x到节点y的Reeds-Shepp曲线段路径。
9.一种非结构化道路自动驾驶路径规划系统，其特征在于，包括GPS定位单元、障碍物检测单元、数据处理单元和路径存储单元，所述GPS定位单元用于接收GPS信息，所述障碍物检测单元用于收集障碍物信息，所述路径存储单元中存储有先前规划出的自动驾驶路径，以能够将所述自动驾驶路径作为待行驶路径，所述数据处理单元中预制有权利要求1至8中任一项所述的非结构化道路自动驾驶路径规划方法。
10.一种车辆，其特征在于，包括权利要求9所述的道路自动驾驶路径规划系统。
CN202210505059.0A 2022-05-10 2022-05-10 非结构化道路自动驾驶路径规划方法、系统及车辆 Pending CN114859917A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210505059.0A CN114859917A (zh) 2022-05-10 2022-05-10 非结构化道路自动驾驶路径规划方法、系统及车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210505059.0A CN114859917A (zh) 2022-05-10 2022-05-10 非结构化道路自动驾驶路径规划方法、系统及车辆

Publications (1)

Publication Number Publication Date
CN114859917A true CN114859917A (zh) 2022-08-05

Family

ID=82638101

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210505059.0A Pending CN114859917A (zh) 2022-05-10 2022-05-10 非结构化道路自动驾驶路径规划方法、系统及车辆

Country Status (1)

Country Link
CN (1) CN114859917A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115840454A (zh) * 2023-02-20 2023-03-24 江苏集萃清联智控科技有限公司 非结构化道路冲突区域的多车轨迹协同规划方法及装置

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115840454A (zh) * 2023-02-20 2023-03-24 江苏集萃清联智控科技有限公司 非结构化道路冲突区域的多车轨迹协同规划方法及装置

Similar Documents

Publication Publication Date Title
CN111679678B (zh) 一种横纵向分离的轨迹规划方法、系统及计算机设备
CN110632617B (zh) 一种激光雷达点云数据处理的方法及装置
KR101683984B1 (ko) 라이더 데이터 필터링 시스템 및 그 방법
CN111537994B (zh) 一种无人矿卡障碍物检测方法
CN114812581A (zh) 一种基于多传感器融合的越野环境导航方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN112444263B (zh) 全局路径规划方法及装置
CN110361028B (zh) 一种基于自动驾驶循迹的路径规划结果生成方法及系统
JP5941091B2 (ja) 車線ベースの位置特定
CN113188562B (zh) 可行驶区域的路径规划方法、装置、电子设备及存储介质
EP3846074A1 (en) Predicting future events in self driving cars
CN114020015A (zh) 基于障碍物地图双向搜索的无人机路径规划系统及方法
CN114859917A (zh) 非结构化道路自动驾驶路径规划方法、系统及车辆
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN112660128A (zh) 用于确定自动驾驶车辆的车道变更路径的设备及其方法
US20220343637A1 (en) Traffic flow machine-learning modeling system and method applied to vehicles
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
CN116311127A (zh) 一种道路边界检测方法、计算机设备、可读存储介质及机动车
CN114537447A (zh) 安全通行方法、装置、电子设备和存储介质
US20230394694A1 (en) Methods and apparatus for depth estimation using stereo cameras in a vehicle system
CN114407919B (zh) 一种基于自动驾驶的碰撞检测方法及系统
CN111650934A (zh) 自动驾驶系统局部路径规划的方法
US20220402489A1 (en) Determining a Discrete Representation of a Roadway Section in Front of a Vehicle
US20220413147A1 (en) Method for detecting ground using lidar sensor and ground detection device performing same
CN117272690B (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