[发明专利]一种面向大型救援装备的激光雷达点云畸变矫正方法

说明书

本发明提出了一种面向大型救援装备的激光雷达点云畸变矫正方法，首先利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵，然后利用激光雷达处位置变化确定点云的平移变换矩阵，最后利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性。该方法兼顾激光雷达处旋转的角度和角速度，保证旋转变换矩阵的准确性；分析大型救援装备灾后施救的操作，将激光雷达处的位置变化分为两种，提高平移变换矩阵使用场景的适应性；利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声，进而提高激光雷达点云畸变矫正的准确性。

技术领域

本发明涉及一种激光雷达点云畸变矫正方法，具体的是一种面向大型救援装备的激光雷达点云畸变矫正方法，属于灾后救援领域。

背景技术

我国是全球受地质灾害最为严重的国家之一，地质灾害包括地震、泥石流、滑坡等，具有发生频率高、危害程度高等特点。地质灾害发生后，救援队伍需要借助工程机械开展应急救援工作。现有的以人作业为主的大型救援装备施救时，现场需要大量的救援人员，导致现场安全无保障、救援效率低下等突出问题。为克服上述问题，需要提高大型救援装备的智能化程度，因此在装备上增加了很多传感器，包括激光雷达、摄像头、毫米波雷达等。

激光雷达具有较广的覆盖范围，能够适用于各种比较恶劣的环境；具有较强的抗干扰能力，能够有效地规避周边环境中无线电波的干扰；具有较高的测距精度，能够准确地获得周围环境中目标的位置信息。当激光雷达收集周围环境的点云时，由于载体的运动或转向，每一帧点云中的第一个点和最后一个点的坐标系间都存在一定的位姿差。也就是说，在同一帧点云中各个点都不在同一个坐标系中，这就导致了点云畸变。

激光雷达点云畸变矫正的原理为：利用系列的坐标变换将一帧中所有激光点都变换到第一个激光点的坐标系下。何磊等针对无人驾驶车辆，利用惯性导航系统和轮速传感器的信息，提出了一种激光雷达点云去畸变方法(专利申请号：201811479464.X)。该方法可以去除因激光雷达载体航向角变化和平面运动产生的误差，但忽略了载体俯仰角和侧倾角的影响。杨文武等针对无人船，综合考虑船身的三个姿态角和位置变化，以矫正激光雷达点云的畸变(专利申请号：201911222073.4)。

大型救援装备中最常使用的是履带式挖掘机，装备的上部转台和行走机构可独立工作，本发明以挖掘机为例介绍提出的方法。装备行驶时，不得做回转动作，激光雷达与装备的位置变化保持一致；装备进行回转操作时，装备的位置保持不变，而激光雷达进行圆周运动。然而现有方法认为激光雷达与其载体的位置变化相同，未考虑激光雷达与其载体的位置变化相异的场景。大型救援装备作业和在凹凸不平路面上行驶时，激光雷达安装处的俯仰角、侧倾角和航向角会持续变化，通过传感器测量存在一定的误差。然而现有方法未考虑如何保障三个角度测量的准确性。

发明内容

针对以上问题，本发明提出一种面向大型救援装备的激光雷达点云畸变矫正方法，该方法兼顾激光雷达处旋转的角度和角速度，保证旋转变换矩阵的准确性；分析大型救援装备灾后施救的操作，将激光雷达处的位置变化分为两种，提高平移变换矩阵使用场景的适应性；利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声，进而提高激光雷达点云畸变矫正的准确性。

本发明提供一种面向大型救援装备的激光雷达点云畸变矫正方法，具体步骤如下：

步骤一：利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵；

激光雷达固定在装备驾驶舱的顶部，坐标系满足右手准则，Z轴正方向指向驾驶舱的正上方，Y轴正方向指向驾驶舱的正前方，X轴正方向指向驾驶舱的正右方；

在一帧点云中，第i个激光点的坐标系需要依次绕X,Y,Z轴旋转a_xi,a_yi,a_zi度才能与第1个激光点的坐标系保持对应坐标轴同向且平行，旋转方向满足右手法则记为正值，反之记为负值；