基于非结构化环境下无人压雪车的全覆盖路径规划方法

本发明属于无人压雪车路径规划，具体涉及一种基于非结构化环境下无人压雪车的全覆盖路径规划方法。

背景技术：

1、我国滑雪场逐年递增，参与冰雪运动的人次也逐年显著増加，国人对冰雪运动的热情空前高涨。这为冰雪装备技术的发展带来了新的机遇和挑战。

2、无人驾驶压雪车代替人工驾驶，解决了驾驶员要面对夜间低温、疲劳和高噪声的困难。压雪车由驾驶室和凹凸不平的履带组成，机身前端连接推雪铲，后端用支架牵引雪犁和压雪板，用来打碎和压平雪面，形成的面条雪可以为滑雪者提供舒适的滑雪体验。在无人压雪车作业中，针对滑雪场雪道的路径规划则显得尤为重要。

3、压雪车全覆盖路径规划是指在满足某种性能指标最优的前提下，寻找一条在给定雪道区域内经过所有可达区域并避开所有障碍物的连续路径。

4、通过对现有全覆盖路径规划的分析可知，目前压雪车全覆盖路径规划主要存在以下挑战：基于传统的栅格地图进行路径规划，忽视了地形变化的多方面信息；未考虑将动态障碍物信息融合到多层代价地图进行动态避障；未能充分协调能量消耗、转弯次数、地形变化等路径性能指标。

技术实现思路

1、针对现有技术中存在不足，本发明提供了一种基于非结构化环境下无人压雪车的全覆盖路径规划方法。

2、本发明是通过以下技术手段实现上述技术目的的。

3、基于非结构化环境下无人压雪车的全覆盖路径规划方法：

4、基于无人压雪车顶部3d激光雷达的扫描数据，构建滑雪场雪道的全局点云地图；

5、基于所述全局点云地图拟合点云平面，依据点云平面中激光点的高度变化，进行滑雪场雪道地形检测，将滑雪场雪道地形划分为平坦雪道地形和斜坡雪道地形；

6、在平坦雪道地形下，先沿着压雪车初始工作方向进行封边路径规划，然后沿着工作区域最长边界方向，采用梭形法遍历完平坦雪道的内部工作区域，最后沿着与初始工作方向平行的方向进行封边路径规划；

7、在斜坡雪道地形下，沿着压雪车初始工作方向采用能耗最优的全覆盖路径规划算法进行遍历，所述算法综合考虑路径长度、旋转角度、地形高度变化和压雪车打滑因素，用于获取最优覆盖路径规划；

8、采用改进a*路径规划算法，确定连接当前雪道地形覆盖区域终点到下一不同雪道地形工作区域起点的最优路径，完成所有工作区域的全覆盖路径规划；

9、在平坦雪道地形和斜坡雪道地形路径规划过程中，若遇到障碍物，将基于多层代价地图中的膨胀层进行静态障碍物避障路径规划，基于多层代价地图中的动态层进行动态障碍物避障路径规划。

10、进一步地，所述工作区域最长边界和压雪车初始工作方向是由变分辨率栅格地图获取压雪车工作区域的边界确定的。

11、更进一步地，所述压雪车工作区域边界是将变分辨率栅格地图的所有边界顶点向内缩短0.5倍的压雪车工作覆盖宽度，依次连接缩短后的所有边界顶点，计算每条边界的距离，将最长的边作为工作区域最长边界，将工作区域最长边界的垂直方向视为压雪车初始工作方向。

12、进一步地，所述静态障碍物避障路径规划具体为：压雪车的行驶路径避开膨胀层的边界。

13、进一步地，所述动态障碍物避障路径规划具体为：依据动态层中初始两帧激光雷达扫描数据中动态障碍物的位置和两帧时间间隔，得到动态障碍物的平均速度，结合压雪车工作时间，判断动态障碍物位移曲线与压雪车工作路径的距离，进行动态避障；其中动态障碍物的平均速度vd和压雪车工作路径距离动态障碍物位移曲线的距离d满足：

14、

15、其中：(x0,y0)、(x1,y1)分别代表初始两帧激光雷达扫描数据中动态障碍物的位置，δt代表初始两帧时间间隔，(xc,yc)代表压雪车在工作路径中距离动态障碍物最近的位置，t代表激光雷达第二帧到当前帧的时间；

16、当d＜3米时，压雪车立即停车避障，当d≥3米时，压雪车沿着工作路线正常行驶。

17、进一步地，所述能耗最优的全覆盖路径规划算法，具体为：

18、e(m,n)＝λ1·dt(m,n)+λ2·dr(m,n)+λ3·δh+λ4·δs

19、

20、

21、δh＝mz-nz

22、δs＝ms-ns

23、其中：e(m,n)表示从位姿m到位姿n的能量消耗，m＝(x,y,z,θ)表示压雪车当前位姿，n表示相邻栅格位姿，dt(m,n)表示从位姿m到位姿n的路径长度，dr(m,n)表示从位姿m到位姿n的旋转角度，△h表示从位姿m到位姿n的地形高度变化，△s表示从位姿m到位姿n的滑转率，λ1、λ2、λ3、λ4分别为对应权重系数，mx表示位姿m的x坐标值，nx表示位姿n的x坐标值，my表示位姿m的y坐标值，ny表示位姿n的y坐标值，mz表示位姿m的z坐标值，nz表示位姿n的z坐标值，ms表示位姿m的滑转率，ns表示位姿n的滑转率，mθ表示位姿m的朝向角，nθ表示表示位姿n的朝向角。

24、进一步地，所述多层代价地图基于变分辨率栅格地图进行构建，将变分辨率栅格地图作为多层代价地图的静态层，按照膨胀安全距离将代价地图中的障碍物往外膨胀作为膨胀层，将压雪车前、后部3d激光雷达实时采集的雪道动态环境作为动态层。

25、更进一步地，所述变分辨率栅格地图根据地形检测后滑雪场雪道的全局点云地图生成；变分辨率栅格地图构建过程为：在平坦雪道地形下，将三维点云向地面投影得到二维栅格地图，在斜坡雪道地形下，将地形表面同一栅格内三维激光点的高度平均值作为该栅格的高度，得到2.5d高程栅格地图。

26、更进一步地，根据无人压雪车的工作覆盖宽度，设定变所述分辨率栅格地图中每个栅格的长/宽度均等于无人压雪车工作覆盖宽度。

27、进一步地，所述改进a*路径规划算法引入新的启发函数、转弯代价函数和自适应权重来选择最优路径规划，具体为：

28、fn＝gn+ω′hn+αtn

29、

30、hmd＝∣xn-xg∣+∣yn-yg∣

31、

32、ω′＝(1+dn/d)

33、α＝(1-nobs/m)

34、其中：fn为起点到目标点的总代价函数，gn为起点到当前点(xn，yn)的实际代价，hn为当前点到目标点(xg，yg)的启发函数，tn为当前点转弯的代价函数，mdx、mdy分别代表变分辨率栅格地图中x轴、y轴的曼哈顿距离，hmd代表曼哈顿距离，k为常系数，ω'为启发函数的权重系数，α为压雪车转弯代价函数的权重系数，dn为当前节点到目标点的距离，d为起点到目标点的距离，nobs为搜索路径过程中搜索到障碍物的数量，m为栅格地图中的栅格总数，xn表示当前点的x坐标值，yn表示当前点的y坐标值，xg表示目标点的x坐标值，yg表示目标点的y坐标值，θn表示当前点的转弯角度。

35、本发明的有益效果为：

36、1.本发明通过lio-sam算法构建滑雪场雪道全局点云地图，依据全局点云地图生成变分辨率栅格地图，采用变分辨率栅格地图不仅降低了地形描述占用的存储空间，也保留了雪道的地形变化，为后续斜坡雪道全覆盖能耗最优的路径选择奠定了基础。

37、2.本发明根据变分辨率栅格地图生成多层代价地图，利用多层代价地图中的动态层进行动态障碍物的避障规划，动态路径规划使得无人压雪车能够在复杂障碍物环境中进行工作，提高了路径规划的安全性。

38、3.本发明在平坦雪道采用梭形法和封边规划进行遍历，在斜坡雪道采用能耗最优的全覆盖路径规划算法，利用改进a*算法连接当前区域终点到下一工作区域起点，完成所有雪道的全覆盖路径规划；本方法提升了无人压雪车的覆盖率，减少了全覆盖工作时的能量消耗。