一种融合车辆特性的三维越野路径规划方法和装置与流程

本发明涉及无人驾驶，特别是涉及一种融合车辆特性的三维越野路径规划方法和装置。

背景技术：

1、随着人工智能、机器学习、自主导航技术、虚拟和现实增强技术、元宇宙等新型科学技术的不断迭代发展及其更迫切减少作战人员伤亡的需求，陆地无人作战平台已投入了应用并在军事战场中发挥了极其重要的作用。

2、当前无人驾驶在城市及高速公路等结构化道路上已初具规模，随着高精地图(hd-map)、组合惯性导航(gpsins/gnssins)以及地形重构(slam)等技术产业的崛起，这类情形下的交通环境的信息和车辆在整个地图中的确切位置可以很容易确定，环境相对简单。

3、但是，在岩石、沙滩、山地、森林湖泊丘陵等复杂越野环境下的非结构化道路上，无人驾驶要考虑更多的因素，仍缺乏有效的路径规划方案。

技术实现思路

1、本发明的目的在于提供一种融合车辆特性的三维越野路径规划方法和装置，至少部分解决上述技术问题。

2、为实现上述目的，本发明一方面提供一种融合车辆特性的三维越野路径规划方法，包括：

3、步骤一，将包括初始点和目标点的三维点云环境地图的数据结构转化为八叉树结构；

4、步骤二，设置车辆的初始姿态和目标姿态，其中姿态信息包括水平横纵坐标以及竖直坐标、横摆角、侧倾角和俯仰角；

5、步骤三，根据偏置采样概率法确定下一路径节点xi处车辆的控制输入速度；

6、步骤四，通过下述迭代公式得到下一节点xi处车辆的位置信息和横摆角：

7、

8、φi-＝φi-1+ωiδt

9、其中，xi和yi是节点xi处车辆的横向和纵向位置坐标，φi表示横摆角，φi-表示地形信息更新前的横摆角度状态；

10、i是节点索引，δt是控制输入的时间周期；

11、速度νi和角速度ωi由步骤三得到的节点xi处车辆的控制输入速度得到；

12、上标“-”表示地形信息更新前的横摆角度状态；

13、步骤五，根据地形信息确定节点xi处车辆的高度z、侧倾角ψ和俯仰角γ：

14、zi＝zmax＝max(zk),k＝1,2...n

15、其中，k为索引，n为节点xi的矩形区域内单元体的总数，max(zk)为所有单元体的高度值中的最大值；

16、

17、其中，和分别为节点xi和xi-1的坡度角，和分别为节点xi和xi-1的坡向角；

18、步骤六，通过预设约束条件排除障碍节点；

19、步骤七，重复步骤三至六，直到得到全部路径节点以及每一路径点处车辆的姿态信息。

20、优选的，步骤一包括将三维点云环境地图的数据结构转化为octomap数据格式。

21、优选的，步骤三包括：

22、应用下列概率分布函数p(v)对节点xi的线性控制速度νi进行偏抽样：

23、

24、f2(v)＝puni(v)

25、其中，f1(v)表示速度的高斯分布，vu为平均速度，vσ为预设的偏抽样参数；f2(v)表示速度的均匀分布；

26、使用均匀概率分布函数对节点xi的角速度ωi进行采样。

27、优选的，步骤六中约束条件包括octomap干扰检查，包括：

28、从地图数据中获取障碍点位置信息；

29、对于任一节点xi，根据车辆本身尺寸参数判断车辆在节点xi是否受到障碍点干扰；

30、如果是，排除节点xi。

31、优选的，步骤六中约束条件包括车辆特性检查，包括：

32、获取车速与车身最大侧倾角度以及最大俯仰角的对应关系；

33、根据当前车速确定当前车辆最大侧倾角度以及俯仰角；

34、如果节点xi处的车身侧倾角度大于最大侧倾角度或者俯仰角大于最大俯仰角，则排除节点xi。

35、优选的，车辆特性检查还包括：

36、获取车辆速度vi与ωi的比值，如果比值小于最小转弯半径，则排除节点xi。

37、优选的，车辆特性检查还包括：

38、计算节点xi处车辆所处位置与下一待扩展节点之间的第一坡度角以及与父节点之间的第二坡度角；

39、如果第一坡度角大于等于车辆的接近角，或者第二坡度角大于车辆的离去角，则排除节点xi。

40、优选的，步骤六中约束条件还包括：

41、计算节点xi处的坡度角；

42、如果坡度角大于车辆最大爬坡度，则排除节点xi。

43、本发明另一方面提供一种融合车辆特性的三维越野路径规划装置，包括：

44、地图数据处理模块，用于将包括初始点和目标点的三维点云环境地图的数据结构转化为八叉树结构；

45、姿态设置模块，用于设置车辆的初始姿态和目标姿态，其中姿态信息包括水平横纵坐标以及竖直坐标、横摆角、侧倾角和俯仰角；

46、速度控制模块，用于根据偏置采样概率法确定下一路径节点xi处车辆的控制输入速度；

47、姿态确定模块，用于：

48、通过下述迭代公式得到下一节点xi处车辆的位置信息和横摆角：

49、

50、φi-＝φi-1+ωiδt

51、其中，xi和yi是节点xi处车辆的横向和纵向位置坐标，φi表示横摆角，φi-表示地形信息更新前的横摆角度状态；

52、i是节点索引，δt是控制输入的时间周期；

53、速度νi和角速度ωi由步骤三得到的节点xi处车辆的控制输入速度得到；

54、上标“-”表示地形信息更新前的横摆角度状态；

55、以及，根据地形信息确定节点xi处车辆的高度z、侧倾角ψ和俯仰角γ：

56、zi＝zmax＝max(zk),k＝1,2...n

57、其中，k为索引，n为节点xi的矩形区域内单元体的总数，max(zk)为所有单元体的高度值中的最大值；

58、

59、其中，和分别为节点xi和xi-1的坡度角，和分别为节点xi和xi-1的坡向角；

60、约束模块，用于通过预设约束条件排除障碍节点。

61、优选的，速度控制模块用于：

62、应用下列概率分布函数p(v)对节点xi的线性控制速度νi进行偏抽样：

63、

64、f2(v)＝puni(v)

65、其中，f1(v)表示速度的高斯分布，vu为平均速度，vσ为预设的偏抽样参数；f2(v)表示速度的均匀分布；

66、使用均匀概率分布函数对节点xi的角速度ωi进行采样。

67、通过采用本发明实施例提供的方案，根据车辆的三位坐标以及横摆角、侧倾角和俯仰角规划车辆在越野路径，能够适应复杂越野环境下的非结构化道路上的路径规划。

68、针对越野崎岖地形环境下无人特种车辆路径规划面临的挑战，首先量化自主导航车辆的机动可靠性，定义基于车辆特性的状态机动可靠性和作战人员的任务机动可靠性两种类型的机动可靠性指标，并将其纳入任务规划。

69、再基于八叉树地图完成预设三维激光点云地图数据处理，用三角形面积法完成octomap地图干扰检测以及考虑车辆特性(主要考虑车辆最小转弯半径、车辆最大车身侧倾角、车辆最大爬坡度、车辆底盘的最小离地间隙和车辆前后悬的接近角和离去角、轮胎的充气压力、车辆悬挂的侧倾刚度等)，完成车辆可通行区域的与障碍区域节点的判断，最终提供一种改进目标偏置的双向快速扩展随机树算法(improved ib-rrt*)完成作战人员在不同和任务规则模式请求下的且满足车辆特性的最优路径规划。