履带式移动机械及其既定轨迹跟踪控制方法、装置、介质制造方法及图纸

本发明专利技术提供了一种履带式移动机械及其既定轨迹跟踪控制方法、装置、介质，通过车载多传感器融合平台采集到的环境信息，基于所述环境信息构建全局点云地图，并根据点云地图对采集到的点云进行配准，以确定最终整车位姿，根据给定的跟踪轨迹以及整车跟踪速度，生成履带式移动机械左右履带速度，通过CAN总线将速度信号输出至控制系统，以实现履带式移动机械既定轨迹跟踪控制，解决履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。位信号弱无法获取准确定位的问题。位信号弱无法获取准确定位的问题。

【技术实现步骤摘要】
履带式移动机械及其既定轨迹跟踪控制方法、装置、介质


[0001]本专利技术涉及无人驾驶领域，特别涉及一种履带式移动机械及其既定轨迹跟踪控制方法、装置、介质。

技术介绍

[0002]履带式移动机械应用非常广泛，有的可能运用于山区里比较平坦的区域，有的运用于矿山、高原、南北极等环境比较恶劣的地形，以及地震、泥石流以及辐射等救灾抢险的恶劣工况中。履带式移动机械越野性能较强，能够在泥泞、湿地、矿山等工况恶劣的场所高效作业。传统履带移动机械以内燃机为驱动，普遍存在污染物排放高、噪声大和效率低等问题，电动式履带式移动机械具有零排放、低噪声和传动效率高等优点，在克服上述问题的基础上方便了履带式移动机械智能化的发展。由于履带式移动机械工作环境恶劣，对驾驶员的身体各方面的素质要求非常高，随时可能给驾驶员的生命造成危险，通过无人驾驶技术对车辆进行控制，在作业时自主对环境进行感知并做出决策，极大的降低了操作人员的作业风险，减少了劳动力的浪费，提高了作业效率。
[0003]目前无人驾驶技术主要应用在汽车领域，在履带式移动机械领域的相关技术研究相对较少。现有的无人驾驶履带式移动机械多以远程遥控为主，其存在履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。
[0004]有鉴于此，提出本申请。

技术实现思路

[0005]本专利技术公开了一种履带式移动机械及其既定轨迹跟踪控制方法、装置、介质，旨在解决履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。
[0006]本专利技术第一实施例提供了一种履带式移动机械的既定轨迹跟踪控制方法，包括：
[0007]获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；
[0008]根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；
[0009]根据所述点云配准，确定最终整车位姿；
[0010]获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；
[0011]获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；
[0012]将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。
[0013]优选地，所述获取通过车载多传感器融合平台采集到的环境信息，并基于所述环
境信息构建全局点云地图，具体为：
[0014]利用正态分布变换算法对所述车载多传感器融合平台采集的环境信息中的点云信息进行点云配准，确定相邻帧点云之间的坐标变换矩阵；
[0015]利用所述相邻帧之间的坐标变换矩阵对点云信息进行逐帧拼接，构建全局点云地图，其中，所述车载多传感器融合平台包括激光雷达、RTK、IMU和里程计传感器，所述坐标变换矩阵包括所述相邻帧点云之间的旋转矩阵和平移矩阵。
[0016]优选地，所述根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准，具体为：
[0017]将所述车载多传感器融合平台采集到的点云数据其中一帧作为目标点云，并对所述目标点云进行网格划分；
[0018]将所述目标点云加载到所述划分网格内，计算每一个网格均值向量其具体如下：
[0019][0020][0021]其中，表示单个网格内点云的所有点的坐标；Σ表示所述单个网格内点云数据的协方差矩阵；
[0022]利用单个网格内点云数据均值向量和协方差矩阵Σ的计算公式，计算划分网格内每个网格中点云分布的概率密度函数其具体如下；
[0023][0024]利用所述车载多传感器融合平台中的IMU和里程计获得的初始坐标变换矩阵，将源点云坐标系旋转平移至目标点云坐标系下，其中，所述源点云经所述初始坐标变换矩阵变换后将分布于所述划分网格内的每一个网格中；
[0025]利用所述每个网格中点云分布的概率密度函数求解源点云坐标为的概率X'；
[0026]将所有所述源点云坐标概率相乘，得到最大目标似然函数，其具体如下：
[0027][0028]对所述最大目标似然函数两边取对数，简化所述最大目标似然函数，其具体如下；
[0029][0030]利用牛顿迭代法求解最优坐标变换矩阵，完成点云配准，其具体如下：
[0031][0032]T＝T+ΔT
[0033]其中，H为所述最大目标似然函数的黑塞矩阵；g为所述最大目标似然函数的梯度向量；T为所述坐标变换矩阵。
[0034]优选地，所述根据所述点云配准，确定最终整车位姿，具体为：
[0035]通过所述点云配准，获得坐标变换矩阵，确定整车位姿；
[0036]利用所述车载多传感器融合平台中的RTK获取全局位姿对所述整车位姿进行校正，确定最终整车位姿。
[0037]优选地，所述获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径，具体为：
[0038]将所述预瞄点坐标从所述全局点云地图坐标系下转换到所述整车坐标系下，并定义为(x0',y0')；
[0039]利用相应几何关系，对所述运动路径的半径进行求解，具体如下：
[0040]D+x0′
＝R
[0041]D2+y0'2＝R2[0042]x0'2+y0'2＝L
02
[0043][0044]其中，(x0',y0')所述预瞄点在所述整车坐标系下的坐标；L0为所述整车坐标系原点到预瞄点的距离，R为所述运动路径的半径。
[0045]优选地，所述获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度，具体为：
[0046]根据所述整车跟踪速度、整车与所述预瞄点间运动路径的半径，求解出所述整车绕所述运动路径的圆点的角速度w，其具体如下：
[0047][0048]其中，w为所述整车绕所述运动路径的圆点的角速度，v
c
为所述整车跟踪速度，R为所述整车与所述预瞄点间运动路径的半径；
[0049]根据所述整车绕所述运动路径的圆点角速度w、所述整车与所述预瞄点间运动路径的半径R和所述履带式移动机械左右履带间距半值b计算左右履带速度，其具体如下：
[0050]左转：
[0051]v
L
＝w
×
(R
‑
b)
[0052]v
R
＝w
×
(R+b)
[0053]右转：
[0054]v
L
＝w
×
(R+b)
[0055]v
R
＝w
×
(R
‑
b)
[0056]直行：
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，包括：获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；根据所述点云配准，确定最终整车位姿；获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。2.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图，具体为：利用正态分布变换算法对所述车载多传感器融合平台采集的环境信息中的点云信息进行点云配准，确定相邻帧点云之间的坐标变换矩阵；利用所述相邻帧之间的坐标变换矩阵对点云信息进行逐帧拼接，构建全局点云地图，其中，所述车载多传感器融合平台包括激光雷达、RTK、IMU和里程计传感器，所述坐标变换矩阵包括所述相邻帧点云之间的旋转矩阵和平移矩阵。3.根据权利要求2所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准，具体为：将所述车载多传感器融合平台采集到的点云数据其中一帧作为目标点云，并对所述目标点云进行网格划分；将所述目标点云加载到所述划分网格内，计算每一个网格均值向量其具体如下：其具体如下：其中，表示单个网格内点云的所有点的坐标；Σ表示所述单个网格内点云数据的协方差矩阵；利用单个网格内点云数据均值向量和协方差矩阵Σ的计算公式，计算划分网格内每个网格中点云分布的概率密度函数其具体如下；
利用所述车载多传感器融合平台中的IMU和里程计获得的初始坐标变换矩阵，将源点云坐标系旋转平移至目标点云坐标系下，其中，所述源点云经所述初始坐标变换矩阵变换后将分布于所述划分网格内的每一个网格中；利用所述每个网格中点云分布的概率密度函数求解源点云坐标为的概率X'；将所有所述源点云坐标概率相乘，得到最大目标似然函数，其具体如下：对所述最大目标似然函数两边取对数，简化所述最大目标似然函数，其具体如下；利用牛顿迭代法求解最优坐标变换矩阵，完成点云配准，其具体如下：T＝T+ΔT其中，H为所述最大目标似然函数的黑塞矩阵；g为所述最大目标似然函数的梯度向量；T为所述坐标变换矩阵。4.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述根据所述点云配准，确定最终整车位姿，具体为：通过所述点云配准，获得坐标变换矩阵，确定整车位姿；利用所述车载多传感器融合平台中的RTK获取全局位姿对所述整车位姿进行校正，确定最终整车位姿。5.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径，具体为：将所述预瞄点坐标从所述全局点云地图坐标系下转换到所述整车坐标系下，并定义为(x0',y0')；利用相应几何关系，对所述运动路径的半径进行求解，具体如下：D+x0′
＝RD2+y0...

【专利技术属性】
技术研发人员：任好玲，吴江东，林添良，缪骋，李钟慎，付胜杰，陈其怀，姚瑜，张春晖，
申请(专利权)人：华侨大学，
类型：发明
国别省市：