一种基于高精地图的自动驾驶矿用车辆的速度规划方法

本发明涉及自动驾驶矿车运动规划，具体涉及一种基于高精地图的自动驾驶矿用车辆的速度规划方法。

背景技术：

1、目前，露天矿区的矿用车辆多采用按传统高精地图行驶的方法，其路径信息中速度限制信息的设置过程大多为在路径采集后，人工根据道路状况及驾驶经验对速度限制信息进行标定。然而，人工标定速度限制信息存在以下问题：(1)人工标定速度限制信息耗费人力资源且用时较长，在不同的非结构化道路条件下，均需要重新依靠道路状况与经验标定，且在自动驾驶矿车运输完成后可能评价标定效果不佳，需要再次重新标定和跑车验证；(2)人工标定速度限制信息受主观因素影响，难以保证一致性，并且可能出现数据错漏等问题；(3)人工标定一般为大段路径设置相同限速值，标定出的速度限制信息为阶跃速度曲线，不易于自动驾驶矿用车辆进行速度跟随。

2、中国专利cn113593022a实现了三种地图的联动效果，适用于复杂露天矿山环境中提取出矿区地面、周边山体以及碎石堆等道路，但该发明未涉及高精地图中行驶路径限速信息的标定或生成，缺少相关方法，不能对高精地图限速进行设定；同时，自动生成速度限制信息时，又存在因运算量过大而导致求解时间过长影响效率的问题；中国专利cn116142230a，根据预置的障碍物st风险场和速度决策结果，得到速度规划st曲线，但该发明高度依赖车辆的环境感知模块，需要通过感知模块获取的大量环境信息，需要较大运算量，影响求解效率；且该发明未考虑道路曲率、坡度等约束，未能充分保障自动驾驶运输矿用车辆行驶安全。

技术实现思路

1、鉴于上述问题，本发明提供了一种基于高精地图的自动驾驶矿用车辆的速度规划方法，通过改善自动驾驶车辆行驶速度的跟随效果，即，车辆运行过程中的实际车速要尽可能与其期望车速(即算法中的速度上限)一致(即令实际车速跟随期望车速)，进而提高其行驶安全性与效率，节约了人力成本和算力。

2、一种基于高精地图的自动驾驶矿用车辆的速度规划方法，包括：

3、s1、获取高精地图中原始路径信息文件，提取原始路径信息中路径点信息，对路径点信息进行有效性检查，获得正确的路径点信息；优选的，所述有效性检查具体为：判断路径点信息正确与否，若路径点信息错误，则输出错误信息；若路径点信息正确，则将正确的路径点信息进步骤s2计算；

4、优选的，所述路径点信息为包含12列路径数据信息的文本文件，分别为路径点经度、纬度、高程、曲率、坡度、最大纵向加速度、最大纵向减速度、最大侧向加速度、加加速度上界、加加速度下界、速度上界和速度下界；

5、所述速度上界为矿区限制自动驾驶矿车最高行驶速度。

6、优选的，所述有效性检查的检查规则具体包括：

7、a，检查路径点信息是否完整，即传入的路径点信息中是否均包含所述12列路径数据信息，若路径点信息中缺少某列路径数据信息，则输出该路径点信息索引以及其所缺路径数据信息；

8、b，检查路径点信息是否正确，即传入的路径点信息中12列路径数据信息是否满足检查条件，若不满足检查条件，则输出该路径点信息以及其不满足的路径数据信息。

9、所述检查条件为：

10、路径点信息中速度、加速度以及加加速度约束信息是否合理；

11、进一步的，所述加速度约束信息为路径点信息中包含的最大纵向加速度、最大纵向减速度、最大侧向加速度、加加速度上界、加加速度下界、速度上界、速度下界。

12、进一步的，所述检查条件具体包括：

13、路径点曲率的绝对值不大于曲率上界cub，且前后两个路径点曲率的差值的绝对值不大于曲率变化上界ccb；

14、坡度不大于坡度上界sub，并不小于坡度下界slb，且前后两个路径点坡度的差值的绝对值不大于坡度变化上界scb；

15、最大纵向加速度大于0，最大纵向减速度小于0；

16、最大侧向加速度应大于0；

17、加加速度上界大于0，加加速度下界小于0；速度上界、速度下界均大于0，且速度上界大于速度下界。

18、s2、获取步骤s1所述正确的路径点信息的坐标信息，将所述坐标信息进行转换，获得更新坐标系；计算各路径点与其前一路径点的间距，获得间距数据集；优选的，坐标信息进行转换的具体方法为：

19、将第i个路径点的经纬高坐标转换为东北天坐标，第i个路径点的东北天坐标(xi,yi,zi)为：

20、xi＝(re+alti)cos(lati)cos(loni)

21、yi＝(re+alti)cos(lati)sin(loni)

22、zi＝(re+alti)sin(lati)

23、其中，re表示地球半径，取6378137m；alti表示第i个路径点高程；alti表示第i个路径点纬度，以赤道为基准，向北为正称为北纬(0～90°)，向南为负称为南纬(0～-90°)，纬度值域为(-90°～90°)；loni表示第i个路径点经度，以本初子午线为基准，向东为正称为东经(0～180°)，向西为负称为西经(0～-180°)，实际表示用(-180°～180°)；alti表示第i个路径点高程，表示该点沿其法线到球面的距离，向上为正，向下为负；i＝0,1,2,…,n，n表示路径点总数。

24、各路径点与其前一路径点的间距表达式为：

25、

26、其中，di表示第i个路径点与第(i+1)个路径点之间的距离，将起点间距记为0，即d0＝0。

27、本发明技术方案中将坐标信息进行转换，高精地图中的行驶路径一般按较小单位间隔均匀采样，然而同一矿区内路径点间距较小，所以在进行坐标转换时可以将地球近似看作一个标准球体以简化计算。

28、s3、基于更新坐标系和间距数据集，以及步骤s1所述正确的路径点信息获得路径点速度上界；

29、优选的，所述路径点速度上界表达式为：

30、vi,max＝max(min(vi,lmt,vi,c,vi,s,vi,ta),vi,min)

31、其中，vi,max表示第i个路径点速度上界，m/s；vi,lmt表示矿区限制最高行驶速度，为路径信息读取与写入模块的原始路径信息中速度上界，m/s；vi,c表示第i个路径点曲率约束速度，m/s；vi,s表示第i个路径点安全制动约束速度，m/s；vi,ta表示第i个路径点最大允许前轮转角变化率约束速度，m/s；vi,min表示第i个路径点速度下界(此处取最大值为保证速度上界大于速度下界)，m/s。

32、进一步的，第i个路径点曲率约束速度表达式为：

33、

34、其中，ai,lateral表示第i个路径点最大侧向加速度；ri表示第i个路径点曲率半径，其值为第i个路径点曲率绝对值的倒数。

35、第i个路径点安全制动约束速度表达式为：

36、

37、其中，ai,min表示第i个路径点最大纵向减速度(为负值)，m/s2；ai,s表示第i个路径点坡度加速度，m/s2；dsensor表示车辆对障碍物的有效感知距离，m；dstop表示车辆停障安全距离，即车辆停车后与障碍物之间的安全距离，m；τ表示车辆制动响应时间，s。

38、更进一步，当路径点曲率变化时，限定行驶中车辆的车速以确保车辆前轮转角满足最大允许前轮转角变化率约束，表达式为；

39、

40、其中，ωmax表示最大允许前轮转角变化率，rad；l表示车辆轴距，m；ci表示第i个路径点曲率，1/m，ci-1表示第i-1个上一路径点曲率；将起点最大允许前轮转角变化率约束速度记为其矿区限制最高行驶速度，即记v0,ta＝v0,lmt。

41、s4、基于步骤s3所述路径点速度上界对路径整体速度上界进行平滑，获得平滑后的路径点速度上界；优选的，所述平滑的具体步骤为：

42、按照路径点速度上界对路径进行分级，获得路径点路径等级和分级后的路径点；一个实施例为，所述路径点路径等级与路径点速度上界关系如下：

43、1级路径：vi,max≥11.11m/s(40km/h)；

44、2级路径：9.72m/s(35km/h)≤vi,max<11.11m/s(40km/h)；

45、3级路径：8.33m/s(30km/h)≤vi,max<9.72m/s(35km/h)；

46、4级路径：6.94m/s(25km/h)≤vi,max<8.33m/s(30km/h)；

47、5级路径：5.56m/s(20km/h)≤vi,max<6.94m/s(25km/h)；

48、6级路径：4.17m/s(15km/h)≤vi,max<5.56m/s(20km/h)；

49、7级路径：2.78m/s(10km/h)≤vi,max<4.17m/s(15km/h)；

50、8级路径：1.39m/s(5km/h)≤vi,max<2.78m/s(10km/h)；

51、9级路径：vi,max<1.39m/s(5km/h)。

52、将分级后的路径点进行分段，将相邻且路径等级相同的路径点划为同一路段，获得分段后的路径；

53、将分段后的路径按照从路径起点到路径终点的顺序进行路段标号，获得标号路段；

54、检查各标号路段长度；若所有路段长度均大于等于路段长度阈值k，则分段完成，获得检查后的标号路段；若某一路段长度小于路段长度阈值k，则将其并入前一路段，特别地，若长度小于路段长度阈值k的路段为首段，则并入后一路段，获得合并后的标号路段，将合并后的标号路段对应的路径点路径等级记为该标号路段中所有路径点路径等级中最大值(即速度上限最小的路径点对应的路径等级)，同时获得更新路段标号，重复进行标号路段长度检查，获得检查后的标号路段；

55、按照各路径点路径等级对应的速度最小值对路径内所有路径点速度上界进行赋值获得平滑后的路径点速度上界。

56、s5、基于步骤s1输出的正确的路径点信息和步骤s4所述平滑后的路径点速度上界制定优化模型目标函数的约束条件；

57、所述优化模型的目标函数表达式为：

58、

59、其中，ai表示第i个路径点加速度，反映控制投入，m/s2；jerki表示第i个路径点加加速度，反应车辆纵向运动平稳性，m/s2；(vi,max-vi)表示第i个路径点速度上界与第i个路径点速度行驶速度的差值，反映行驶效率，m/s；wa表示加速度投入量在目标函数中对应的权重值，wjerk表示加加速度投入量在目标函数中对应的权重值，wrefer表示速度上界与目标行驶速度的差值在目标函数中对应的权重值。

60、所述约束条件为：

61、

62、其中，ai,min表示最大纵向减速度，m/s2；ai,max表示最大纵向加速度，m/s2；jerki,min表示加加速度下界，m/s3；jerki,max表示加加速度上界，m/s3；vi,min表示速度下界，m/s。

63、s6、基于约束条件求解所述优化模型的目标函数，获得路径点最优速度上界。

64、本发明的另一个实施例，还包括

65、步骤s7、在求解完成后，将所述路径点最优速度上界传回步骤s1所述原始路径信息文件，获得优化路径信息文件，将所述优化路径信息文件传回高精地图，获得更新高精地图；

66、基于所述更新高精地图对下一时刻自动驾驶车辆的速度进行规划。

67、与现有技术相比，本发明至少具有现如下有益效果：

68、(1)本发明根据路径信息自动生成路径限速，无需人工设置，减少时间和人力成本的浪费；

69、(2)本发明避免因人工标定速度上限产生的错标、漏标问题，并且路径限速为连续变化曲线而非阶跃曲线，便于自动驾驶矿用车辆进行速度跟随保证自动驾驶矿用车辆行驶安全；

70、(3)本发明生成易于自动驾驶矿用车辆进行速度跟随的平滑速度上界曲线，减少纵向控制超调以及纵向偏差过大的问题；

71、(4)本发明考虑由坡度引起的加速度，减少车辆自身控制投入量；

72、(5)本发明算法简单，无需大量复杂计算，节约算力，适用于自动驾驶矿用车辆工况。