一种改进型D*Lite车辆动态路径规划方法与流程

本发明属于无人驾驶汽车路径规划领域，涉及一种改进型d*lite车辆动态路径规划方法。

背景技术：

无人驾驶车辆的研究起源于20世纪60年代，通过车身传感器信息和环境信息，在有障碍物的环境中，完成定位、避障、目标寻找等一系列自动操作过程，是近年来的国内外学界和企业界研究热点。无人驾驶技术主要包括环境感知，路径规划，运动控制三个部分。其中路径规划是无人驾驶技术中十分关键的组成部分，为环境感知和运动控制起着承上启下的作用。基于感知系统的环境数据，车辆需要在复杂的道路环境中，基于一定的性能指标规划出一条安全可靠的、车辆可行驶的、从起始位置到目标位置的最短无碰撞路径。

无人车路径规划中常见的基于启发式的搜索算法有a*类，d*类算法，a*类算法适用于静态环境，d*类算法适用于动态环境。无人车所处的环境时刻在变化，这就要求路径重新规划的速度足够快，以满足车辆行驶实时性的要求，这使得a*类算法不能满足实时路况下车辆重新规划的要求。d*类算法是基于a*类算法的改进，在环境发生变动时会利用先前的搜索信息来提高本次搜索效率，与重复使用a*类算法进行重规划相比，搜索效率更高，所以d*类算法更适合解决无人车的动态路径规划问题。d*类算法中d*lite算法原理简单，容易实现，更适合用于无人车的动态路径规划中。

然而，在栅格化环境中，如果直接将a*类、d*类算法应用于车辆路径规划中，则只能将车辆看成一个点，利用四邻域或者八邻域的搜索方法搜索路径，这样生成的路径不平滑，不受车辆运动学的限制，常常导致路径不可行驶。

中国专利cn104590259公开了一种基于车辆运动学模型的a*算法，列出了车辆运动学方微分方程，这样在使用a*算法搜索路径时就考虑了车辆的运动学模型，最终得到的路径一定是可行的。但是该专利所使用的方法只能应用于静态环境中，如果环境发生变动，则需要遍历整张地图重新规划，不适合动态环境下的规划，而无人车所处的环境又是在不断变化的，所以该算法并不适合无人车的实时路径规划。另外，该专利提出的方法的由于只使用了车辆运动学模型，所以到达终点时车辆的朝向角只能近似到达目标状态，误差取决于朝向角分辨率的大小，这在非结构化环境下(如停车场)是不实用的。

技术实现要素：

本发明的目的在于克服上述现有技术的缺点，提供了一种改进型d*lite车辆动态路径规划方法，该方法能够在动态的环境下实现路径的规划。

为达到上述目的，本发明所述的改进型d*lite车辆动态路径规划方法包括以下步骤：

1)获取车辆当前状态及目标状态，并获取当前环境地图信息，再使用形态学操作对当前环境地图中的障碍物进行膨胀；

2)构建车辆运行学模型；

3)基于车辆运动学模型的限制，使用d*lite算法在膨胀后的环境地图上生成最短路径；

4)根据reeds-shepp曲线对到达终点时车辆的朝向角进行调整；

5)根据步骤3)生成的最短路径及步骤4)中的reeds-shepp曲线构建最终路径，然后使用能量函数及梯度下降法对最终路径进行平滑，完成改进型d*lite车辆动态路径规划。

步骤2)的具体操作为：

设车辆限制在平面运动上，且轮子不会在地面上打滑，则车辆运行学模型为：

其中，x为车辆运动状态，(x,y)为车辆的位置坐标，θ为车辆的航向角，v为车辆的速度，δ为车辆前轮的转角，l为车辆前轴与后轴之间的距离，及分别对应为x、y及θ的一阶求导。

步骤5)中的能量函数为：

其中，n为路径点的数量，δxi+1＝xi+1-xi,δxi＝xi-xi-1。

d*lite算法扩展节点时，根据车辆运动学模型生成不同位置及不同方向的子节点，然后将生成的子节点插入于d*lite的开列表中。

本发明具有以下有益效果：

本发明所述的改进型d*lite车辆动态路径规划方法在具体操作时，基于车辆运动学模型的限制，使用d*lite算法在膨胀后的环境地图上生成最短路径，从而在动态环境中快速找到一条最短路径，另外，本发明根据reeds-shepp曲线对到达终点时车辆的朝向角进行调整，实现即将达到终点时车辆朝向角的调整，再利用平滑函数及梯度下降法对最终路径进行平滑，完成动态环境下车辆实时准确的路径规划，操作方便、简单。

附图说明

图1为本发明的流程图；

图2为本发明中基于车辆运动学模型的d*lite扩展节点图；

图3为使用d*lite和reeds-shepp曲线生成的路径图；

图4为使用平滑路径函数前后路径的对比图。

具体实施方式

下面结合附图对本发明做进一步详细描述：

参考图1，本发明所述的改进型d*lite车辆动态路径规划方法包括以下步骤：

1)获取车辆当前状态及目标状态，并获取当前环境地图信息，再使用形态学操作对当前环境地图中的障碍物进行膨胀；

2)构建车辆运行学模型；

步骤2)的具体操作为：

设车辆被限制在平面运动上，且轮子不会在地面上打滑，则有车辆运行学模型为：

其中，x为车辆运动状态，(x,y)为车辆的位置坐标，θ为车辆的航向角，v为车辆的速度，δ为车辆前轮的转角，l为车辆前轴与后轴之间的距离，及分别对应为x、y及θ的一阶求导。

3)基于车辆运动学模型的限制，使用d*lite算法在膨胀后的环境地图上搜索路径；

根据车辆运动学模型的限制，对d*lite算法进行改进，该d*lite算法的输入是一张障碍物膨胀后的栅格化地图、车辆起始位姿xs＝(xs,ys,θs)及目标位姿xg＝(xg,yg,θg)输出是一条满足车辆运动学模型限制的路径，该路径由一系列连续的车辆位姿表示。

d*lite算法的启发式搜索函数为：

f(p)＝g(p)+h(p)

其中，f(p)为该节点的代价值，g(p)为起点到任意节点n的最小代价值，h(p)为到目标节点n的代价启发值。

d*lite的开列表中存放有规划路径上每个节点到目标节点的最短路径信息、每个节点的父节点、f(p)、g(p)、h(p)以及父节点到当前节点的轨迹。

当从父节点扩展子节点时，常规的方法每个节点只包含车辆的坐标信息，以父节点(x,y)为中心，往上、下、左及右4个方向或上、下、左、右、左上、左下、右上及右下8个方向扩展子节点(x’,y’)，最终搜索得到的路径没有考虑车辆运动学模型，可能会出现急转、原地掉头等车辆不可能做出的行为。

而考虑车辆运动学模型的限制扩展子节点时，每个节点包含车辆的坐标信息及朝向信息，从父节点(x,y,θ)开始，以固定的速度按不同的转向角经过一定时间，到达的所有下一节点(x’,y’,θ’)进行扩展，最终搜索得到的路径满足车辆运动学模型，车辆可以直接行驶。

具体搜索过程为：

从当前节点开始，以车辆运动学模型前进和后退行驶一段固定时间，转向角越多，则所产生的状态也就越多，得到的路径也越平滑。转向角太少，则会导致找不到路径，转向角太多会导致计算量增加，因此选取适中的转向角数量很重要。根据实际中车辆的最大转向角，如图2所示，使用10个转向角，即车辆前进和后退各五个转向角(-35°-20°0°20°35°)，经过一段固定时间后的车辆状态。

扩展完一个节点之后，判断其生成的轨迹上有没有障碍物，若没有障碍物，则保留该条路径，并将这条路径的终点状态插入open表中，若有障碍物，则舍弃。

因此每扩展一个节点都会获得若干新的节点，而节点与节点之间的路径则是根据车辆运用学模型产生，这样使用d*lite算法搜索到某点时，只要沿着该点倒着寻找其父节点，再寻找父节点的父节点，以此类推，直到起点停止。所有路径点确定之后，再找出从父节点到子节点的路径，即可生成一条在车辆运动学模型限制下的路径。

离散d*lite中，搜索时把所扩展的栅格插入到open表中，但在车辆运动学模型限制下的d*lite中，由于位置坐标是连续的，可以出现在栅格中的任意位置，如果不做处理直接插入open链表中时，则会有无穷多种状态，因此需要设置位置分辨率及朝向分辨率，则在同一分辨率下的两个节点视为同一种状态。

4)根据reeds-shepp曲线对到达终点时车辆的朝向角进行调整；

在d*lite算法搜索路径到接近终点区域时，使用reeds-shepp曲线将d*lite的搜索点与终点之间进行连接。

当仅用d*lite算法搜索到终点时，由于搜索过程中不考虑终点的朝向，车辆不能以正确的姿态驶入终点，这对于停车场这种对角度特别敏感的环境就不适合；在终点附近区域内引入reeds-shepp曲线，能有效解决接近终点时朝向角问题。reeds-shepp曲线在车辆能够倒车且最小转弯半径及速度确定的情况下，可以生成两个位姿(x1,y1,θ1)和(x2,y2,θ2)之间的最短路径。

当d*lite搜索节点进入reeds-shepp路径搜索区域时，从当前节点生成到终点的reeds-shepp曲线，若该曲线上没有障碍物，则reeds-shepp路径生成成功；若该曲线上有障碍物，则reeds-shepp路径生成失败，则从当前节点扩展子节点，从子节点继续生成到终点的reeds-shepp曲线，如此循环，直到该曲线上没有障碍物，路径生成成功为止。在扩展子节点时增加转向角度的个数，以生成更多的子节点状态，增大寻找到reeds-shepp路径的可能性。

则最终路径由d*lite路径与reeds-shepp曲线拼接而成。图3为最终路径示意图。

5)根据步骤3)生成的最短路径及步骤4)中的reeds-shepp曲线构建最终路径，然后使用能量函数及梯度下降法对最终路径进行平滑，完成改进型d*lite车辆动态路径规划。

能量函数为：

其中，n为路径点的数量，δxi+1＝xi+1-xi,δxi＝xi-xi-1。

采用梯度下降方法，使能量分摊到每个路径点上；先求每个路径点的一阶导数：

然后选定一个常数μ，计算：

重复上述两个循环，直到前一次能量函数的值和后一次能量函数的值相减的绝对值小于预设值ε为止。

路径平滑前后的对比如图4所示。可以看出虽然原始路径是基于车辆运动学模型的，但是局部的波动震荡比较多，并不符合人在驾驶车时的行为。而平滑过的路径没有局部震荡，曲线变得很平滑。

另外，车辆按照生成的轨迹行驶，当环境信息变化时，如果新地图中路径上仍没有障碍物，原路径仍然可行，则不重新进行路径的规划；如果原定的路径现在不可行，则进行d*lite算法，重新找到一条无碰撞的路径，以快速完成实时规划与动态避障。