一种面向无人驾驶车辆的三维参考路径规划方法

本发明涉及路径规划，尤其涉及一种面向无人驾驶车辆的三维参考路径规划方法。

背景技术：

1、越野环境下无人驾驶车辆的路径规划，对于保证车辆的稳定驾驶和无人作战意义重大。

2、路径规划应该能根据感知系统输入的越野环境信息，综合考虑环境 约束和车辆特性，生成从当前车辆位置到指定目标位置的无碰撞、车辆控制算法可跟踪、安全平稳的路径。然而，大部分基于搜索、采样和优化的路径规划方法多集中于结构化道路或路面条件良好的城市环境。

3、目前无人驾驶车辆常见的路径规划方法，基于平面二值栅格地图，仅考虑障碍物约束，避障的同时只追求路径长度或时间最短，没有考虑地形信息和车辆姿态，无法保证规划路径的可通行性。

技术实现思路

1、鉴于上述的分析，本发明实施例旨在提供一种面向无人驾驶车辆的三维参考路径规划方法，用以解决现有规划路径的可通行性不高的问题。

2、本发明实施例提供了一种面向无人驾驶车辆的三维参考路径规划方法，包括以下步骤：

3、采集越野环境的图像数据和雷达点云数据，建模得到分层地图，分层地图包括：障碍物层、高度层和路面语义层；

4、采用改进的算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径；

5、基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点；

6、对优化后的路径节点拟合并投影到高度层，得到三维参考路径。

7、基于上述方法的进一步改进，根据分层地图，获取路径节点的动态扩展步长，包括：

8、将当前路径节点作为父节点，根据父节点的位置和预置的步长范围，获取在障碍物层的最大扩展步长，根据最大扩展步长，生成父节点的二维子节点；

9、将二维子节点投影到高度层，得到三维子节点，根据三维子节点与父节点间的单位攀升高度，及障碍物层的最大扩展步长，得到父节点在各个三维子节点方向上的动态扩展步长。

10、基于上述方法的进一步改进，通过以下公式得到父节点在各个三维子节点方向上的动态扩展步长：

11、，

12、其中，表示父节点向第 j个三维子节点方向扩展的步长，表示父节点在障碍物层的最大扩展步长，表示第 j个三维子节点与父节点间的单位攀升高度，表示预置的最大曲面曲率值。

13、基于上述方法的进一步改进，根据父节点的位置和预置的步长范围，获取障碍物层的最大扩展步长，包括：

14、基于障碍物层，根据父节点的位置，获取父节点离障碍物的最近距离；当最近距离在预置的步长范围内，则最近距离作为最大扩展步长；当最近距离小于预置的步长范围的最小步长，则取最小步长为最大扩展步长；当最近距离大于预置的步长范围的最大步长，则取最大步长为最大扩展步长。

15、基于上述方法的进一步改进，根据最大扩展步长，生成父节点的二维子节点，是在以父节点为圆心，以最大扩展步长为半径的圆周上，根据预置的间隔角度生成均匀分布的二维子节点。

16、基于上述方法的进一步改进，代价函数包括移动代价和启发项代价；其中移动代价包括父节点的移动代价、父子节点间的欧式距离、父子节点间的静态稳定性代价和子节点的路面语义代价；启发项代价是子节点与终点间的欧式距离；子节点是父节点根据动态扩展步长扩展的子节点且满足平面无碰撞和静态稳定性的约束条件。

17、基于上述方法的进一步改进，父子节点间的静态稳定性代价通过以下公式计算得到：

18、，

19、其中，表示第 i个子节点，表示父节点；表示第 i个子节点与父节点间的俯仰角差值绝对值，表示第 i个子节点与父节点间的侧倾角差值绝对值，表示第 i个子节点与父节点间的单位攀升高度。

20、基于上述方法的进一步改进，子节点的路面语义代价是根据无人驾驶车辆在子节点处所占区域，计算出区域内各路面类型所占比例，分别与对应的权重相乘后再求和而得到；路面类型包括：土路、草地和灌木。

21、基于上述方法的进一步改进，基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点，包括：

22、在每次迭代优化中，将初始路径中除起点和终点以外的路径节点作为待优化节点，依次对每个待优化节点执行以下更新操作，获取更新后的路径长度：获取待优化节点在其前后相邻的路径节点连线上的投影点；从投影点至其对应的待优化节点的连线上根据地图最小分辨率生成等间距的多个待调整节点；依次将每个待调整节点投影到高度层，将第一个满足平面无碰撞和静态稳定性的约束条件的三维待调整节点更新为对应的待优化节点，如果不存在满足平面无碰撞和静态稳定性的约束条件的三维待调整节点，则不更新对应的待优化节点；

23、当更新后的路径长度小于前一次迭代优化的长度且长度缩短率小于优化阈值，或者达到最大迭代次数时，结束迭代优化。

24、基于上述方法的进一步改进，平面无碰撞的约束条件是路径节点离障碍物的最近距离大于无人驾驶车辆半径；静态稳定性的约束条件是无人驾驶车辆在路径节点处的俯仰角和侧倾角均小于对应的角度阈值。

25、与现有技术相比，本发明至少可实现如下有益效果之一：

26、1、在规划过程中引入分层地图，考虑环境中地形的高度和路面语义信息，将原有基于平面二值栅格地图的局部路径规划算法推广到三维空间，提升了路径规划所能获取的环境信息。

27、2、改进代价函数，引导规划过程尽量避开地形起伏和难以行驶的灌木区，同时考虑越野环境下的无碰撞约束和车辆的静态稳定性约束，提升了路径的可通行性，保证车辆的安全与高效通行。

28、3、引入无梯度迭代优化方法，减小了路径的松弛度。

29、本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。

技术特征：

1.一种面向无人驾驶车辆的三维参考路径规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据分层地图，获取路径节点的动态扩展步长，包括：

3.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，通过以下公式得到父节点在各个三维子节点方向上的动态扩展步长：

4.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据父节点的位置和预置的步长范围，获取障碍物层的最大扩展步长，包括：

5.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据所述最大扩展步长，生成父节点的二维子节点，是在以父节点为圆心，以最大扩展步长为半径的圆周上，根据预置的间隔角度生成均匀分布的二维子节点。

6.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述代价函数包括移动代价和启发项代价；其中移动代价包括父节点的移动代价、父子节点间的欧式距离、父子节点间的静态稳定性代价和子节点的路面语义代价；启发项代价是子节点与终点间的欧式距离；所述子节点是父节点根据动态扩展步长扩展的子节点且满足平面无碰撞和静态稳定性的约束条件。

7.根据权利要求6所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述父子节点间的静态稳定性代价通过以下公式计算得到：

8.根据权利要求6所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述子节点的路面语义代价是根据无人驾驶车辆在子节点处所占区域，计算出区域内各路面类型所占比例，分别与对应的权重相乘后再求和而得到；所述路面类型包括：土路、草地和灌木。

9.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点，包括：

10.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述平面无碰撞的约束条件是路径节点离障碍物的最近距离大于无人驾驶车辆半径；所述静态稳定性的约束条件是无人驾驶车辆在路径节点处的俯仰角和侧倾角均小于对应的角度阈值。

技术总结
本发明涉及一种面向无人驾驶车辆的三维参考路径规划方法，属于路径规划技术领域，解决了现有技术中规划路径的可通行性不高的问题。包括：采集越野环境的图像数据和雷达点云数据，建模得到分层地图，所述分层地图包括：障碍物层、高度层和路面语义层；采用改进的A‑star算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径；基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点；对优化后的路径节点拟合并投影到高度层，得到三维参考路径。实现了路径的安全与高效通行。

技术研发人员：刘海鸥,关海杰,陶俊峰,陈慧岩
受保护的技术使用者：北京理工大学
技术研发日：
技术公布日：2024/1/12