一种等高线引导线的自主车三维路径规划方法
技术领域
本发明涉及地形分析与路径规划领域，特别涉及一种等高线引导线的自主车三维路径规划方法。
背景技术
传统的地形分析方法，主要是先将传感器获得的DEM数据进行栅格化，得到当前地形的栅格图，然后在车辆运行轨迹上找到车辆的下一个位置，然后在该点上构造分析窗口并获得该窗口上各个坐标点的高程值，再使用统计学工具计算该分析窗口区域的基本地形因子，由此得到该片区域的特征信息，最后由代价函数对这些特征信息进行评估，进而给出所分析的地形区域的代价值。类似地，依次滑动式地构造车辆运行轨迹上的未来所在的各个位置的分析窗口并分析相应窗口下的地形区域，最终得到该条轨迹上一系列位置点的代价值，汇总后成为自主车通过该条轨迹代价。若自主车有多条候选轨迹，则经过以上分析过程，可获得自主车到达目标点各条轨迹的代价值，组织这些代价值可形成自主车通过当前整个地形的代价图。最后，路径规划器可将这个代价图作为其中一个启发式信息进行路径搜索和规划。
这种基于滑动分析窗口须先在轨迹上找到下一个位置点，然后再在该点分析地形，所以对于上述方案而言，如何找到下一个合理位置点是一个非常重要的问题。若下一个位置点离当前点较远，则中间的轨迹区因没被分析而成为空白区域，对自主车来说是地形未知区。因此，比较安全的方法自然是以一个相对较小的步长来滑动分析窗口，这必然导致分析效率低下以及存在冗余计算问题，成为其中一个制约系统效率的瓶颈问题，无法满足实时要求。
为解决上述问题，中国专利公开号为CN104268862A的专利文献中，公开了一种自主车三维地形可通行性分析方法，其主要包括以下步骤：(1)信息感知：获取当前车辆运动的状态信息及环境信息；(2)轨迹搜索与生成：根据当前车辆位置与最终目标位置进行轨迹初步搜索，生成局部车辆可能运动的轨迹集合；(3)地形分析：根据得到的地形高程数据绘制出当前地形的等高线；根据车速v找到候选轨迹与等高线的交点；分析在等高线下交点与自主车当前位置形成的轨迹曲线段的地形特性，计算基本地形因子；(4)可通行性分析：结合车辆的动力学约束和运动学约束，分别计算车辆通行各轨迹曲线段的代价，以此构造代价图；(5)路径规划：以代价图作为启发式信息，寻找最优路径。该文献声称，其具有原理简单、操作简便、判别精度高、计算高效等优点。
上述方法的核心主要是结合车辆的动力学约束和运动学约束以得到构造代价图的，然后通过代价图作为启发式信息，寻找最优路径。在实际使用过程中，存在：
1、在可通行性分析中，需要分别计算车辆通行各轨迹曲线段的代价，存在响应速度慢，在在出现某一轨迹曲线段无法通行时，需要重新进行计算的，进一步造成响应时间慢的问题；
2、在路径规划中，其通过代价图作为启发式信息，寻找最优路径。其定义的最优路径是通过代价图结果中的代价最小实现的，其主要考虑的是最大爬坡角度和最大侧翻角，另外其主要是在步骤2(轨迹搜索与生成)中生成局部车辆可能运动的轨迹集合，然后对候选轨迹与等高线的交点进行分析。由于轨迹集合的数量不可控，其依然存在分析效率非常低的问题。
因此，现在急需一种等高线引导线的自主车三维路径规划方法，能够解决现有三维路径规划方法中存在的响应速度慢、分析效率低的问题。
发明内容
本发明提供了一种等高线引导线的自主车三维路径规划方法，能够解决现有三维路径规划方法中存在的响应速度慢、分析效率低的问题。
为了解决上述技术问题，本申请提供如下技术方案：
一种等高线引导线的自主车三维路径规划方法，包括如下步骤：
S1三维地形建模：通过获取传感器的三维数据，利用姿态传感器，将传感器数据从传感器坐标系转换为全局坐标系，根据约定的分辨率进行栅格化，得到栅格化的坐标系，栅格化的坐标系原点随着车辆的运动定期更新；
S2地面提取：对当前车辆的运动状态进行分析，以车辆当前位置所在平面为中心，在栅格化的坐标系中进行地面提取；
S3等高线提取：以车辆当前点提取的平面高度为基准，计算需要提取的等高线数量和高度，利用高度扫描方法，完成多条等高线的单独提取，并对每条等高线进行平滑和连接，在此基础上，将提取得到的多条等高线合成为一个完整的等高线图；
S4可通行性分析：对于地面上的任意目标点，计算该点附近的两条等高线，并计算该点到两条等高线的最近点距离，进而得到该点的地形方向和坡度；
S5局部路径搜索：以A*方法为基础在栅格化的坐标系中进行局部路径搜索，在节点扩展中，以待扩展点的地形方向进行节点生成，在搜索的启发函数中，增加目标点与该点地形处方向的夹角作为约束，得到搜索路径；
S6全局优化：使用梯度下降法对得到的搜索路径进行平滑，在目标函数中，增加路径点方向和该点处地形方向的夹角作为优化指标，得到最终路径。
基础方案原理及有益效果如下：
由于在现有方案中，该方法在初步轨迹搜索过程中，没有充分利用三维信息，更没有考虑三维地形对车辆运行的引导作用；
该方法在地形分析过程中，对轨迹集上离散位置运用地形因子对地形可通行性进行分析，在分析过程中，运行了车辆的几何模型，导致计算量大；
该方法主要是在初步规划生成轨迹集合的基础上，利用三维地形信息和车辆几何信息在轨迹集合内进行评价和寻优，初步规划结果对后期结果的影响过大。
在本方案的S1中，利用成熟技术，对三维地形建模，并进行栅格化处理，得到栅格化的坐标系，充分考虑三维地形对车辆的引导作用。一方面在S2-S4中，进行相关的分析，使得相应的坐标能够有准确的表达；另一方面，是在S5中，利用A*方法的特性，结合栅格化的坐标系的特性，使得本方案能够直接在栅格化的坐标系中进行局部的路径搜索，而不需要额外的预处理。即本方案，利用现有的技术快速的得到栅格化的坐标系，另外利用A*方法需要进行方格化处理，且依赖“父方格”进行分析的特性，使得本方案能够快速的进行局部路径搜索。
在S2中，能够实时的对地面进行提取，并通过S3中，获得完整的等高线图。上述过程中，均未与目标点相关，即在车辆的正常行进过程中，可以一直保持运行，时刻等待响应。
然后是在S4中，对于给定的任意目标点，是通过该点附近的两条等高线，得到该点的地形方向和坡度，然后再利用S5中的A*方法以及栅格化的坐标系进行局部路径搜索，得到搜索路径，实现在获得目标点后的快速响应。最后是在S6中，利用梯度下降法对搜索路径进行平滑处理(其本身计算量非常小)，最后再考虑路径点方向与该点地形处方向的夹角作为优化指标，得到最终路径，与现有方案相比，并未运行车辆的几何模型，进一步减小了计算量。
在本方案中，在前面不需要计算各轨迹曲线段的代价，减小的相关计算量，减少了响应等待时间，在本方案中，S1-S3均不需要目标点的参与，节省了在轨迹集合中进行筛选分析的时间，避免了无效分析。本方案中，S1-S3对外界情况进行预处理，在出现目标点后，迅速获得目标点的地形方向和坡度，然后利用A*方法和栅格化的坐标系的特性(结合相关的约束条件)，避免了现有方案中初步规划结果严重影响后期结果的问题，实现了快速的得到搜索路径和最终路径，达到响应速度快、分析效率高的效果。
进一步，所述S1的具体流程为：
S1.1在初始T0时刻，构建以车辆起始位置为中心，以WGS84坐标系为坐标轴方向的局部地图；
S1.2局部地图用栅格地图表示，每个栅格的属性包括最大高度Hmax，最小高度Hmin，点数Pnum，平均高度Hmean，周围四邻域栅格的平均高度Hneib；
S1.3整个局部地图的属性包括地图中心点的GPS坐标，栅格分辨率，目前车辆所在的栅格位置。
进一步，所述S1还包括：
S1.4通过多线激光雷达采集周围环境数据，通过高频IMU测量车辆运动信息。
进一步，所述S1还包括：
S1.5当车辆速度大于1米每秒时，采用IMU测量得到的车体姿态数据，完成激光雷达数据从车体坐标系到局部地图坐标变换的转换，并将点云数据栅格化，对局部地形进行更新；
S1.6当车辆速度小于1米每秒时，采用激光雷达前后帧匹配的方法，得到车辆位姿，完成激光雷达数据从车体坐标系到局部地图坐标变换的转换，并将点云数据栅格化，对局部地形进行更新；
S1.7当车辆在栅格地图中的移动位置dx＞1/2栅格宽度或者dy＞1/2栅格高度时，对局部地形进行平移，将局部地图的中心点位置移动至车辆当前位置。
进一步，所述S2具体流程包括：
S2.1采集车辆当前的运行速度，纵向加速度数据，对当前车辆的运行状态进行判断；
S2.2如车辆运行状态平稳，判定为车辆目前所在位置为一个平面，在局部地图中，以车辆所处位置为起始平面，对三维地面进行地形数据进行平面拟合；
S2.3如车辆运行状态不稳定，判定为车辆目前所在位置不是一个平面，在局部地图中，以上一帧车辆所在位置为起始平面，对三维地面进行地形数据进行平面拟合，并将该平面进行栅格化；
S2.4确定局部地图中的最大高度MHmax，最小高度MHmin，平均高度MHmean；
S2.5以预设阈值为步长，以地面拟合的平面为基准，生成一系列与该平面平行的栅格化之后的基准平面。
进一步，所述S2.5中的预设阈值为±10cm，±20cm，±30cm，±50cm。
进一步，所述S3的具体流程包括：
S3.1构建大小与局部地图相等的，数量与基准平面相同的栅格地图；
S3.2对局部地图进行扫描，对于每一个栅格点，判断其与基准平面的关系，如果该点的高度大于基准平面中该点的高度，则在对应的局部等高地图中，将该点的位置置1，否则置0；
S3.3将每个局部等高地图中的二值图像进行边缘细化，像素连接，平滑，形成闭环的曲线，并计算等高线上每个点的方向。
S3.4将各个独立局部等高线地图进行综合，得到完整的局部等高线地图。
进一步，所述S4包括：
S4.1计算栅格地图中任意一点对应的两条等高线，这两条等高线即为该点高度对应上界等高线和下界等高线；
S4.2计算该点到两条等高线上最近的点，并计算到两个最近点的距离a和距离b；
S4.3该点的地形前进方向表示为theta＝theat1*a/(a+b)+theta2*b/(a+b)；
S4.4该点的坡度大小表示为等高线高度差deltaH/(a+b)，方向表示为theta+pi/2。
进一步，所述S5包括：
S5.1车辆当前位置所在的栅格为(m，n)，车头方向为theta；
S5.2确定扩展步长为1的节点方向：theta，theta+pi/4，theta-pi/4；
确定扩展步长为2的节点方向：theta，theta+atan(0.5)，theta-atan(0.5)；
S5.3确定扩展的栅格位置：
沿着确定的扩展方向分别前进一个步长和两个步长，得到步长为1的3个待扩展位置和步长为2的3个待扩展位置，对上述得到的位置进行栅格化，得到待扩展的栅格的坐标；
S5.4计算待扩展栅格的代价函数：
代价函数＝走过的实际路径长度；
S5.5计算待扩展栅格的启发函数：
计算目标点所在等高线位置，
计算目标点的高度和地形方向；
如果与当前位置处在同一个等高线带：
启发函数＝到目标点的欧几里得距离+Dlen*sin(道路方向与到目标点直线夹角)
Dlen用以调节扩展步长和扩展方向的权值；
如果不在同一个等高线带内：
启发函数＝到目标点的欧几里得距离；
S5.6综合代价函数＝代价函数+启发函数；
S5.7当节点扩展位置离目标点距离不大于Dgoal时，搜索结束，返回搜索结果。
进一步，所述S6包括：
S6.1xi-1，xi，xi+1等表示搜索规划给出的路径点，ΔXi表示从xi指向xi-1的向量，在每一个xi处，有一个前面S5.4计算出的方向rdi；
S6.2在梯度下降优化目标中，增加
S6.3运用梯度下降法对优化目标进行优化。
附图说明
图1为一种等高线引导线的自主车三维路径规划方法实施例的流程图；
图2为一种等高线引导线的自主车三维路径规划方法实施例S1中的T0和T1关系的示意图；
图3为一种等高线引导线的自主车三维路径规划方法实施例S2中的基准平面的示意图；
图4为一种等高线引导线的自主车三维路径规划方法实施例S3中的局部等高线地图；
图5为S4中的目标点与两条等高线的关系示意图；
图6为S5中的A*方法的示意图；
图7为S5中的算法逻辑图；
图8为S6中优化效果示意图。
具体实施方式
下面通过具体实施方式进一步详细说明：
实施例一
本实施例的一种等高线引导线的自主车三维路径规划方法，如图1所示，包括如下步骤：
S1三维地形建模：通过获取传感器的三维数据，利用姿态传感器，将传感器数据从传感器坐标系转换为全局坐标系，根据约定的分辨率进行栅格化，得到栅格化的坐标系，栅格化的坐标系原点随着车辆的运动定期更新；
S2地面提取：对当前车辆的运动状态进行分析，以车辆当前位置所在平面为中心，在栅格化的坐标系中进行地面提取；
S3等高线提取：以车辆当前点提取的平面高度为基准，计算需要提取的等高线数量和高度，利用高度扫描方法，完成多条等高线的单独提取，并对每条等高线进行平滑和连接，在此基础上，将提取得到的多条等高线合成为一个完整的等高线图；
S4可通行性分析：对于地面上的任意目标点，计算该点附近的两条等高线，并计算该点到两条等高线的最近点距离，进而得到该点的地形方向和坡度；
S5局部路径搜索：以A*方法为基础在栅格化的坐标系中进行局部路径搜索，在节点扩展中，以待扩展点的地形方向进行节点生成，在搜索的启发函数中，增加目标点与该点地形处方向的夹角作为约束，得到搜索路径；
S6全局优化：使用梯度下降法对得到的搜索路径进行平滑，在目标函数中，增加路径点方向和该点地形处方向的夹角作为优化指标，得到最终路径。
具体而言，S1包括如下步骤：
S1.1在初始T0时刻，构建以车辆起始位置为中心，以WGS84坐标系为坐标轴方向的局部地图；
S1.2局部地图用栅格地图表示，每个栅格的属性包括最大高度Hmax，最小高度Hmin，，点数Pnum，平均高度Hmean，周围四邻域栅格的平均高度Hneib；
S1.3整个局部地图的属性包括地图中心点的GPS坐标，栅格分辨率，目前车辆所在的栅格位置；
S1.4通过多线激光雷达采集周围环境数据，通过高频IMU测量车辆运动信息；
S1.5当车辆速度大于1米每秒时，采用IMU测量得到的车体姿态数据，完成激光雷达数据从车体坐标系到局部地图坐标变换的转换，并将点云数据栅格化，对局部地形进行更新；
S1.6当车辆速度小于1米每秒时，采用激光雷达前后帧匹配的方法，得到车辆位姿，完成激光雷达数据从车体坐标系到局部地图坐标变换的转换，并将点云数据栅格化，对局部地形进行更新；
S1.7当车辆在栅格地图中的移动位置dx＞1/2栅格宽度或者dy＞1/2栅格高度时(如图2所示)，对局部地形进行平移，将局部地图的中心点位置移动至车辆当前位置。
S2具体包括：
S2.1采集车辆当前的运行速度，纵向加速度数据，对当前车辆的运行状态进行判断；
S2.2如车辆运行状态平稳，判定为车辆目前所在位置为一个平面，在局部地图中，以车辆所处位置为起始平面，对三维地面进行地形数据进行平面拟合(如图3所示)；
S2.3如车辆运行状态不稳定，判定为车辆目前所在位置不是一个平面，在局部地图中，以上一帧车辆所在位置为起始平面，对三维地面进行地形数据进行平面拟合，并将该平面进行栅格化；
S2.4确定局部地图中的最大高度MHmax，最小高度MHmin，平均高度MHmean；
S2.5以±10cm，±20cm，±30cm，±50cm为步长，以地面拟合的平面为基准，生成一系列与该平面平行的栅格化之后的基准平面。
S3包括：
S3.1构建大小与局部地图相等的，数量与基准平面相同的栅格地图(如图4所示)；
S3.2对局部地图进行扫描，对于每一个栅格点，判断其与基准平面的关系，如果该点的高度大于基准平面中该点的高度，则在对应的局部等高地图中，将该点的位置置1，否则置0：
S3.3将每个局部等高地图中的二值图像进行边缘细化，像素连接，平滑，形成闭环的曲线，并计算等高线上每个点的方向。
S3.4将各个独立局部等高线地图进行综合，得到完整的局部等高线地图。
S4包括：
S4.1计算栅格地图中任意一点对应的两条等高线，这两条等高线即为该点高度对应上界等高线和下界等高线。
S4.2计算该点到两条等高线上最近的点，并计算到两个最近点的距离a和距离b(如图5所示)；
S4.3该点的地形前进方向表示为theta＝theat1*a/(a+b)+theta2*b/(a+b)；
S4.4该点的坡度大小表示为等高线高度差de1taH/(a+b)，方向表示为theta+pi/2。
S5包括：
S5.1车辆当前位置所在的栅格为(m，n)，车头方向为theta(如图6所示)；
S5.2确定扩展步长为1的节点方向：theta，theta+pi/4，theta-pi/4；
确定扩展步长为2的节点方向：theta，theta+atan(0.5)，theta-atan(0.5)；
S5.3确定扩展的栅格位置：
沿着确定的扩展方向分别前进一个步长和两个步长，得到步长为1的3个待扩展位置和步长为2的3个待扩展位置，对上述得到的位置进行栅格化，得到待扩展的栅格的坐标；
S5.4计算待扩展栅格的代价函数：
代价函数＝走过的实际路径长度；
S5.5计算待扩展栅格的启发函数：
计算目标点所在等高线位置，
计算目标点的高度和地形方向；
如果与当前位置处在同一个等高线带：
启发函数＝到目标点的欧几里得距离+Dlen*sin(道路方向与到目标点直线夹角)
Dlen用以调节扩展步长和扩展方向的权值；
如果不在同一个等高线带内：
启发函数＝到目标点的欧几里得距离；
S5.6综合代价函数＝代价函数+启发函数；
S5.7当节点扩展位置离目标点距离不大于Dgoal时(如图7所示)，搜索结束，返回搜索结果。
S6包括：
S6.1xi-1，xi，xi+1等表示搜索规划给出的路径点，ΔXi表示从xi指向xi-1的向量，在每一个xi处，有一个前面S5.4计算出的方向rdi(如图8所示)
S6.2在梯度下降优化目标中，增加
S6.3运用梯度下降法对优化目标进行优化。
具体实现时，栅格化的坐标系的相关构建技术，为现有技术。本实施例中，在S2-S4中，进行相关的分析，使得相应的坐标能够有准确的表达；另一方面，是在S5中，利用A*方法的特性，结合栅格化的坐标系的特性，使得本方案能够直接在栅格化的坐标系中进行局部的路径搜索，而不需要额外的预处理。即本方案，利用现有的技术快速的得到栅格化的坐标系，另外利用A*方法需要进行方格化处理，且依赖“父方格”进行分析的特性，使得本方案能够快速的进行局部路径搜索。
在S2中，能够实时的对地面进行提取，并通过S3中，获得完整的等高线图。上述过程中，均未与目标点相关，即在车辆的正常行进过程中，可以一直保持运行，时刻等待响应。
然后是在S4中，对于给定的任意目标点，是通过该点附近的两条等高线，得到该点的地形方向和坡度，然后再利用S5中的A*方法以及栅格化的坐标系进行局部路径搜索，得到搜索路径，实现在获得目标点后的快速响应。最后是在S6中，利用梯度下降法对搜索路径进行平滑处理(其本身计算量非常小)，最后再考虑路径点方向与该点地形处方向的夹角作为优化指标，得到最终路径。
在本实施例中，S1-S3均不需要目标点的参与，节省了在轨迹集合中进行筛选分析的时间，避免了无效分析。本方案中，S1-S3对外界情况进行预处理，在出现目标点后，迅速获得目标点的地形方向和坡度，然后利用A*方法和栅格化的坐标系的特性(结合相关的约束条件)，快速的得到搜索路径和最终路径，达到响应速度快、分析效率高的效果。
以上的仅是本发明的实施例，该发明不限于此实施案例涉及的领域，方案中公知的具体结构及特性等常识在此未作过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本申请给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本申请的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。
