一种应用于干散货船舱自动驾驶清舱机的路径规划方法

本发明属于自动驾驶，尤其涉及一种应用于干散货船舱自动驾驶清舱机的路径规划方法。

背景技术：

1、随着自动化技术的飞速发展，港口智能化和作业智慧化已成为当前港口建设的主要趋势和发展方向。其中，利用自动驾驶技术驱动机器人在港口进行无人作业是最受关注的研究领域之一。这不仅能节省时间、降低人力成本，还极大地提升了港口资源的优化配置能力。在这一系统中，路径规划扮演着极其关键的角色。路径规划的核心是如何有效地从起点导航至终点，同时考虑到路线的安全性、效率以及可行性。由于船舱环境的复杂性，制定一个既可靠又高效的路径规划方案是富有挑战性的。

2、现有的路径规划技术主要依赖于各种高级算法，如a*算法、dijkstra算法和基于图的搜索算法等。这些算法能够处理复杂的路网结构，通过计算最短或最优路径来引导机器人行驶。此外，随着深度学习技术的应用，一些基于数据驱动的路径规划方法也在不断涌现。这些方法通过分析大量的历史行驶数据，可以预测交通状况并据此优化路径选择。然而，这些现有技术主要集中于地面结构化场景的路径规划，而对于船舱这一非结构化场景的研究相对较少。

3、区别于传统的路面环境，在船舱环境中，机器人需要面对因矿料堆积而产生的崎岖不平的路面，且路面的坡度会随着清料作业的进行而不断变化。同时需要避免在清料过程中与环境障碍物发生碰撞。因此，这不仅要求路径规划方法能够实时响应环境变化，还要在有限的计算资源下保持高效率，规划出一条尽可能平坦且高效的机器人行驶路径。而现有的路径规划技术在应对这类特殊环境时往往力不从心，无法满足清舱机操作的特殊要求。因此，迫切需要一种应用于干散货船舱自动驾驶清舱机的路径规划方法，确保船舱内无人清料任务能够安全、高效地执行。

技术实现思路

1、本发明的技术解决问题是：克服现有技术的不足，提出一种应用于干散货船舱自动驾驶清舱机的路径规划方法，该方法通过处理接收的激光雷达点云数据，利用数字高程模型(digital elevation model,dem)对环境建模。对建模后得到的dem中的高程数据和粗糙度信息，采用信息滤波进行数据积累、使用清舱机自身位姿进行数据更新、利用归一化卷积进行插值填充，保证数据的有效性和准确度。利用改进后的坡度计算模型提取环境地形信息，使用动态窗口法(dynamic window approach,dwa)通过控制空间采样生成备选路径，结合清舱机动态特性对备选路径进行评估，得到一条适合在船舱内的平稳、安全的最优行驶轨迹。此外，本发明结合船舱结构信息，构建不同的路径清扫方案，确保清舱机的无人清料作业任务能高效、完善地完成。

2、本发明的技术解决方案是：

3、一种应用于干散货船舱自动驾驶清舱机的路径规划方法，该方法的步骤包括：

4、s1、结合船舱结构信息，构建不同的路径清扫方案，保证清料任务的完善执行。具体步骤包括：

5、s11、根据船舱内料体的堆积情况进行路径清扫方案的构建。

6、s12、根据清舱机的位置信息进行路径清扫方案的构建。

7、s2、采集清舱机状态信息及环境数据，包括清舱机当前位置(x,y)、线速度v、角速度ω、姿态角θ和周围的地形信息。

8、s3、利用基于高斯分布的方法估计环境地形，为后续的路径规划提供准确的环境信息。具体步骤包括：

9、s31、处理激光雷达点云数据，建立基于规则格网的dem对环境地形进行描述。

10、原始环境测量数据为激光雷达实时采集的三维点云。dem中第i个网格单元pi中存储的信息为：

11、pi＝(xi,yi,hi,σi2)   (1)

12、其中(xi,yi)表示第i个单元的位置，hi表示该单元处的高程估计，σi2为对应的高程方差估计。dem中的高程数据大致服从高斯分布，即：

13、h～n(μh,σh2)   (2)

14、激光雷达安装在清舱机机身外，涉及不同坐标系之间的转换。设w为世界坐标系，m为dem坐标系，v为清舱机坐标系，s为激光雷达坐标系。tvs表示激光雷达坐标系到清舱机坐标系之间的相对位姿。twv表示清舱机坐标系到世界坐标系之间的相对位姿。tmw表示世界坐标系到dem坐标系的相对位姿。若激光雷达获取的三维点云中某位置的坐标为pis，则其对应在dem坐标系下的坐标为：

15、pim＝tmw·twv·tvs·pis   (3)

16、假设落入dem第i个单元网格中的点的集合为{pkm},0≤k≤n-1，其中n为集合中点的个数。利用集合中的点拟合成三维平面z，再使用最小二乘法计算拟合得到平面参数，可得第i个网格测量的中心高程值hi：

17、hi＝axi+byi+c   (4)

18、以及其测量方差σi2：

19、

20、其中z(·)表示获取原始高程值的函数，h(·)表示获取拟合平面对应坐标高程值的函数。

21、s32、使用一维信息滤波更新dem，保证获取到环境信息的实时变化。

22、由原始点云得到测量高程值及方差，可计算测量的信息值和测量的信息矩阵：

23、

24、其中k表示更新时刻，表示k时刻原始点云的测量高程值，表示k时刻原始点云的测量方差。则其中每个网格高程估计的更新方程为：

25、

26、

27、其中，表示k时刻的估计信息向量，表示k时刻的估计信息矩阵，imax为设置的最大信息矩阵。可得到高程估计的更新公式：

28、

29、s33、使用清舱机位姿数据对dem再次更新，保证环境信息的准确性。

30、通过s2获得清舱机的当前位置信息，计算清舱机所处地面的切平面，假设切平面的数学模型为：

31、z＝ax+by+c  (10)

32、已知清舱机坐标系v在dem坐标系m下的坐标为则清舱机底部与地面接触的点为：

33、

34、其中d为清舱机坐标系到清舱机底部的距离，为清舱机位姿计算得到的切平面的法向量，其表达式为：

35、

36、其中rego为清舱机姿态对应的旋转矩阵，为方向沿z轴向上的单位向量，可得切平面数学模型中的各个参数为：

37、

38、其中x(·)、y(·)、z(·)分别表示获取向量x、y、z轴分量的函数。

39、根据切平面方程以及清舱机下方对应dem网格的中心位置(xcell,ycell)，可得该网格测量的中心高程值为：

40、hcell＝a·xcell+b·ycell+c   (14)

41、由于在定位模块中已经对自车运动进行了估计，因此自车下方的网格无需参与s32中基于信息滤波的数据积累，避免再次对其进行滤波估计。可由上式计算得到的高程值直接对dem网格进行更新。

42、s34、使用归一化卷积对dem进行插值填充，为路径规划提供更充足的环境信息。

43、对于缺失高程数据的dem单元网格，定义围绕该单元5×5的网格邻域，构建适用度函数a，其表达式为：

44、

45、构建确定度函数c，其表达式为：

46、

47、应用归一化卷积的结果可表示为：

48、

49、其中表示卷积运算，h是邻域内的高程值向量。利用式(17)可以对缺失高程数据的dem网格进行填充。

50、s4、结合清舱机动态特性及环境约束，使用dwa算法进行备选路径规划。具体步骤包括：

51、s41、利用dem提取环境坡度信息。

52、坡度是高度的变化率，其数学表达式为：

53、

54、其中，fx、fy分别是地面高程的曲面函数对x、y的偏导数。

55、由于实际采集到的高程数据为离散的，将其拟合成连续函数计算量过大，为保证清舱机运行的实时性，dem的坡度可以通过高度差分得到，其计算公式为：

56、

57、其中为g为dem网格尺寸，z(·)表示获取原始高程值的函数，(xi+a,yi)和(xi-b,yi)表示离(xi,yi)在x方向前后最近的可用数据的dem网格坐标，(xi,yi+c)和(xi,yi-d)表示离(xi,yi)在y方向前后最近的可用数据的dem网格坐标。a、b、c、d参数的表达式为：

58、

59、s42、结合船舱环境建图数据，选取路径规划的终点位置。

60、路径规划的起点为清舱机所处的当前位置，由定位模块提供。终点位置需要结合全局路径选取。假设清舱机所在点为p，全局路径上离点p最近的一点为p'，则p'是p在全局路径上的对应点。当清舱机中心p离全局路径较远时，应使清舱机能够尽快回到全局路径上来，因此将终点选在全局路径上p'点往后k1m处；如果清舱机中心p离全局路径较近，则以清舱机在前向预测时间内能前进的最远距离为半径，p为圆心作一个圆，该圆与全局路径的交点再往前k2m处的全局路径上的点则为选取的终点；如果车中心p离全局路径终点较近，则可直接将全局路径终点作为局部路径的终点。

61、s43、建立动态窗口进行控制量采样生成备选路径。

62、根据s2中采集的数据，清舱机当前的时刻的状态为(v0,ω0)，其中v0为线速度，ω0为角速度。根据清舱机的动态特性，可以得到其最大线加速度为最大角加速度为从而确定清舱机在时间间隔δt内可以达到的线速度和角速度的二维控制量窗口，根据不同的控制量组合可生成其对应一段时间内的轨迹。离散轨迹点的计算公式为：

63、

64、其中r为转向半径，(x,y,θ)和(x',y',θ')分别为当前时刻和下一时刻的二维坐标及航向角。

65、s44、对备选路径结合环境约束进行筛选。

66、对路径的可通行性判断表达式为：

67、

68、其中si表示候选路径上第i个点对应dem网格的坡度；σi2表示第i个点对应的地面高度方差，其与地面粗糙度呈正相关；thslope和throughness表示允许清舱机安全行驶的坡度阈值和粗糙度阈值。

69、s45、结合环境信息构建代价函数，对备选路径进行评估，输出最优路径。

70、代价函数的表达式如下：

71、c(v,ω)＝ωheading·cheading+ωtrav·ctrav+ωvel·cvel   (23)

72、其中cheading表示与目标连线的方向角误差，ctrav表示地面的可通行性，cvel表示清舱机行驶的速度大小，ωheading、ωtrav、ωvel表示三者的加权系数。代价函数中三个部分的数学表达式为：

73、

74、

75、ctrav＝-vi   (26)

76、其中(xi,yi,θi)表示(vi,ωi)对应的候选路径终点，(xt,yt)表示目标位置，sk和σk2分别表示第i条候选路径上的第k个点对应的dem网格的坡度和方差，wslope和wroughness分别表示坡度和粗糙度的加权系数。

77、s5、构建清舱机实时碰撞检测模块，保证在复杂船舱环境中无人作业的安全进行。

78、构建占据栅格查找表，将清舱机朝向角进行离散化，提前在内存中存储清舱机在各个朝向下所占据的栅格位置，在实时碰撞检测中遍历当前朝向对应的栅格位置是否存在障碍物。当查找表的每个栅格处都未被障碍物占据，则表示清舱机当前的行进路径不会发生碰撞。

79、本发明的有益效果在于：

80、通过融合实时更新的高精度环境信息并对dwa算法的代价函数进行改进，该方法有助于清舱机更准确地应对各种障碍和船舱内的地形变化，为清舱机在船舱这类非结构化环境中提供更有效、安全的路径规划。且该方法结合船舱结构信息构建不同的路径清扫方案，能够确保无人清料作业高效、完善地执行。