CN115480579A - 履带式移动机械及其既定轨迹跟踪控制方法、装置、介质 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

目前无人驾驶技术主要应用在汽车领域，在履带式移动机械领域的相关技术研究相对较少。现有的无人驾驶履带式移动机械多以远程遥控为主，其存在履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。

有鉴于此，提出本申请。

发明内容

本发明公开了一种履带式移动机械及其既定轨迹跟踪控制方法、装置、介质，旨在解决履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。

本发明第一实施例提供了一种履带式移动机械的既定轨迹跟踪控制方法，包括：

获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

根据所述点云配准，确定最终整车位姿；

获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

优选地，所述获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图，具体为：

利用正态分布变换算法对所述车载多传感器融合平台采集的环境信息中的点云信息进行点云配准，确定相邻帧点云之间的坐标变换矩阵；

利用所述相邻帧之间的坐标变换矩阵对点云信息进行逐帧拼接，构建全局点云地图，其中，所述车载多传感器融合平台包括激光雷达、RTK、IMU和里程计传感器，所述坐标变换矩阵包括所述相邻帧点云之间的旋转矩阵和平移矩阵。

优选地，所述根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准，具体为：

将所述车载多传感器融合平台采集到的点云数据其中一帧作为目标点云，并对所述目标点云进行网格划分；

将所述目标点云加载到所述划分网格内，计算每一个网格均值向量

其具体如下：

其中，

表示单个网格内点云的所有点的坐标；Σ表示所述单个网格内点云数据的协方差矩阵；

利用单个网格内点云数据均值向量

和协方差矩阵Σ的计算公式，计算划分网格内每个网格中点云分布的概率密度函数

其具体如下；

利用所述车载多传感器融合平台中的IMU和里程计获得的初始坐标变换矩阵，将源点云坐标系旋转平移至目标点云坐标系下，其中，所述源点云经所述初始坐标变换矩阵变换后将分布于所述划分网格内的每一个网格中；

利用所述每个网格中点云分布的概率密度函数

求解源点云坐标为的概率X'；

将所有所述源点云坐标概率相乘，得到最大目标似然函数，其具体如下：

对所述最大目标似然函数两边取对数，简化所述最大目标似然函数，其具体如下；

利用牛顿迭代法求解最优坐标变换矩阵，完成点云配准，其具体如下：

T＝T+ΔT

其中，H为所述最大目标似然函数的黑塞矩阵；g为所述最大目标似然函数的梯度向量；T为所述坐标变换矩阵。

优选地，所述根据所述点云配准，确定最终整车位姿，具体为：

通过所述点云配准，获得坐标变换矩阵，确定整车位姿；

利用所述车载多传感器融合平台中的RTK获取全局位姿对所述整车位姿进行校正，确定最终整车位姿。

优选地，所述获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径，具体为：

将所述预瞄点坐标从所述全局点云地图坐标系下转换到所述整车坐标系下，并定义为(x₀',y₀')；

利用相应几何关系，对所述运动路径的半径进行求解，具体如下：

D+x₀′＝R

D²+y₀'²＝R²

x₀'²+y₀'²＝L₀ ²

其中，(x₀',y₀')所述预瞄点在所述整车坐标系下的坐标；L₀为所述整车坐标系原点到预瞄点的距离，R为所述运动路径的半径。

优选地，所述获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度，具体为：

根据所述整车跟踪速度、整车与所述预瞄点间运动路径的半径，求解出所述整车绕所述运动路径的圆点的角速度w，其具体如下：

其中，w为所述整车绕所述运动路径的圆点的角速度，v_c为所述整车跟踪速度，R为所述整车与所述预瞄点间运动路径的半径；

根据所述整车绕所述运动路径的圆点角速度w、所述整车与所述预瞄点间运动路径的半径R和所述履带式移动机械左右履带间距半值b计算左右履带速度，其具体如下：

左转：

v_L＝w×(R-b)

v_R＝w×(R+b)

右转：

v_L＝w×(R+b)

v_R＝w×(R-b)

直行：

v_L＝v_R＝v_c

其中，v_L为所述履带式移动机械左履带速度，v_R为所述履带式移动机械右履带速度，v_c为所述整车跟踪速度。

优选地，还包括：

根据所述车载多传感器融合平台中的激光雷达实时观测环境信息、探测障碍物，并将所述探测障碍物实时投影到代价地图；

根据所述代价地图，结合所述混合A*算法实时重规划跟踪轨迹。

本发明第二实施例提供了一种履带式移动机械的既定轨迹跟踪控制装置，包括：

全局点云地图构建单元，用于获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

点云配准单元，用于根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

最终整车位姿确定单元，用于根据所述点云配准，确定最终整车位姿；

运动路径计算单元，用于获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

左右履带速度生成单元，用于获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

既定轨迹跟踪控制单元，用于将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

本发明第三实施例提供了一种履带式移动机械，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

本发明第四实施例提供了一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

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

附图说明

图1是本发明第一实施例提供的一种履带式移动机械及其既定轨迹跟踪控制方法的流程示意图；

图2是本发明提供的向量法标定RTK原理图；

图3是本发明提供的整车与预瞄点间运动路径半径求解几何关系原理图；

图4是本发明提供的履带式移动机械运动模型原理图；

图5是本发明第二实施例提供的一种履带式移动机械及其既定轨迹跟踪控制装置的模块示意图；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。

应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地，取决于语境，短语“如果确定”或“如果检测(陈述的条件或事件)”可以被解释成为“当确定时”或“响应于确定”或“当检测(陈述的条件或事件)时”或“响应于检测(陈述的条件或事件)”。

实施例中提及的“第一\第二”仅仅是是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二”区分的对象在适当情况下可以互换，以使这里描述的实施例能够以除了在这里图示或描述的那些以外的顺序实施。

以下结合附图对本发明的具体实施例做详细说明。

本发明公开了一种履带式移动机械及其既定轨迹跟踪控制方法、装置、介质，旨在解决履带式移动机械工况GPS定位信号弱无法获取准确定位的问题。

请参阅图1，本发明第一实施例提供了一种履带式移动机械的既定轨迹跟踪控制方法，其可由履带式移动机械来执行，特别的，由所述履带式移动机械内的一个或者多个处理器来执行，以至少实现如下步骤：

S1O1，获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

具体地，在本实施例中，利用正态分布变换算法对所述车载多传感器融合平台采集的环境信息中的点云信息进行点云配准，确定相邻帧点云之间的坐标变换矩阵；

利用所述相邻帧之间的坐标变换矩阵对点云信息进行逐帧拼接，构建全局点云地图，其中，所述车载多传感器融合平台包括激光雷达、RTK、IMU和里程计传感器，所述坐标变换矩阵包括所述相邻帧点云之间的旋转矩阵和平移矩阵

需要说明的是，利用车载多传感器融合平台中的激光雷达采集环境点云信息，视为初始点云，将初始点云通过滤波模块进行滤波和下采样处理，生成源点云，将构建出的全局点云地图作为目标点云；其中，一般将所述采集环境点云信息的第一帧作为初始目标点云；利用所述车载多传感器融合平台中的IMU和里程计为NDT算法提供所述源点云和所述目标点云间的初始坐标变换矩阵；利用NDT(正态分布变换)算法对所述源点云和所述目标点云点云配准，确定相邻帧点云之间的坐标变换矩阵；利用所述相邻帧之间的坐标变换矩阵对所述采集环境点云信息进行逐帧拼接，构建全局点云地图。

其中，激光雷达：获取环境信息，并以点云数据形式呈现出来；

RTK：获取经纬度信息，实现所述整车的全局定位；

IMU：获取所述整车在X、Y、Z轴上的角速度信息和加速度信息；

里程计：获取所述整车的行程信息；

在本实施例中，还可以包括对车载多传感器融合平台进行标定，使其测量的结果更加准确，具体地：

利用imu_utils工具对所述车载多传感器融合平台中的IMU进行标定；

利用lidar_align标定工具对所述车载多传感器融合平台中的激光雷达和IMU进行标定；

利用向量法对所述车载多传感器融合平台中的RTK进行标定。

请参阅图2，所述向量法包括一下步骤：

将所述车载多传感器融合平台中的RTK获取地理坐标(lon,lat)平面化，得到平面坐标(lon_len,lat_len)，其具体为：

lon_len＝R*lon*cos(lat)

lat_len＝R*lat

其中，所述R为地球半径；所述lon为地理坐标经度；所述lat为地理坐标纬度；所述lon_len为所述地理坐标经度lon对应的平面长度；所述lat_len为所述地理坐标纬度lat对应的平面长度；

在所述全局点云地图坐标系下，选取两个平面坐标(x₁,y₁)和(x₂,y₂)，并通过所述车载多传感器融合平台中的RTK获得对应的经纬度坐标(lon₁,lat₁)和(lon₂,lat₂)；

利用所述选取的两个平面坐标和所述对应的经纬度坐标，根据两组向量在不同线性变换坐标系下长度比和夹角相同的原理，完成所述车载多传感器融合平台中的RTK标定，其具体为：

a＝b

其中，所述lon和lat为所述车载多传感器融合平台中的RTK实时测得的经纬度；所述x和y分别为所述RTK实时测得的经纬度转换到所述全局地图坐标系下的X轴和Y轴坐标值。

初始坐标变换矩阵的方法，还可以包括：

利用所述IMU获取的XYZ轴上的加速度和角速度信息进行积分获取坐标变换矩阵；

通过所述里程计获取的里程信息对所述IMU积分获取的坐标变换矩阵进行校验。

S1O2，根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

具体地，在本实施例中：

将所述车载多传感器融合平台采集到的点云数据其中一帧作为目标点云，并对所述目标点云进行网格划分；

将所述目标点云加载到所述划分网格内，计算每一个网格均值向量

其具体如下：

其中，

表示单个网格内点云的所有点的坐标；Σ表示所述单个网格内点云数据的协方差矩阵；

利用单个网格内点云数据均值向量

和协方差矩阵Σ的计算公式，计算划分网格内每个网格中点云分布的概率密度函数

其具体如下；

利用所述车载多传感器融合平台中的IMU和里程计获得的初始坐标变换矩阵，将源点云坐标系旋转平移至目标点云坐标系下，其中，所述源点云经所述初始坐标变换矩阵变换后将分布于所述划分网格内的每一个网格中；

利用所述每个网格中点云分布的概率密度函数

求解源点云坐标为的概率X'；

将所有所述源点云坐标概率相乘，得到最大目标似然函数，其具体如下：

对所述最大目标似然函数两边取对数，简化所述最大目标似然函数，其具体如下；

利用牛顿迭代法求解最优坐标变换矩阵，完成点云配准，其具体如下：

T＝T+ΔT

其中，H为所述最大目标似然函数的黑塞矩阵；g为所述最大目标似然函数的梯度向量；T为所述坐标变换矩阵。

S1O3，根据所述点云配准，确定最终整车位姿；

具体地，在本实施例中，可以通过所述点云配准，获得坐标变换矩阵，确定整车位姿；

利用所述车载多传感器融合平台中的RTK获取全局位姿对所述整车位姿进行校正，确定最终整车位姿。

S1O4，获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

在本实施例中：利用所述整车位姿和所述预瞄点间的几何关系(如图3所示)，对所述整车与所述预瞄点间的运动路径的半径进行求解，其具体如下：

将所述预瞄点坐标从所述全局点云地图坐标系下转换到所述整车坐标系下，并定义为(x₀',y₀')；

利用相应几何关系，对所述运动路径的半径进行求解，具体如下：

D+x₀′＝R

D²+y₀'²＝R²

x₀'²+y₀'²＝L₀ ²

其中，(x₀',y₀')所述预瞄点在所述整车坐标系下的坐标；L₀为所述整车坐标系原点到预瞄点的距离，R为所述运动路径的半径。

需要说明的是，所述跟踪轨迹可以下方式给出，具体地：

方式1、加载所述全局点云地图，驾驶所述履带式移动机械按计划路径行进，利用所述基于NDT融合SLAM定位方法获取沿途整车位姿作为计划路径的离散路径点，将所述离散路径点保存为路径点文件作为所述跟踪轨迹；

方式2、在所述全局点云地图的基础上，利用高精地图标注工具，对所述全局点云地图进行标注，制作矢量地图，并在矢量地图上定义所述跟踪路径；

方式3、将所述车载多传感器融合平台中激光雷达观测的实时点云投影为实时代价地图，基于所述实时代价地图，给定行进终点位姿和所述整车当前位姿，结合混合A*算法，自主构建出所述跟踪轨迹。

S1O5，获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

具体地，在本实施例中：

根据所述整车跟踪速度、整车与所述预瞄点间运动路径的半径，求解出所述整车绕所述运动路径的圆点的角速度w，其具体如下：

其中，w为所述整车绕所述运动路径的圆点的角速度，v_c为所述整车跟踪速度，R为所述整车与所述预瞄点间运动路径的半径；

利用所述整车绕所述运动路径的圆点角速度w、所述整车与所述预瞄点间运动路径的半径R和所述履带式移动机械左右履带间距半值b计算左右履带速度。由于履带式移动机械属于差速型运动机械，故履带式移动机械转向是通过左右履带的速度差来实现的。图4为履带式移动机械运动模型原理图，当右履带速度v_R大于左履带速度v_L时，履带式移动机械左转；反之，则右转。当左右履带速度相等时，履带式移动机械直行。其具体如下：

左转：

v_L＝w×(R-b)

v_R＝w×(R+b)

右转：

v_L＝w×(R+b)

v_R＝w×(R-b)

直行：

v_L＝v_R＝v_c

其中，v_L为所述履带式移动机械左履带速度，v_R为所述履带式移动机械右履带速度，v_c为所述整车跟踪速度

S1O6，将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

在本发明一个可能的实施例中，还包括：

根据所述车载多传感器融合平台中的激光雷达实时观测环境信息、探测障碍物，并将所述探测障碍物实时投影到代价地图，其中，利用所述车载多传感器融合平台中的激光雷达实时观测环境信息，生成观测点云；将所述观测点云一定高度范围内的点云垂直投影到所述代价地图平面上，生成代价地图；

根据所述代价地图，结合所述混合A*算法实时重规划跟踪轨迹，其中，通过循迹，可以实现所述履带式移动机械实时规避障碍物功能。

请参阅图5，本发明第二实施例提供了一种履带式移动机械的既定轨迹跟踪控制装置，包括：

全局点云地图构建单201，用于获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

点云配准单元202，用于根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

最终整车位姿确定单元203，用于根据所述点云配准，确定最终整车位姿；

运动路径计算单元204，用于获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

左右履带速度生成单元205，用于获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

既定轨迹跟踪控制单元206，用于将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

本发明第三实施例提供了一种履带式移动机械，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

本发明第四实施例提供了一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

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

示例性地，本发明第三实施例和第四实施例中所述的计算机程序可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述实现一种履带式移动机械中的执行过程。例如，本发明第二实施例中所述的装置。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种履带式移动机械的既定轨迹跟踪控制方法的控制中心，利用各种接口和线路连接整个所述实现对一种履带式移动机械的既定轨迹跟踪控制方法的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现一种履带式移动机械的既定轨迹跟踪控制方法的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、文字转换功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、文字消息数据等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘、智能存储卡(Smart Media Card,SMC)、安全数字(Secure Digital,SD)卡、闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

其中，所述实现的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一个计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (10)

1.一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，包括：

获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

根据所述点云配准，确定最终整车位姿；

获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

2.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图，具体为：

利用正态分布变换算法对所述车载多传感器融合平台采集的环境信息中的点云信息进行点云配准，确定相邻帧点云之间的坐标变换矩阵；

利用所述相邻帧之间的坐标变换矩阵对点云信息进行逐帧拼接，构建全局点云地图，其中，所述车载多传感器融合平台包括激光雷达、RTK、IMU和里程计传感器，所述坐标变换矩阵包括所述相邻帧点云之间的旋转矩阵和平移矩阵。

3.根据权利要求2所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准，具体为：

将所述车载多传感器融合平台采集到的点云数据其中一帧作为目标点云，并对所述目标点云进行网格划分；

将所述目标点云加载到所述划分网格内，计算每一个网格均值向量

其具体如下：

其中，

表示单个网格内点云的所有点的坐标；Σ表示所述单个网格内点云数据的协方差矩阵；

利用单个网格内点云数据均值向量

和协方差矩阵Σ的计算公式，计算划分网格内每个网格中点云分布的概率密度函数

其具体如下；

利用所述车载多传感器融合平台中的IMU和里程计获得的初始坐标变换矩阵，将源点云坐标系旋转平移至目标点云坐标系下，其中，所述源点云经所述初始坐标变换矩阵变换后将分布于所述划分网格内的每一个网格中；

利用所述每个网格中点云分布的概率密度函数

求解源点云坐标为的概率X'；

将所有所述源点云坐标概率相乘，得到最大目标似然函数，其具体如下：

对所述最大目标似然函数两边取对数，简化所述最大目标似然函数，其具体如下；

利用牛顿迭代法求解最优坐标变换矩阵，完成点云配准，其具体如下：

T＝T+ΔT

其中，H为所述最大目标似然函数的黑塞矩阵；g为所述最大目标似然函数的梯度向量；T为所述坐标变换矩阵。

4.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述根据所述点云配准，确定最终整车位姿，具体为：

通过所述点云配准，获得坐标变换矩阵，确定整车位姿；

利用所述车载多传感器融合平台中的RTK获取全局位姿对所述整车位姿进行校正，确定最终整车位姿。

5.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径，具体为：

将所述预瞄点坐标从所述全局点云地图坐标系下转换到所述整车坐标系下，并定义为(x₀',y₀')；

利用相应几何关系，对所述运动路径的半径进行求解，具体如下：

D+x₀′＝R

D²+y₀'²＝R²

x₀'²+y₀'²＝L₀ ²

其中，(x₀',y₀')所述预瞄点在所述整车坐标系下的坐标；L₀为所述整车坐标系原点到预瞄点的距离，R为所述运动路径的半径。

6.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，所述获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度，具体为：

根据所述整车跟踪速度、整车与所述预瞄点间运动路径的半径，求解出所述整车绕所述运动路径的圆点的角速度w，其具体如下：

其中，w为所述整车绕所述运动路径的圆点的角速度，v_c为所述整车跟踪速度，R为所述整车与所述预瞄点间运动路径的半径；

根据所述整车绕所述运动路径的圆点角速度w、所述整车与所述预瞄点间运动路径的半径R和所述履带式移动机械左右履带间距半值b计算左右履带速度，其具体如下：

左转：

v_L＝w×(R-b)

v_R＝w×(R+b)

右转：

v_L＝w×(R+b)

v_R＝w×(R-b)

直行：

v_L＝v_R＝v_c

其中，v_L为所述履带式移动机械左履带速度，v_R为所述履带式移动机械右履带速度，v_c为所述整车跟踪速度。

7.根据权利要求1所述的一种履带式移动机械的既定轨迹跟踪控制方法，其特征在于，还包括：

根据所述车载多传感器融合平台中的激光雷达实时观测环境信息、探测障碍物，并将所述探测障碍物实时投影到代价地图；

根据所述代价地图，结合所述混合A*算法实时重规划跟踪轨迹。

8.一种履带式移动机械的既定轨迹跟踪控制装置，其特征在于，包括：

全局点云地图构建单元，用于获取通过车载多传感器融合平台采集到的环境信息，并基于所述环境信息构建全局点云地图；

点云配准单元，用于根据所述全局点云地图与所述车载多传感器融合平台采集的实时观测点云进行点云配准；

最终整车位姿确定单元，用于根据所述点云配准，确定最终整车位姿；

运动路径计算单元，用于获取给定的跟踪轨迹，并选取所述给定跟踪轨迹距离所述整车位姿最近的一点作为预瞄点，生成整车与所述预瞄点间的运动路径的半径；

左右履带速度生成单元，用于获取给定所述整车跟踪速度，并根据所述半径以及履带式移动机械的履带间距，生成所述履带式移动机械左右履带速度；

既定轨迹跟踪控制单元，用于将所述履带式移动机械左右履带速度进行平滑处理并转换成占空比信号，通过CAN总线将所述占空比信号输出至履带式移动机械的控制系统，以实现履带式移动机械既定轨迹跟踪控制。

9.一种履带式移动机械，其特征在于，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如权利要求1至7任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

10.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如权利要求1至/7任意一项所述的一种履带式移动机械的既定轨迹跟踪控制方法。

Images