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

本发明涉及自动驾驶领域，尤其是一种非结构化道路全局路径规划方法与系统。

背景技术：

自动驾驶车辆的发展与推广在提高车辆通行效率、保障行车安全、建设智慧交通体系等方面具有重大意义。近几年来，对自动驾驶车辆相关技术的研究成为各个领域专家学者关注的焦点。路径规划作为自动驾驶车辆最重要的关键技术之一，其主要任务是：给定自动驾驶车辆起始点和目标点后，在一定的环境模型基础上按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。根据不同场景，自动驾驶车辆的路径规划可分为结构化道路的路径规划和非结构化道路的路径规划。其中，非结构化道路由于环境信息的复杂性，其路径规划方法一直是自动驾驶领域的重要研究内容。

目前，针对非结构化道路场景的路径规划方法主要可分为基于采样的方法和基于图搜索的方法两大类。基于采样的方法主要有rrt算法及其衍生算法，此类方法虽然具有很高效率，但其搜索得到的路径较为随机，无法满足稳定性需求。基于图搜索的方法主要有dijkstra算法、a*算法等。dijkstra算法是一种具有代表性的最短路径规划算法，其以起始点为中心向外层层拓展，直到扩展到终点为止，但该算法计算效率不高，无法满足车辆路径规划的实时性需求。a*算法是在dijkstra算法的基础上引入了启发函数，在保证最优性的同时，可大大减少节点拓展数量从而提高路径搜索效率，因而成为非结构化道路路径规划中最常用的方法。

如专利文件cn110220528a提出分别从起始点和目标点同时使用a*算法进行搜索，直到搜索到相同节点时终止。其虽然提高了搜索效率，但未考虑车辆的非完整约束特性，无法满足自动驾驶车辆路径规划需求；专利文件cn110333659a中提出在a*算法的启发函数中引入安全代价估计，使得规划出的路径远离障碍物。其虽然保证了路径安全性，但同样的，该方法并未考虑车辆的非完整约束特性。专利文件cn108444488a中，在节点拓展的过程中考虑了车辆的非完整约束，并根据栅格精度、车辆行驶速度和局部路径更新时间来确定搜索步长，但使用该方法无法精确到达目标位置和方向，且搜索步长的选择依赖于车辆行驶速度，故只适合于车辆的实时局部路径规划。专利文件cn110487290a提出的变步长a*搜索局部路径规划方法中，其根据拓展点与障碍物之间的距离自适应的从事先设置好的模板中得到不同的搜索步长。该方法虽然在一定程度上降低了节点拓展数量，但因为步长不固定，导致搜索得到的路点距离不均匀，从而使得路径后处理变得十分困难，并且获取拓展点与障碍物之间的距离的过程也会影响路径搜索效率。

综上，现有的基于a*算法的非结构化道路路径规划方法都无法做到：既能满足车辆的非完整约束特性要求，又能在保证车辆安全性的同时加快路径搜索速度，同时保证路点的均匀性。

技术实现要素：

针对现有技术存在的不足，本发明的目的在于提供一种非结构化道路全局路径规划方法与系统，通过改进a*算法的节点拓展方式，同时对搜索得到的路径进行圆弧插值，以克服或减轻现有技术的上述缺陷。

为实现上述目的，本发明提供了如下技术方案：一种非结构化道路全局路径规划方法，包括如下步骤：

步骤s1，输入车辆起始位姿、目标位姿和全局地图信息，设置搜索空间的栅格位置分辨率ξ和角度分辨率γ，并根据栅格位置分辨率ξ对地图进行栅格化处理，其中，起始位姿信息和目标位姿信息包括车辆的后轴中心坐标信息(x,y)和车辆横摆角信息

步骤s2，设置评估函数f＝g+h，并通过预计算得到h值查询表，预计算的方式为选取广度优先算法、dijkstra算法或动态规划算法的其中一个，在二维栅格地图中计算得到从每个离散栅格位置到达目标栅格位置的最近距离作为启发值h；步骤s3，初始化open集与close集，将起始节点放入open集中，该起始节点包含车辆位姿信息车辆挡位信息d、所在位置栅格信息所在角度栅格信息值、f值和父节点信息，其中的车辆档位信息d包括车辆的前进、倒退信息；所述的位置栅格信息和角度栅格信息通过以下各式计算得到：

其中，o为选取的坐标原点；

步骤s4，从open集中选出f值最小的节点作为当前节点，并将该节点从open集中移除，加入到close集中，若open集为空，则规划失败，即无法找到从起始位姿到达目标位姿的可行路径；

步骤s5，判断当前点是否到达终点附近，若是，则采用曲线拟合方法精确拟合得到从当前位姿到目标位姿的路径；若否，则执行步骤s7；

步骤s6，对曲线拟合得到的路径中的各个离散路径点进行碰撞检测，若所有的离散路径点位姿都通过碰撞检测，则搜索结束，执行步骤s12，否则执行步骤s7

步骤s7，以当前节点作为父节点，其位姿信息与车辆挡位信息可表述为根据车辆运动学模型分别向前和向后进行节点拓展，得到若干个待处理子节点；

步骤s8，选择一个子节点，判断车辆在该子节点处是否与障碍物发生碰撞，若无碰撞则计算其所在位置栅格信息与所在角度栅格信息，并计算从起始节点到该子节点的累计代价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；δθ为相邻转角差，计算公式为：

s7-3，确定从每个离散角度处向前或向后拓展的子节点数量m。当向后拓展时，在该离散角度处的拓展子节点数量m固定为1；当向前拓展时，在该离散角度处的拓展子节点数量m由下式计算得到：

其中，i与步骤s7-2中式(1)中的i一致；n为前轮转角离散数量，其必须为一个奇数，可根据实际需要和角度离散分辨率γ合理选择；

s7-4，从父节点开始，在每个离散前轮转角处根据车辆运动学模型生成m个子节点

其中，k＝1,2,…,m，即当k取不同数值时，在同一个离散前轮转角处可拓展得到多个子节点；l为拓展步长；wb为车辆轴距；r为车辆的转弯半径；f为正负号标志，向前拓展时f取1，向后拓展时f取-1；

s7-5，将所有拓展得到的子节点的父节点都设为同一点，即步骤s7-4中的np。作为本发明的进一步改进，所述步骤s8中，累计代价gi的具体计算公式如下：

其中，gp为当前子节点的父节点累计代价；α1、α2、α3为权重系数；表示从父节点到当前点的步长代价，表示从父节点到当前点的角度变化代价，表示从父节点到当前点的方向切换代价。

作为本发明的进一步改进，所述步骤s12中，对搜索得到的初始路径进行圆弧插值处理具体包括以下步骤：

s12-1，从起点开始，依次计算相邻两点之间的距离d，若该距离d与搜索步长l近似，则不需要进行插值；若距离d明显大于步长l，则执行下一步骤；

s12-2，根据几何关系，可推导得出相邻两点的转弯半径r，具体计算方法如下：

其中，为相邻两点的横摆角差值；

s12-3，选取不同的k值，从而得到相应的β值，之后获得各插值点的位置信息和横摆角信息；

s12-4，当计算得到的插值点与原路径点距离过近时，则放弃该插值点，并停止在该相邻两点的插值操作，继续执行步骤s12-1，直到所有点之间插值完后结束。作为本发明的进一步改进，所述步骤s12中，得到的相邻路点之间的插值点所在位置即对应通过步骤s7拓展得到的该相邻路点之间的其他子节点的位置。

本发明另一方面提供了一种系统，包括车载环境感知单元、决策规划单元、底层控制单元，所述决策规划单元内搭载有执行上述方法的程序。

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

附图说明

图1是本发明一种非结构化道路全局路径规划方法的流程图；

图2是本发明使用的车辆运动学模型示意图；

图3是本发明改进的节点拓展方式示意图；

图4是本发明搜索得到路点之间几何关系示意图；

图5是本发明路径点圆弧插值示意图；

图6是本发明非结构化道路全局路径规划效果图；

图7是本发明非结构化道路全局路径规划系统示意图。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

参照图1所示，本实施例的一种非结构化道路全局路径规划方法，其流程如图1所示，具体包括以下步骤：

s1，输入车辆起始位姿、目标位姿和全局地图信息，设置搜索空间的栅格位置分辨率ξ和角度分辨率γ，并根据栅格位置分辨率ξ对地图进行栅格化处理。所述的车辆起始位姿和目标位姿包括车辆的后轴中心坐标信息(x,y)和车辆横摆角信息

s2，设置评估函数f＝g+h，并预计算得到h值查询表。使用的预计算方法根据需要可选取广度优先算法、dijkstra算法或动态规划算法的其中一个，本实施例中采用的是dijkstra算法。通过计算，可得到地图内从所有离散栅格点到达目标栅格点的最小距离，以此作为本方法中的启发值h。值得注意的是，该计算过程应该在步骤s1中构建的栅格化地图中进行，且计算起始点应选择为步骤s1中目标位姿对应的栅格位置点。使用此方法计算得到的启发值h相较于使用简单的欧氏距离作为启发值h，能有效的避免搜索到死胡同区域；

s3，初始化open集与close集，将起始节点放入open集中。所述节点包含位姿信息车辆挡位信息d、所在位置栅格信息所在角度栅格信息值、f值和父节点信息。车辆挡位信息指的是前进、倒退信息，前进设为1，倒退设为0，所在位置栅格信息和角度栅格信息通过以下各式计算得到：

其中，o为选取的坐标原点。

特别地，起始点的挡位信息默认为前进档，即设为1；g值设为0，h值则根据位置栅格信息在h值查询表中查询得到，继而算得f值；父节点设为自身；

s4，从open集中选出f值最小的节点作为当前节点，并将该节点信息从open集中移除，加入到close集中。若open集为空，则规划失败，即无法找到从起始位姿到达目标位姿的可行路径；

s5，判断当前点是否到达终点附近，即判断当前点与目标点的距离是否小于一定阈值。若是，则采用曲线拟合方法精确拟合得到从当前位姿到目标位姿的路径；若否，则执行步骤s7。所述的曲线拟合方法可根据需要可选择rs曲线拟合、dubins曲线拟合等方法。具体的，若需要考虑倒退，则使用rs曲线拟合方法，否则可使用dubins曲线拟合方法。本本实施例中使用的是rs曲线拟合方法；

s6，对曲线拟合得到的路径中的各个离散路径点进行碰撞检测。若所有的节点位姿都通过碰撞检测，则搜索结束，执行步骤s12，否则执行步骤s7；

s7，以当前节点作为父节点，其位姿信息与车辆挡位信息可表述为根据车辆运动学模型分别向前和向后进行节点拓展，得到若干个待处理子节点；

s8，选择一个子节点，判断车辆在该子节点处是否与障碍物发生碰撞。若无碰撞则根据公式(1)和公式(2)计算其所在位置栅格信息与所在角度栅格信息，并计算从起始节点到该子节点的累计代价gi，然后根据该子节点所在位置栅格信息在h值查询表中查得该子节点的启发值hi，继而计算出fi。若车辆在该子节点位姿处与障碍物发生碰撞，则意味着沿着该子节点对应前轮转角处将继续向前拓展是不安全的，故将该子节点对应离散前轮转角处的其他待处理的子节点舍弃。累计代价gi的具体计算公式如下：

其中，gp为当前子节点的父节点的累计代价；α1、α2、α3为权重系数；表示从父节点到当前点的步长代价，表示从父节点到当前点的角度变化代价，表示从父节点到当前点的方向切换代价。

通过一方面考虑了节点拓展时的步长代价，另一方面对后退的搜索进行了惩罚；通过使得车辆尽量少的改变运动方向，保证了路径的平滑性；通过有效避免了搜索得到的路径出现频繁的前进后退切换现象；

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；δθ为相邻转角差，计算公式为：

值得注意的是，θi的正负决定了车辆的转向，当θi<0时，表示车辆左转，当θi>0时，表示车辆右转；

s7-3，确定从每个离散角度处向前或向后拓展的子节点数量m。当向后拓展时，在该离散角度处的拓展子节点数量m固定为1；当向前拓展时，在该离散角度处的拓展子节点数量m由下式计算得到：

其中，i与步骤s7-2中式(1)中的i一致；n为前轮转角离散数量，其必须为一个奇数，可根据实际需要和角度离散分辨率γ合理选择。

s7-4，从父节点位姿开始，在每个离散前轮转角处根据车辆运动学模型生成m个子节点

其中，k＝1,2,…,m，即当k取不同数值时，在同一个离散前轮转角处可拓展得到多个子节点；l为拓展步长；wb为车辆轴距；r为车辆的转弯半径；f为正负号标志，向前拓展时f取1，向后拓展时f取-1。

特别需要指出的是，当离散的前轮转向角θi＝0时，通过上式计算得到车辆转弯半径为无穷大，公式(8)和公式(9)此时不再使用，子节点的位置和横摆角可通过另外简单的几何关系计算得到，这里不作详细阐述。另外，选择的拓展步长l应满足以下关系：以保证相邻两个节点不会落到同一栅格内；

s7-5，将所有拓展得到的子节点的父节点都设为同一点，即步骤s7-4中的np。

本实施例中，设置前轮转角离散数量n＝11，拓展步长l＝2，拓展得到的子节点分布情况如图3所示。图中的每个黑实心点都表示一个子节点位置。从图中可以看出，本方法在当前所设参数下一次最多能向前拓展12m。

结合附图4和附图5，本实施例中，对搜索得到的初始路径进行圆弧插值处理包括以下步骤：

s12-1，从起点开始，依次计算相邻两点之间的距离d。若该距离d与搜索步长l近似，则不需要进行插值；若距离d明显大于步长l，则执行下一步骤；

s12-2，如图4所示，根据几何关系，可推导得出相邻两点的转弯半径r，具体计算方法如下：

其中，为相邻两点的横摆角差值。

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所示，该系统包括车载环境感知单元、决策规划单元的底层控制单元。其中，环境感知单元用于获取环境信息，经处理后得到全局地图信息；决策规划单元则使用本发明中的方法进行全局路径规划，得到的全局路径经后处理后传给底层控制单元；底层控制单元则负责控制车辆沿着当前路径行驶。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。