CN112612266A - 一种非结构化道路全局路径规划方法与系统 - Google Patents

一种非结构化道路全局路径规划方法与系统 Download PDF

Info

Publication number
CN112612266A
CN112612266A CN202011406640.4A CN202011406640A CN112612266A CN 112612266 A CN112612266 A CN 112612266A CN 202011406640 A CN202011406640 A CN 202011406640A CN 112612266 A CN112612266 A CN 112612266A
Authority
CN
China
Prior art keywords
node
information
vehicle
path
pose
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
CN202011406640.4A
Other languages
English (en)
Other versions
CN112612266B (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.)
Hunan University
Original Assignee
Hunan 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 Hunan University filed Critical Hunan University
Priority to CN202011406640.4A priority Critical patent/CN112612266B/zh
Publication of CN112612266A publication Critical patent/CN112612266A/zh
Application granted granted Critical
Publication of CN112612266B publication Critical patent/CN112612266B/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
    • 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

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)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种非结构化道路全局路径规划方法，通过在A*算法子节点拓展时考虑车辆的运动学模型，使得搜索得到的路径满足车辆的非完整约束要求。其次，通过在同一个离散前轮转角处向前拓展多步，得到多层候选子节点，从而实现在一次节点拓展时完成多步长跨越，大大加快了路径搜索的效率。同时对每个候选子节点进行碰撞检测，保证即使是大步长的跨越也能满足车辆安全性需求。最后，对搜索得到的路径进行圆弧插值处理，保证了路点的均匀性。本发明的有益效果在于：对A星算法的子节点拓展方式作出改进，能够在非结构化道路中快速的搜索出一条从起始位姿到达目标位姿的平滑、安全、高效的全局路径。

Description

一种非结构化道路全局路径规划方法与系统
技术领域
本发明涉及自动驾驶领域，尤其是一种非结构化道路全局路径规划方法与系统。
背景技术
自动驾驶车辆的发展与推广在提高车辆通行效率、保障行车安全、建设智慧交通体系等方面具有重大意义。近几年来，对自动驾驶车辆相关技术的研究成为各个领域专家学者关注的焦点。路径规划作为自动驾驶车辆最重要的关键技术之一，其主要任务是：给定自动驾驶车辆起始点和目标点后，在一定的环境模型基础上按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。根据不同场景，自动驾驶车辆的路径规划可分为结构化道路的路径规划和非结构化道路的路径规划。其中，非结构化道路由于环境信息的复杂性，其路径规划方法一直是自动驾驶领域的重要研究内容。
目前，针对非结构化道路场景的路径规划方法主要可分为基于采样的方法和基于图搜索的方法两大类。基于采样的方法主要有RRT算法及其衍生算法，此类方法虽然具有很高效率，但其搜索得到的路径较为随机，无法满足稳定性需求。基于图搜索的方法主要有Dijkstra算法、A*算法等。Dijkstra算法是一种具有代表性的最短路径规划算法，其以起始点为中心向外层层拓展，直到扩展到终点为止，但该算法计算效率不高，无法满足车辆路径规划的实时性需求。A*算法是在Dijkstra算法的基础上引入了启发函数，在保证最优性的同时，可大大减少节点拓展数量从而提高路径搜索效率，因而成为非结构化道路路径规划中最常用的方法。
如专利文件CN 110220528 A提出分别从起始点和目标点同时使用A*算法进行搜索，直到搜索到相同节点时终止。其虽然提高了搜索效率，但未考虑车辆的非完整约束特性，无法满足自动驾驶车辆路径规划需求；专利文件CN110333659A中提出在A*算法的启发函数中引入安全代价估计，使得规划出的路径远离障碍物。其虽然保证了路径安全性，但同样的，该方法并未考虑车辆的非完整约束特性。专利文件CN 108444488 A中，在节点拓展的过程中考虑了车辆的非完整约束，并根据栅格精度、车辆行驶速度和局部路径更新时间来确定搜索步长，但使用该方法无法精确到达目标位置和方向，且搜索步长的选择依赖于车辆行驶速度，故只适合于车辆的实时局部路径规划。专利文件CN110487290A提出的变步长A*搜索局部路径规划方法中，其根据拓展点与障碍物之间的距离自适应的从事先设置好的模板中得到不同的搜索步长。该方法虽然在一定程度上降低了节点拓展数量，但因为步长不固定，导致搜索得到的路点距离不均匀，从而使得路径后处理变得十分困难，并且获取拓展点与障碍物之间的距离的过程也会影响路径搜索效率。
综上，现有的基于A*算法的非结构化道路路径规划方法都无法做到：既能满足车辆的非完整约束特性要求，又能在保证车辆安全性的同时加快路径搜索速度，同时保证路点的均匀性。
发明内容
针对现有技术存在的不足，本发明的目的在于提供一种非结构化道路全局路径规划方法与系统，通过改进A*算法的节点拓展方式，同时对搜索得到的路径进行圆弧插值，以克服或减轻现有技术的上述缺陷。
为实现上述目的，本发明提供了如下技术方案：一种非结构化道路全局路径规划方法，包括如下步骤：
步骤S1，输入车辆起始位姿、目标位姿和全局地图信息，设置搜索空间的栅格位置分辨率ξ和角度分辨率γ，并根据栅格位置分辨率ξ对地图进行栅格化处理，其中，起始位姿信息和目标位姿信息包括车辆的后轴中心坐标信息(x,y)和车辆横摆角信息
Figure BDA0002818709060000031
步骤S2，设置评估函数F＝G+H，并通过预计算得到H值查询表，预计算的方式为选取广度优先算法、Dijkstra算法或动态规划算法的其中一个，在二维栅格地图中计算得到从每个离散栅格位置到达目标栅格位置的最近距离作为启发值H；步骤S3，初始化open集与close集，将起始节点放入open集中，该起始节点包含车辆位姿信息
Figure BDA0002818709060000032
车辆挡位信息d、所在位置栅格信息
Figure BDA0002818709060000033
所在角度栅格信息
Figure BDA0002818709060000034
值、F值和父节点信息，其中的车辆档位信息d包括车辆的前进、倒退信息；所述的位置栅格信息
Figure BDA0002818709060000035
和角度栅格信息
Figure BDA0002818709060000036
通过以下各式计算得到：
Figure BDA0002818709060000037
Figure BDA0002818709060000038
其中，
Figure BDA0002818709060000039
o为选取的坐标原点；
步骤S4，从open集中选出F值最小的节点作为当前节点，并将该节点从open集中移除，加入到close集中，若open集为空，则规划失败，即无法找到从起始位姿到达目标位姿的可行路径；
步骤S5，判断当前点是否到达终点附近，若是，则采用曲线拟合方法精确拟合得到从当前位姿到目标位姿的路径；若否，则执行步骤S7；
步骤S6，对曲线拟合得到的路径中的各个离散路径点进行碰撞检测，若所有的离散路径点位姿都通过碰撞检测，则搜索结束，执行步骤S12，否则执行步骤S7
步骤S7，以当前节点作为父节点，其位姿信息与车辆挡位信息可表述为
Figure BDA0002818709060000041
根据车辆运动学模型分别向前和向后进行节点拓展，得到若干个待处理子节点；
步骤S8，选择一个子节点，判断车辆在该子节点
Figure BDA0002818709060000042
处是否与障碍物发生碰撞，若无碰撞则计算其所在位置栅格信息与所在角度栅格信息，并计算从起始节点到该子节点的累计代价Gi，然后根据该子节点所在位置栅格信息在H值查询表中查得该子节点的启发值Hi，继而计算出Fi；
步骤S9，判断选择的子节点是否已经存在于close集当中，若是，则放弃该节点，返回步骤S8，其中判断方法为：计算得到当前节点的所在位置栅格信息、角度栅格信息和车辆挡位信息，遍历close集判断其中是否存在具有同样上述信息的节点；
步骤S10，判断选择的子节点是否已经存在于open集当中，若是，则进行累计代价比较与节点更新处理；若否，则将该节点加入到open集当中；
步骤S11，以步骤S4所述的当前节点位姿作为起点，根据每个节点所存储的父节点信息进行路径回溯，得到搜索出的初始路径；
步骤S12，对搜索出的初始路径进行圆弧插值处理，得到一条路点间隔均匀的路径，并结合曲线拟合路径，最终得到从起始位姿到达目标位姿的全局路径。作为本发明的进一步改进，所述步骤S10中的累计代价Gi比较与节点更新处理的具体方法为：判断存在于open集中的节点No的累计代价值Go与当前节点Ni的累计代价值Gi的大小，若Go>Gi，则将open集中存放的节点No的位姿信息替换为节点Ni的位姿信息，车辆挡位信息do替换为di，然后将累计代价值Go替换为Gi，最后将父节点信息替换为节点Ni的父节点信息；若Go<Gi，则不进行任何处理，继续执行步骤S8，直到所有子节点处理完毕后返回步骤S4。
作为本发明的进一步改进，所述步骤S7根据车辆运动学模型分别向前和向后进行节点拓展具体包括以下步骤：
S7-1，设置车辆的轴距Wb、最大前轮转向角θmax；
S7-2，确定前轮转角离散数量N，将前轮转角范围[-θmax,θmax]进行均匀离散，得到每个离散位置的前轮转向角计算公式为：
θi＝-θmax+i·Δθ
其中，i＝0,1,…,N-1；Δθ为相邻转角差，计算公式为：
Figure BDA0002818709060000051
S7-3，确定从每个离散角度处向前或向后拓展的子节点数量M。当向后拓展时，在该离散角度处的拓展子节点数量M固定为1；当向前拓展时，在该离散角度处的拓展子节点数量M由下式计算得到：
Figure BDA0002818709060000052
其中，i与步骤S7-2中式(1)中的i一致；N为前轮转角离散数量，其必须为一个奇数，可根据实际需要和角度离散分辨率γ合理选择；
S7-4，从父节点
Figure BDA0002818709060000053
开始，在每个离散前轮转角处根据车辆运动学模型生成M个子节点
Figure BDA0002818709060000054
Figure BDA0002818709060000055
Figure BDA0002818709060000056
Figure BDA0002818709060000057
Figure BDA0002818709060000058
Figure BDA0002818709060000061
Figure BDA0002818709060000062
其中，k＝1,2,…,M，即当k取不同数值时，在同一个离散前轮转角处可拓展得到多个子节点；L为拓展步长；Wb为车辆轴距；R为车辆的转弯半径；f为正负号标志，向前拓展时f取1，向后拓展时f取-1；
S7-5，将所有拓展得到的子节点的父节点都设为同一点，即步骤S7-4中的Np。作为本发明的进一步改进，所述步骤S8中，累计代价Gi的具体计算公式如下：
Figure BDA0002818709060000063
Figure BDA0002818709060000064
Figure BDA0002818709060000065
Figure BDA0002818709060000066
其中，Gp为当前子节点的父节点累计代价；α1、α2、α3为权重系数；
Figure BDA0002818709060000067
表示从父节点到当前点的步长代价，
Figure BDA0002818709060000068
表示从父节点到当前点的角度变化代价，
Figure BDA0002818709060000069
表示从父节点到当前点的方向切换代价。
作为本发明的进一步改进，所述步骤S12中，对搜索得到的初始路径进行圆弧插值处理具体包括以下步骤：
S12-1，从起点开始，依次计算相邻两点之间的距离D，若该距离D与搜索步长L近似，则不需要进行插值；若距离D明显大于步长L，则执行下一步骤；
S12-2，根据几何关系，可推导得出相邻两点的转弯半径R，具体计算方法如下：
Figure BDA00028187090600000610
其中，
Figure BDA00028187090600000611
为相邻两点的横摆角差值；
S12-3，选取不同的k值，从而得到相应的β值，之后获得各插值点的位置信息和横摆角信息；
S12-4，当计算得到的插值点与原路径点距离过近时，则放弃该插值点，并停止在该相邻两点的插值操作，继续执行步骤S12-1，直到所有点之间插值完后结束。作为本发明的进一步改进，所述步骤S12中，得到的相邻路点之间的插值点所在位置即对应通过步骤S7拓展得到的该相邻路点之间的其他子节点的位置。
本发明另一方面提供了一种系统，包括车载环境感知单元、决策规划单元、底层控制单元，所述决策规划单元内搭载有执行上述方法的程序。
本发明的有益效果，本发明提出的非结构化道路全局路径规划方法，通过改进A*算法的节点拓展方式，在节点拓展时考虑车辆的运动学模型，使得搜索得到的路径满足车辆的非完整约束要求。其次，通过在同一个离散前轮转角处向前拓展多步，得到多个候选子节点，从而实现在一次节点拓展时完成多步长跨越，大大加快了路径搜索的效率。同时对每个候选子节点都进行了碰撞检测，能够保证即使是大步长的跨越也能满足车辆安全性需求。最后，对搜索得到的路径进行了圆弧插值处理，保证了路点的均匀性，以便于路径的后处理。总之，该方法能够在非结构化道路中快速的搜索出一条从起始位姿到达目标位姿的平滑、安全、高效的全局路径。
附图说明
图1是本发明一种非结构化道路全局路径规划方法的流程图；
图2是本发明使用的车辆运动学模型示意图；
图3是本发明改进的节点拓展方式示意图；
图4是本发明搜索得到路点之间几何关系示意图；
图5是本发明路径点圆弧插值示意图；
图6是本发明非结构化道路全局路径规划效果图；
图7是本发明非结构化道路全局路径规划系统示意图。
具体实施方式
下面将结合附图所给出的实施例对本发明做进一步的详述。
参照图1所示，本实施例的一种非结构化道路全局路径规划方法，其流程如图1所示，具体包括以下步骤：
S1，输入车辆起始位姿、目标位姿和全局地图信息，设置搜索空间的栅格位置分辨率ξ和角度分辨率γ，并根据栅格位置分辨率ξ对地图进行栅格化处理。所述的车辆起始位姿和目标位姿包括车辆的后轴中心坐标信息(x,y)和车辆横摆角信息
Figure BDA0002818709060000081
S2，设置评估函数F＝G+H，并预计算得到H值查询表。使用的预计算方法根据需要可选取广度优先算法、Dijkstra算法或动态规划算法的其中一个，本实施例中采用的是Dijkstra算法。通过计算，可得到地图内从所有离散栅格点到达目标栅格点的最小距离，以此作为本方法中的启发值H。值得注意的是，该计算过程应该在步骤S1中构建的栅格化地图中进行，且计算起始点应选择为步骤S1中目标位姿对应的栅格位置点。使用此方法计算得到的启发值H相较于使用简单的欧氏距离作为启发值H，能有效的避免搜索到死胡同区域；
S3，初始化open集与close集，将起始节点放入open集中。所述节点包含位姿信息
Figure BDA0002818709060000082
车辆挡位信息d、所在位置栅格信息
Figure BDA0002818709060000083
所在角度栅格信息
Figure BDA0002818709060000084
值、F值和父节点信息。车辆挡位信息指的是前进、倒退信息，前进设为1，倒退设为0，所在位置栅格信息
Figure BDA0002818709060000085
和角度栅格信息
Figure BDA0002818709060000086
通过以下各式计算得到：
Figure BDA0002818709060000087
Figure BDA0002818709060000091
其中，
Figure BDA0002818709060000092
o为选取的坐标原点。
特别地，起始点的挡位信息默认为前进档，即设为1；G值设为0，H值则根据位置栅格信息在H值查询表中查询得到，继而算得F值；父节点设为自身；
S4，从open集中选出F值最小的节点作为当前节点，并将该节点信息从open集中移除，加入到close集中。若open集为空，则规划失败，即无法找到从起始位姿到达目标位姿的可行路径；
S5，判断当前点是否到达终点附近，即判断当前点与目标点的距离是否小于一定阈值。若是，则采用曲线拟合方法精确拟合得到从当前位姿到目标位姿的路径；若否，则执行步骤S7。所述的曲线拟合方法可根据需要可选择RS曲线拟合、Dubins曲线拟合等方法。具体的，若需要考虑倒退，则使用RS曲线拟合方法，否则可使用Dubins曲线拟合方法。本本实施例中使用的是RS曲线拟合方法；
S6，对曲线拟合得到的路径中的各个离散路径点进行碰撞检测。若所有的节点位姿都通过碰撞检测，则搜索结束，执行步骤S12，否则执行步骤S7；
S7，以当前节点作为父节点，其位姿信息与车辆挡位信息可表述为
Figure BDA0002818709060000093
根据车辆运动学模型分别向前和向后进行节点拓展，得到若干个待处理子节点；
S8，选择一个子节点，判断车辆在该子节点
Figure BDA0002818709060000094
处是否与障碍物发生碰撞。若无碰撞则根据公式(1)和公式(2)计算其所在位置栅格信息与所在角度栅格信息，并计算从起始节点到该子节点的累计代价Gi，然后根据该子节点所在位置栅格信息在H值查询表中查得该子节点的启发值Hi，继而计算出Fi。若车辆在该子节点位姿处与障碍物发生碰撞，则意味着沿着该子节点对应前轮转角处将继续向前拓展是不安全的，故将该子节点对应离散前轮转角处的其他待处理的子节点舍弃。累计代价Gi的具体计算公式如下：
Figure BDA0002818709060000101
其中，Gp为当前子节点的父节点的累计代价；α1、α2、α3为权重系数；
Figure BDA0002818709060000102
表示从父节点到当前点的步长代价，
Figure BDA0002818709060000103
表示从父节点到当前点的角度变化代价，
Figure BDA0002818709060000104
表示从父节点到当前点的方向切换代价。
通过
Figure BDA0002818709060000105
一方面考虑了节点拓展时的步长代价，另一方面对后退的搜索进行了惩罚；通过
Figure BDA0002818709060000106
使得车辆尽量少的改变运动方向，保证了路径的平滑性；通过
Figure BDA0002818709060000107
有效避免了搜索得到的路径出现频繁的前进后退切换现象；
S9，判断该节点是否已经存在于close集当中。若是，则放弃该节点，返回步骤8。所述的判断方法为：计算得到当前节点所在位置栅格信息、角度栅格信息和车辆挡位信息，遍历close集判断其中是否存在具有同样上述信息的节点；
S10，判断该节点是否已经存在于open集当中。若是，则进行累计代价比较与节点更新处理；若否，则将该节点加入到open集当中。所述的判断方法与步骤S9中的一致；所述的累计代价Gi比较与节点更新处理的具体方法为：判断存在于open集中的节点No的累计代价值Go与当前节点Ni的累计代价值Gi的大小。若Go>Gi，则将open集中存放的节点No的位姿信息替换为节点Ni的位姿信息，车辆挡位信息do替换为di，然后将累计代价值Go替换为Gi，最后将父节点信息替换为节点Ni的父节点信息；若Go<Gi，则不进行任何处理，继续执行步骤S8，直到所有子节点处理完毕后返回步骤S4；
S11，以步骤S4所述的当前节点位姿作为起点，根据每个节点所存储的父节点信息进行路径回溯，得到搜索出的初始路径；
S12，为保证路点均匀性，对搜索出的初始路径进行圆弧插值处理，得到一条路点间隔均匀的路径，并结合曲线拟合路径，最终得到从起始位姿到达目标位姿的全局路径。
结合附图2，本实施例中，步骤S7根据车辆运动学模型生成离散子节点的过程为：
S7-1，设置车辆的轴距Wb、最大前轮转向角θmax；
S7-2，确定前轮转角离散数量N，将前轮转角范围[-θmax,θmax]进行均匀离散，得到每个离散位置的前轮转向角计算公式为：
θi＝-θmax+i·Δθ (3)
其中，i＝0,1,…,N-1；Δθ为相邻转角差，计算公式为：
Figure BDA0002818709060000111
值得注意的是，θi的正负决定了车辆的转向，当θi<0时，表示车辆左转，当θi>0时，表示车辆右转；
S7-3，确定从每个离散角度处向前或向后拓展的子节点数量M。当向后拓展时，在该离散角度处的拓展子节点数量M固定为1；当向前拓展时，在该离散角度处的拓展子节点数量M由下式计算得到：
Figure BDA0002818709060000121
其中，i与步骤S7-2中式(1)中的i一致；N为前轮转角离散数量，其必须为一个奇数，可根据实际需要和角度离散分辨率γ合理选择。
S7-4，从父节点位姿
Figure BDA0002818709060000122
开始，在每个离散前轮转角处根据车辆运动学模型生成M个子节点
Figure BDA0002818709060000123
Figure BDA0002818709060000124
Figure BDA0002818709060000125
Figure BDA0002818709060000126
Figure BDA0002818709060000127
Figure BDA0002818709060000128
Figure BDA0002818709060000129
其中，k＝1,2,…,M，即当k取不同数值时，在同一个离散前轮转角处可拓展得到多个子节点；L为拓展步长；Wb为车辆轴距；R为车辆的转弯半径；f为正负号标志，向前拓展时f取1，向后拓展时f取-1。
特别需要指出的是，当离散的前轮转向角θi＝0时，通过上式计算得到车辆转弯半径为无穷大，公式(8)和公式(9)此时不再使用，子节点的位置和横摆角可通过另外简单的几何关系计算得到，这里不作详细阐述。另外，选择的拓展步长L应满足以下关系：
Figure BDA00028187090600001210
以保证相邻两个节点不会落到同一栅格内；
S7-5，将所有拓展得到的子节点的父节点都设为同一点，即步骤S7-4中的Np。
本实施例中，设置前轮转角离散数量N＝11，拓展步长L＝2，拓展得到的子节点分布情况如图3所示。图中的每个黑实心点都表示一个子节点位置。从图中可以看出，本方法在当前所设参数下一次最多能向前拓展12m。
结合附图4和附图5，本实施例中，对搜索得到的初始路径进行圆弧插值处理包括以下步骤：
S12-1，从起点开始，依次计算相邻两点之间的距离D。若该距离D与搜索步长L近似，则不需要进行插值；若距离D明显大于步长L，则执行下一步骤；
S12-2，如图4所示，根据几何关系，可推导得出相邻两点的转弯半径R，具体计算方法如下：
Figure BDA0002818709060000131
其中，
Figure BDA0002818709060000132
为相邻两点的横摆角差值。
S12-3，利用公式(7)，从小到大依次选取不同的k值，从而得到相应的β值，再利用公式(8)～(10)即可获得各插值点的位置信息和横摆角信息，如图5所示。
S12-4，当计算得到的插值点与原路径点距离过近时，则放弃该插值点，并停止在该相邻两点的插值操作，继续执行步骤S12-1，直到所有点之间插值完后结束。
步骤S12得到的相邻路点之间的插值点所在位置就是通过步骤S7拓展得到的该相邻路点之间的其他子节点的位置，即所述插值过程就是找到两路点之间其他未加入close集子节点的过程。
如图6所示，为使用本方法在非结构化道路上进行全局路径搜索的效果图。该全局地图大小为120m*100m，栅格分辨率ξ为1m，角度分辨率γ为5度。图中的1表示车辆的起始位姿，2表示车辆的目标位姿，3表示本方法搜索并插值得到的路径，4表示采用曲线拟合方法得到的路径，5表示本方法拓展子节点过程生成的搜索树，6表示障碍物。从图中可以看出，本方法既能能在向前直线拓展时表现出极高效率，又能保证车辆在需要转弯时得到合适的转弯角度。同时，合理设置的启发函数H与代价函数G保证了搜索得到的路径的平滑性、安全性与高效性。
此外，本发明还提供一种非结构化道路全局路径规划系统。如图7所示，该系统包括车载环境感知单元、决策规划单元的底层控制单元。其中，环境感知单元用于获取环境信息，经处理后得到全局地图信息；决策规划单元则使用本发明中的方法进行全局路径规划，得到的全局路径经后处理后传给底层控制单元；底层控制单元则负责控制车辆沿着当前路径行驶。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种非结构化道路全局路径规划方法，其特征在于：包括如下步骤：
步骤S1，输入车辆起始位姿、目标位姿和全局地图信息，设置搜索空间的栅格位置分辨率ξ和角度分辨率γ，并根据栅格位置分辨率ξ对地图进行栅格化处理，其中，起始位姿信息和目标位姿信息包括车辆的后轴中心坐标信息(x,y)和车辆横摆角信息
Figure FDA0002818709050000011
步骤S2，设置评估函数F＝G+H，并通过预计算得到H值查询表，预计算的方式为选取广度优先算法、Dijkstra算法或动态规划算法的其中一个，在二维栅格地图中计算得到从每个离散栅格位置到达目标栅格位置的最近距离作为启发值H；
步骤S3，初始化open集与close集，将起始节点放入open集中，该起始节点包含车辆位姿信息
Figure FDA0002818709050000012
车辆挡位信息d、所在位置栅格信息
Figure FDA0002818709050000013
所在角度栅格信息
Figure FDA0002818709050000014
G值、F值和父节点信息，其中的车辆档位信息d包括车辆的前进、倒退信息；所述的位置栅格信息
Figure FDA0002818709050000015
和角度栅格信息
Figure FDA0002818709050000016
通过以下各式计算得到：
Figure FDA0002818709050000017
Figure FDA0002818709050000018
其中，
Figure FDA0002818709050000019
o为选取的坐标原点；
步骤S4，从open集中选出F值最小的节点作为当前节点，并将该节点从open集中移除，加入到close集中，若open集为空，则规划失败，即无法找到从起始位姿到达目标位姿的可行路径；
步骤S5，判断当前点是否到达终点附近，若是，则采用曲线拟合方法精确拟合得到从当前位姿到目标位姿的路径；若否，则执行步骤S7；
步骤S6，对曲线拟合得到的路径中的各个离散路径点进行碰撞检测，若所有的离散路径点位姿都通过碰撞检测，则搜索结束，执行步骤S12，否则执行步骤S7
步骤S7，以当前节点作为父节点，其位姿信息与车辆挡位信息可表述为
Figure FDA0002818709050000021
根据车辆运动学模型分别向前和向后进行节点拓展，得到若干个待处理子节点；
步骤S8，选择一个子节点，判断车辆在该子节点
Figure FDA0002818709050000022
处是否与障碍物发生碰撞，若无碰撞则计算其所在位置栅格信息与所在角度栅格信息，并计算从起始节点到该子节点的累计代价Gi，然后根据该子节点所在位置栅格信息在H值查询表中查得该子节点的启发值Hi，继而计算出Fi；
步骤S9，判断选择的子节点是否已经存在于close集当中，若是，则放弃该节点，返回步骤S8，其中判断方法为：计算得到当前节点的所在位置栅格信息、角度栅格信息和车辆挡位信息，遍历close集判断其中是否存在具有同样上述信息的节点；
步骤S10，判断选择的子节点是否已经存在于open集当中，若是，则进行累计代价比较与节点更新处理；若否，则将该节点加入到open集当中；
步骤S11，以步骤S4所述的当前节点位姿作为起点，根据每个节点所存储的父节点信息进行路径回溯，得到搜索出的初始路径；
步骤S12，对搜索出的初始路径进行圆弧插值处理，得到一条路点间隔均匀的路径，并结合曲线拟合路径，最终得到从起始位姿到达目标位姿的全局路径。
2.根据权利要求1所述的非结构化道路全局路径规划方法，其特征在于：所述步骤S10中的累计代价Gi比较与节点更新处理的具体方法为：判断存在于open集中的节点No的累计代价值Go与当前节点Ni的累计代价值Gi的大小，若Go>Gi，则将open集中存放的节点No的位姿信息替换为节点Ni的位姿信息，车辆挡位信息do替换为di，然后将累计代价值Go替换为Gi，最后将父节点信息替换为节点Ni的父节点信息；若Go<Gi，则不进行任何处理，继续执行步骤S8，直到所有子节点处理完毕后返回步骤S4。
3.根据权利要求2所述的非结构化道路全局路径规划方法，其特征在于：所述步骤S7根据车辆运动学模型分别向前和向后进行节点拓展具体包括以下步骤：
S7-1，设置车辆的轴距Wb、最大前轮转向角θmax；
S7-2，确定前轮转角离散数量N，将前轮转角范围[-θmax,θmax]进行均匀离散，得到每个离散位置的前轮转向角计算公式为：
θi＝-θmax+i·Δθ
其中，i＝0,1,…,N-1；Δθ为相邻转角差，计算公式为：
Figure FDA0002818709050000031
S7-3，确定从每个离散角度处向前或向后拓展的子节点数量M。当向后拓展时，在该离散角度处的拓展子节点数量M固定为1；当向前拓展时，在该离散角度处的拓展子节点数量M由下式计算得到：
Figure FDA0002818709050000032
其中，i与步骤S7-2中式(1)中的i一致；N为前轮转角离散数量，其必须为一个奇数，可根据实际需要和角度离散分辨率γ合理选择；
S7-4，从父节点
Figure FDA0002818709050000033
开始，在每个离散前轮转角处根据车辆运动学模型生成M个子节点
Figure FDA0002818709050000034
Figure FDA0002818709050000035
Figure FDA0002818709050000036
Figure FDA0002818709050000037
Figure FDA0002818709050000038
Figure FDA0002818709050000041
Figure FDA0002818709050000042
其中，k＝1,2,…,M，即当k取不同数值时，在同一个离散前轮转角处可拓展得到多个子节点；L为拓展步长；Wb为车辆轴距；R为车辆的转弯半径；f为正负号标志，向前拓展时f取1，向后拓展时f取-1；
S7-5，将所有拓展得到的子节点的父节点都设为同一点，即步骤S7-4中的Np。
4.根据权利要求3所述的非结构化道路全局路径规划方法，其特征在于：所述步骤S8中，累计代价Gi的具体计算公式如下：
Figure FDA0002818709050000043
Figure FDA0002818709050000044
Figure FDA0002818709050000045
Figure FDA0002818709050000046
其中，Gp为当前子节点的父节点累计代价；α1、α2、α3为权重系数；
Figure FDA0002818709050000047
表示从父节点到当前点的步长代价，
Figure FDA0002818709050000048
表示从父节点到当前点的角度变化代价，
Figure FDA0002818709050000049
表示从父节点到当前点的方向切换代价。
5.根据权利要求4所述的非结构化道路全局路径规划方法，其特征在于：所述步骤S12中，对搜索得到的初始路径进行圆弧插值处理具体包括以下步骤：
S12-1，从起点开始，依次计算相邻两点之间的距离D，若该距离D与搜索步长L近似，则不需要进行插值；若距离D明显大于步长L，则执行下一步骤；
S12-2，根据几何关系，可推导得出相邻两点的转弯半径R，具体计算方法如下：
Figure FDA00028187090500000410
其中，
Figure FDA00028187090500000411
为相邻两点的横摆角差值；
S12-3，选取不同的k值，从而得到相应的β值，之后获得各插值点的位置信息和横摆角信息；
S12-4，当计算得到的插值点与原路径点距离过近时，则放弃该插值点，并停止在该相邻两点的插值操作，继续执行步骤S12-1，直到所有点之间插值完后结束。
6.根据权利要求5所述的非结构化道路全局路径规划方法，其特征在于：所述步骤S12中，得到的相邻路点之间的插值点所在位置即对应通过步骤S7拓展得到的该相邻路点之间的其他子节点的位置。
7.一种应用于权利要求1至6任意一项所述方法的系统，其特征在于：包括车载环境感知单元、决策规划单元、底层控制单元，所述决策规划单元内搭载有执行上述方法的程序。
CN202011406640.4A 2020-12-04 2020-12-04 一种非结构化道路全局路径规划方法与系统 Active CN112612266B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011406640.4A CN112612266B (zh) 2020-12-04 2020-12-04 一种非结构化道路全局路径规划方法与系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011406640.4A CN112612266B (zh) 2020-12-04 2020-12-04 一种非结构化道路全局路径规划方法与系统

Publications (2)

Publication Number Publication Date
CN112612266A true CN112612266A (zh) 2021-04-06
CN112612266B CN112612266B (zh) 2022-04-01

Family

ID=75228889

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011406640.4A Active CN112612266B (zh) 2020-12-04 2020-12-04 一种非结构化道路全局路径规划方法与系统

Country Status (1)

Country Link
CN (1) CN112612266B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113495566A (zh) * 2021-08-21 2021-10-12 福州大学 一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法
CN115014375A (zh) * 2022-06-06 2022-09-06 北京京深深向科技有限公司 碰撞检测方法、装置及电子设备、存储介质
JP2023000548A (ja) * 2021-06-18 2023-01-04 キャリオ技研株式会社 制御装置
CN115840454A (zh) * 2023-02-20 2023-03-24 江苏集萃清联智控科技有限公司 非结构化道路冲突区域的多车轨迹协同规划方法及装置
CN117516549A (zh) * 2024-01-03 2024-02-06 江苏领创星通卫星通信科技有限公司 一种基于惯导与卫星的路径规划算法

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101769754A (zh) * 2010-01-19 2010-07-07 湖南大学 一种基于类三维地图的移动机器人全局路径规划方法
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法
CN106444769A (zh) * 2016-10-31 2017-02-22 湖南大学 一种室内移动机器人增量式环境信息采样的最优路径规划方法
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
CN109062209A (zh) * 2018-08-07 2018-12-21 安徽工程大学 一种智能辅助驾驶控制系统及其控制方法
CN110471408A (zh) * 2019-07-03 2019-11-19 天津大学 基于决策过程的无人驾驶车辆路径规划方法
US20190361452A1 (en) * 2018-05-22 2019-11-28 King Fahd University Of Petroleum And Minerals Method and system for controlling a vehicle
CN110531746A (zh) * 2018-05-23 2019-12-03 郑州宇通客车股份有限公司 一种自动驾驶车辆控制方法及系统、车辆
CN111427368A (zh) * 2020-06-01 2020-07-17 广州鹄志信息咨询有限公司 一种改进的多目标无人智能车避碰行驶方法
CN111857148A (zh) * 2020-07-28 2020-10-30 湖南大学 一种非结构化道路车辆路径规划方法
US20200363801A1 (en) * 2019-05-15 2020-11-19 Baidu Usa Llc Relaxation optimization model to plan an open space trajectory for autonomous vehicles

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101769754A (zh) * 2010-01-19 2010-07-07 湖南大学 一种基于类三维地图的移动机器人全局路径规划方法
CN105549597A (zh) * 2016-02-04 2016-05-04 同济大学 一种基于环境不确定性的无人车动态路径规划方法
CN106444769A (zh) * 2016-10-31 2017-02-22 湖南大学 一种室内移动机器人增量式环境信息采样的最优路径规划方法
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
US20190361452A1 (en) * 2018-05-22 2019-11-28 King Fahd University Of Petroleum And Minerals Method and system for controlling a vehicle
CN110531746A (zh) * 2018-05-23 2019-12-03 郑州宇通客车股份有限公司 一种自动驾驶车辆控制方法及系统、车辆
CN109062209A (zh) * 2018-08-07 2018-12-21 安徽工程大学 一种智能辅助驾驶控制系统及其控制方法
US20200363801A1 (en) * 2019-05-15 2020-11-19 Baidu Usa Llc Relaxation optimization model to plan an open space trajectory for autonomous vehicles
CN110471408A (zh) * 2019-07-03 2019-11-19 天津大学 基于决策过程的无人驾驶车辆路径规划方法
CN111427368A (zh) * 2020-06-01 2020-07-17 广州鹄志信息咨询有限公司 一种改进的多目标无人智能车避碰行驶方法
CN111857148A (zh) * 2020-07-28 2020-10-30 湖南大学 一种非结构化道路车辆路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
KANGBIN TU: "Hybrid A∗ Based Motion Planning for Autonomous Vehicles in Unstructured Environment", 《2019 IEEE INTERNATIONAL SYMPOSIUM ON CIRCUITS AND SYSTEMS (ISCAS)》 *
诸葛程晨: "复杂环境下的地面无人车辆路径规划算法研究", 《中国博士学位论文全文数据库-工程科技Ⅱ辑》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2023000548A (ja) * 2021-06-18 2023-01-04 キャリオ技研株式会社 制御装置
JP7291354B2 (ja) 2021-06-18 2023-06-15 キャリオ技研株式会社 制御装置
CN113495566A (zh) * 2021-08-21 2021-10-12 福州大学 一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法
CN113495566B (zh) * 2021-08-21 2023-03-31 福州大学 一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法
CN115014375A (zh) * 2022-06-06 2022-09-06 北京京深深向科技有限公司 碰撞检测方法、装置及电子设备、存储介质
CN115014375B (zh) * 2022-06-06 2023-11-03 北京京深深向科技有限公司 碰撞检测方法、装置及电子设备、存储介质
CN115840454A (zh) * 2023-02-20 2023-03-24 江苏集萃清联智控科技有限公司 非结构化道路冲突区域的多车轨迹协同规划方法及装置
CN117516549A (zh) * 2024-01-03 2024-02-06 江苏领创星通卫星通信科技有限公司 一种基于惯导与卫星的路径规划算法
CN117516549B (zh) * 2024-01-03 2024-03-29 江苏领创星通卫星通信科技有限公司 一种基于惯导与卫星的路径规划算法

Also Published As

Publication number Publication date
CN112612266B (zh) 2022-04-01

Similar Documents

Publication Publication Date Title
CN112612266B (zh) 一种非结构化道路全局路径规划方法与系统
CN109764886B (zh) 一种路径规划方法
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN111811514B (zh) 一种基于正六边形栅格跳点搜索算法的路径规划方法
CN108196536A (zh) 一种改进的无人车快速搜索随机树路径规划方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN109542117B (zh) 基于改进rrt的水下航行器滚动规划算法
CN111174798A (zh) 一种足式机器人路径规划方法
CN110220528A (zh) 一种基于a星算法的自动驾驶无人车双向动态路径规划方法
WO2022142893A1 (zh) 双足机器人路径规划方法、装置和双足机器人
CN111982142A (zh) 一种基于改进a星算法的智能车全局路径规划方法
CN113467476B (zh) 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
Fuji et al. Trajectory planning for automated parking using multi-resolution state roadmap considering non-holonomic constraints
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
CN115164907A (zh) 基于动态权重的a*算法的森林作业机器人路径规划方法
CN114527761A (zh) 一种基于融合算法的智能汽车局部路径规划方法
CN115562290A (zh) 一种基于a星惩罚控制优化算法的机器人路径规划方法
CN114428499A (zh) 一种融合Astar与DWA算法的移动小车路径规划方法
CN113341999A (zh) 一种基于优化d*算法的叉车路径规划方法及装置
Meng et al. Improved hybrid A-star algorithm for path planning in autonomous parking system based on multi-stage dynamic optimization
CN111857148A (zh) 一种非结构化道路车辆路径规划方法
CN115950431A (zh) 一种路径规划方法、系统、计算机设备、可读存储介质及机动车
CN114442634A (zh) 一种车辆的路径规划方法、装置、设备及介质
Das et al. Trajectory planning and fuzzy control for perpendicular parking

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