CN110967032B - 一种野外环境下无人车局部行驶路线实时规划方法 - Google Patents

Abstract

本发明提供了一种野外环境下无人车局部行驶路线实时规划方法，主要面向野外环境下无人车感知范围有限、局部地图实时生成、地面属性多样的路线规划场景。本发明在划分完小单元栅格的实时地图基础上，通过考虑以每个可行驶单元栅格为中心点的不同区域大小的周围栅格的障碍物标识，增加了两类单元栅格标识；在此基础上，灵活利用不同标识对应的单元栅格，综合考虑距离以及可行驶子区域的横滚角、俯仰角、粗糙度等地面属性，利用改进后的A*算法，实时规划了局部可行驶路线，提高了路线规划的成功率，优化了到达局部终点的距离和时间成本；当在路线规划过程中失败时，设计了路线再规划策略。

Description

一种野外环境下无人车局部行驶路线实时规划方法

技术领域

本发明属于路径规划领域，具体涉及一种野外环境下的无人车局部行驶路线实时规划方法。

背景技术

根据对环境信息的把握程度，可把路径规划划分为基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划。路径规划包含路线规划和行驶轨迹规划两部分内容。其中，路线规划针对静态地图，生成可供主体行驶的路线，作为动态轨迹规划的输入；轨迹规划确定主体到达每一实际行驶轨迹点的速度和方向。在野外环境下，无人车感知***通过传感器实时感知环境信息，确定自身位置及其感知范围内的障碍物分布情况，实时生成局部地图，从而可以利用规划算法得到从当前位置到局部子目标点的最优路线。常用的局部路线规划方法有A*搜索算法、人工势场法、蚁群算法、遗传算法、粒子群算法、量子粒子群算法、神经网络算法等。人工势场法、蚁群算法、遗传算法等方法容易陷入局部最优，且时间效率不高；神经网络算法等学习方法需要大量样本，在未有划定道路的野外无人车局部路线规划中并不适用。而A*算法恰可以解决这些缺点，既有极大概率求出最优解，又可以减少冗余的时间。因此，本发明拟基于A*算法设计路线规划方法，但传统的基于栅格的A*算法存在以下问题：

第一，可行驶区域的有效利用与计算成本的矛盾。栅格划分大，则其常因为小体积的障碍物而被标识为不可行驶，浪费了栅格内的其它可行驶区域空间，容易造成路线规划失败的问题；栅格划分小，则会提高计算成本，影响规划时间。

第二，只考虑路线的距离成本，很少考虑可行驶区域内的地面属性(如粗糙度、横滚角、俯仰角)等，不能为下一步的轨迹规划提供较优的输入路线，以至于影响轨迹规划中的速度规划，进而影响到达目的地的时间。

第三，缺乏当前栅格划分下路线规划失败时的再规划方法研究。已有的A*算法没有考虑当前地图规划失败时，灵活应用不同的可行驶子区域大小进行栅格的再标定，以形成规划路线；也没有考虑调整车向或位置进行地图的实时生成与路线再规划方法。

发明内容

针对无人车在野外环境进行路线规划时对应的场景特点，以及传统的基于栅格的A*算法存在的上述问题，本发明提出了一种野外环境下无人车局部行驶路线实时规划方法。其特征在于根据激光雷达、超声波雷达、摄像头等多传感器融合数据实时生成的局部地图，以及无人车和最终目的地的位置坐标，考虑距离和野外地面属性，灵活运用3种可行驶子区域划分方法，基于A*算法实时规划出无人车的局部可行驶路线，所述方法包括下列步骤：

S1.设计输入数据：需要多传感器数据融合后实时生成的局部地图，地图由边长为1的单元栅格构成；每个单元栅格的障碍物标识(无影响行驶的障碍物时标0，否则标1)、标识为0的单元栅格的横滚角、俯仰角、粗糙度和坐标；无人车和最终目的地的坐标；已有既定路线数据；

S2.判断当前地图是否可生成局部终点：根据输入数据判断当前地图是否可生成局部终点；如最终目的地与无人车坐标的连线与局部地图无交点，即与车辆坐标系中Y正轴的角度大于90°，则需舍弃当前地图，按既定路线行驶；如无既定路线，则无人车需要调整车向或位置重新感知环境生成地图；

S3.可行驶栅格的标定：将每一个标识为0的单元栅格作为中心质点，判断其C*C个单元栅格是否全为0，其中C为奇数，C值的确定方法为当前无人车多传感器安装条件下，在路线规划失败时无人车可在当前栅格位置上调整方向重新扫描以及在当前栅格位置可实现无人车低速下直角拐弯行驶所需的最小长度值除以栅格分辨率的向上取整值，如向上取整值为偶数，则C的值需加1，如C*C个栅格的标识都为0，则对此单元栅格增加标识“2”；将每一个标识为0的单元栅格作为中心质点，判断其水平奇数A个、垂直奇数C个栅格是否标识为0，A的值等于车宽除以栅格分辨率得出的向上取整值，如取整值为偶数，则A的值需加1，如A*C个栅格的标识都为0，则对此单元栅格增加标识“3”；将每一个标识为0的单元栅格作为中心质点，判断其水平奇数C个栅格、垂直奇数A个栅格是否都标识为0，如是，则对此单元栅格增加标识“4”；

S4.局部地图终点的生成：选取距离最终目的地最近的当前标识为“2”的栅格为局部地图终点(该类栅格可以调转车辆方向)；

S5.局部地图路线生成：综合考虑距离以及子区域所含单元栅格的最大横滚角、最大俯仰度、最大粗糙度，利用A*算法，主要基于“2”类栅格规划可行驶路线；当规划到某一栅格失败时，根据车头方向，考虑“3”类或“4”类相邻单元栅格进行规划；如路线规划成功，则需要比较当前路线与已有路线的成本，选取最优路线行驶；

S6.路线规划失败后的再规划行动策略：如当前地图路线规划失败，且无既定路线，则需执行路线的再规划策略。

相比于传统的栅格一次性划分方法以及只考虑距离的A*算法，本发明的优点在于增加了可在原地调转或直接拐弯到相邻可行驶子区域的单元栅格标识，以及只可够前进不能转向的可行驶子区域对应的单元栅格标识；在此基础上，灵活利用两类标识对应的单元栅格，综合考虑距离以及可行驶子区域的横滚角、俯仰角以及粗糙度等地面属性，基于A*算法，实时规划了局部地图的可行驶路线，提高了路线规划的成功率，优化了到达局部终点的距离和时间成本。

附图说明

图1为野外环境下无人车局部行驶路线实时规划方法的流程。

图2为对可行驶单元栅格的标识流程。

图3为多种可行驶栅格划分下的路线规划流程。

图4为路线规划过程中失败时的处理流程。

具体实施方式

下面结合附图对本发明作进一步说明，构成本发明的一部分。所应理解的是，此处所述仅为本发明的具体实施个例，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内所做的任何等同替换或改进等，均应包含在本发明的保护范围之内。

如图1所示，本发明实施例提供的野外环境下无人车局部行驶路线实时规划方法，主要包括3个步骤：

步骤1，获取相关数据：获取栅格地图数据、无人车当前位置、最终目的地坐标和既定路线输入；

步骤2，如局部地图终点不可生成，但有既定路线，则按既定路线行驶；若局部地图终点不可生成，且无既定路线，则无人车需旋转扫描获取新地图，转步骤3；如局部地图终点可生成，则规划该地图对应的路线，即首先标识不同可行驶子区域划分下的可行驶单元栅格，并生成局部地图终点对应的栅格，之后灵活应用不同单元栅格标识，综合考虑距离及可行驶子区域地面多种属性，利用A*算法，进行路线规划，如路径规划成功，则将新规划输出路线成本与既定路线成本比较，选择成本少的路线行驶，如路径规划失败，则转步骤3；

步骤3，当前地图路线规划失败时的再规划。

为了更好地理解本发明实施例提供的野外环境下无人车局部行驶路线实时规划方法，对上述3个步骤进行详细说明。

设野地无人车宽为2.6米，长5.7米，可实现以车辆质点为中心的原地旋转。参照图2所示，本发明实施例首先提供从读取输入数据到形成不同可行驶子区域划分下的单元栅格标识以及生成局部地图终点的流程，其包括以下步骤：

步骤(1)：输入栅格地图数据、无人车坐标(起点)、最终目的地坐标和既定路线数据，如当前无人车所在位置为最终目的地，则停止路线规划；如否，则判断最终目的地与无人车坐标的连线与局部地图有无交点，即与车辆坐标系中Y正轴的角度是否大于90°，如大于则不能生成局部终点，此时需舍弃当前地图，转步骤(2)；若可生成，则转步骤(3)

步骤(2)：如有既定路线，则按既定路线行驶；如无既定路线，则转图4所关联步骤；

步骤(3)：将每一个标识为0的1*1(m²)单元栅格作为中心质点，判断包含每个质点在内的周围7*7个单元栅格是否全为0，如是，则对此单元栅格增加标识“2”；将每一个标识为0的单元栅格作为中心质点，判断水平(与X轴平行)3个单元栅格(总长刚大于车宽)、垂直7个单元栅格(总长刚大于等于其车长)是否标识为0，如是，则对此单元栅格增加标识“3”；将每一个标识为0的单元栅格作为中心质点，判断水平(与X轴平行)7个单元栅格、垂直(与Y轴平行)3个栅格是否标识为0，如是，则对此单元栅格增加标识“4”；标识了“2”、“3”或“4”的单元栅格称为可行驶栅格，以其为质心的7*7、3*7或7*3个为0的单元栅格构成不同划分下的可行驶子区域；

步骤(4)：选取距离最终目的地最近的当前标识为“2”的栅格为局部地图终点(以利于下一步调转车辆方向扫描生成地图的可行性)，转图3所示的步骤；

局部地图终点和无人车可行驶的单元栅格标识确定后，参照图3所示，综合考虑距离以及可行使子区域所包含单元栅格的最大横滚角、最大俯仰度和最大粗糙度，利用A*算法，灵活应用3类栅格标识规划可行驶路线，其包括以下步骤：

步骤(1)将起点放入Open list，设置其为当前center node，优先判断Centercode的邻居是否有“2”类可行点(7*7)，如有，转步骤(2)，否则转步骤(11)；

步骤(2)判断相邻可行点为质心的可行驶子区域中是否含有最终点坐标，如有，则输出到最终点坐标的路径，路径规划结束；否则，将center node相邻方格的可行点放入Open list，并把这些可行点方格的parent node设置为当前center node，储存记录；

步骤(3)判断相邻方格是否有局部终点，如无，则将起点从Open list移除，加入到Close list，转步骤(5)；如有，则输出局部路径，并判断是否有既定路线，转步骤(4)；

步骤(4)如无既定路线，则按新地图所输出的路线行驶，路径规划结束；如有既定路线，则判断当前起点到新地图局部终点的路线成本+新地图局部终点到最终点的路线成本<当前位置到当前所行驶路线终点成本+当前所行驶路线局部终点到最终点的成本是否成立，如成立，则按新地图所输出的路线行驶，路径规划结束，反之按既定路线行驶，结束路线规划；

步骤(5)计算当前Open list中可行点的G、H、F值并存储。G为从起点移动到指定栅格的移动代价(沿着到达该方格而生成的路径)：G(n)＝G(fon)+D(cn,n)，G(fon)为可行点n的父节点的累积G值，当前其父节点(center node)为地图起点，则G(fon)＝0，D(cn,n)为当前center node到邻居可行点n的距离；H(n)为n到最终目的地的曼哈顿距离；F(n)＝G(n)+H(n)；

步骤(6)计算所有Open list中单元栅格的A和C值。A(n)为对以邻居可行点n为中心质点的可行驶子区域的横滚角

俯仰角

粗糙度

分别归一化后(对应Open list中所有的单元栅格进行归一化)形成的地面属性值，

其中0≤α、β、γ≤1且α+β+γ＝1；

0≤θ≤1，

为归一化后的距离成本，其影响到达目的地的时间，

为归一化后的地面行驶成本，其影响无人车的速度，进而影响到达目的地的时间；选择当前Open list中C值最小的node，放入Close list，设置为center node，转步骤(7)；

步骤(7)当前center node如没有邻居可行点，则判断其是否有既定路线，如有，则按既定路线行驶，结束路线规划，若没有既定路线，则转步骤(10)；如存在邻居可行点，则转(8)；

步骤(8)遍历未被遍历的可行点(如center node拥有“2”类标识，则首先遍历其“2”类邻居；如按“2”类遍历过邻居，则按当前所使用标识进行遍历)，如该可行点为质心的可行驶子区域中含有最终点坐标，则输出到最终点坐标的路线，路线规划结束；如邻居可行点为局部终点，则将局部终点的父亲设为当前的center node，输出路线，转步骤(4)；否则转步骤(9)；邻居可行点都遍历后，转步骤(6)；

步骤(9)判断此邻居可行点是否在Close list，若为是，则转向步骤(8)；若为否，则判断此邻居可行点是否已在Open list；若为是，则假设当前center node为其父节点，计算其G’、F’值，如其G’值小于该可行点的原有G值，则将其父节点设置为当前的centernode，更新原有G、F值，转步骤(8)；若为否，则将其加入Open list，计算其G、H、F值并存储，并设置其父节点为当前center node，转步骤(8)；

步骤(10)判断当前center node是否已按3*7或7*3区域质心进行路线规划，如是则转到图4所关联的步骤；如否则转步骤(11)；

步骤(11)根据车头方向将其作为3*7或7*3区域的质心进行路线规划，判断当前center node的邻居是否有可行点(注意其左右邻居单元格不是邻居)，若是则转步骤(2)，若否，则判断是否有既有路线，若是，则按既有路线行驶，当前路线规划结束，若否，则转图4所关联的步骤。

对当前地图路线规划到某一栅格失败且无既定路线时，则需在当前或最近行驶过的可调转车头的“2”类栅格内旋转车辆，重新扫描生成地图，直到生成可行驶路线，如图4所示，其包括以下步骤：

步骤(1)判断当前所在可行驶子区域能否原地旋转，如不能，则需逆向追溯路线退到行驶过的离当前所在位置最近的“2”类栅格，转步骤(2)；如可原地旋转，则直接转步骤(2)；

步骤(2)对应当前栅格，判断旋转次数(旋转次数需要存储，属于“2”类单元栅格结构体中的成员)是否已等于4，如否，转步骤(3)；如是，则判断当前是否到了最初出发点，如是，则结束当前路线规划，提示出发点选择错误，如否，则需逆向追溯路线退到行驶过的离当前所在位置最近的“2”类栅格，转步骤(2)；

步骤(3)如之前在此栅格时未接收过地图(旋转次数初始值为0)，则将车头转到朝向最终目的地的位置，旋转次数加1，扫描生成地图，读取相关输入数据，转步骤(3)；如之前在此栅格接收过地图(旋转次数初始值为1)，则按朝着离最终点夹角小的方向确定车头旋转方向(第一次确定后则在此栅格旋转时一直遵循此方向)，在最近的拟扫描或接收地图时车头方向角的基础上再旋转90度，将旋转次数加1，如当前方向可生成局部终点，则开始扫描生成地图，转图2所关联的步骤(3)；否则，转步骤(2)。

本发明利用单元栅格障碍物标识与以单元栅格为中心点的不同划分范围下的可行驶子区域，重新标识了单元栅格，有效利用了可行驶空间，提高了路线规划成功率；综合考虑了距离与地面属性，优化了到达终点的时间。此外，提出了当前地图路线规划失败时的解决方法。

上述说明示出并描述了本发明的优选实施例，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实例的排除，而可用于各种其他组合、修改和环境，并能够在本文发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (3)

1.一种野外环境下无人车局部行驶路线实时规划方法，其特征在于，获取激光雷达、超声波雷达、摄像头融合数据实时生成的局部地图以及无人车和最终目的地的位置坐标；所述获取的数据包括，获取栅格地图数据、无人车当前位置、最终目的地坐标和既定路线输入相关数据，采用 3 种可行驶子区域划分方法和局部可行驶路线的生成方法，考虑距离和野外地面属性，基于改进的A*算法实时规划出无人车的局部可行驶路线；如路线规划失败，则执行当前地图路线规划失败时的再规划策略；

其中，3种可行驶子区域划分方法包括：将每一个标识为0的单元栅格作为中心质点，判断其L倍车长半径内的C*C个单元栅格是否全为0，其中，L的确定应满足当前无人车多传感器安装条件下，在路线规划失败时无人车可在当前C*C个单元栅格范围内调整方向重新扫描，以及相邻的C*C栅格可实现低速下直角拐弯行驶；

如是，则对此单元栅格增加标识“2”；将每一个标识为0的单元栅格作为中心质点，判断其与X轴平行的a+1个单元栅格、与X轴垂直的b+1个单元栅格是否标识为0，如是，则对此单元栅格增加标识“3”；将每一个标识为0的单元栅格作为中心质点，判断其与X轴平行的b+1个单元栅格、与Y轴平行的a+1个栅格是否标识为0；

如是，则对此单元栅格增加标识“4”；标识了“2”、“3”或“4”的单元栅格被称为可行驶栅格，以其为质心的C*C、(a+1)*(b+1)或(b+1)*(a+1)个为0的单元栅格构成可行驶子区域；

其中，C为奇数，a为偶数，b为偶数；与X轴平行的a+1个单元栅格的总长刚大于等于车宽、与X轴垂直的b+1个单元栅格的总长刚大于等于车长；与X轴平行的b+1个单元栅格的总长刚大于等于车长、与Y轴平行的a+1个栅格的总长刚大于等于车宽；

其中，当前地图路线规划失败时的再规划策略包括：判断当前所在可行驶子区域能否原地旋转，如不能，则需逆向追溯路线退到行驶过的离当前所在位置最近的“2”类栅格；如可原地旋转，则对应当前栅格，将车头转到朝向最终目的地的位置，旋转次数加1，扫描生成地图，如旋转次数等于4次，则退到行驶过的离当前所在位置最近的“2”类栅格；如退回到达的栅格接收过地图，其中，旋转次数初始值为1，则按朝着离最终点夹角小的方向确定车头旋转方向，其中，第一次确定后则在此栅格旋转时一直遵循此方向，在最近的拟扫描或接收地图时车头方向角的基础上再旋转90度，将旋转次数加1，如当前方向可生成局部终点，则开始扫描生成地图。

2.根据权利要求1所述的野外环境下无人车局部行驶路线实时规划方法，其特征在于，获取栅格地图数据包括：地图由边长为1的单元栅格构成，每个单元栅格的障碍物标识，无影响行驶的障碍物时标为0，否则标为1；标识为0的单元栅格的横滚角、俯仰角、粗糙度和坐标。

3.根据权利要求1所述的野外环境下无人车局部行驶路线实时规划方法，其特征在于，局部可行驶路线的生成方法包括：综合考虑距离以及子区域所含单元栅格的最大横滚角、最大俯仰度、最大粗糙度，利用A*算法，基于“2”类栅格规划可行驶路线；当规划到某一栅格失败时，根据车头方向，考虑“3”类或“4”类相邻单元栅格进行规划；如路线规划成功，则比较当前路线与已有路线的成本，选取最优路线行驶。

Images