一种面向大型救援装备的激光雷达点云畸变矫正方法

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


背景技术：

2.我国是全球受地质灾害最为严重的国家之一，地质灾害包括地震、泥石流、滑坡等，具有发生频率高、危害程度高等特点。地质灾害发生后，救援队伍需要借助工程机械开展应急救援工作。现有的以人作业为主的大型救援装备施救时，现场需要大量的救援人员，导致现场安全无保障、救援效率低下等突出问题。为克服上述问题，需要提高大型救援装备的智能化程度，因此在装备上增加了很多传感器，包括激光雷达、摄像头、毫米波雷达等。
3.激光雷达具有较广的覆盖范围，能够适用于各种比较恶劣的环境；具有较强的抗干扰能力，能够有效地规避周边环境中无线电波的干扰；具有较高的测距精度，能够准确地获得周围环境中目标的位置信息。当激光雷达收集周围环境的点云时，由于载体的运动或转向，每一帧点云中的第一个点和最后一个点的坐标系间都存在一定的位姿差。也就是说，在同一帧点云中各个点都不在同一个坐标系中，这就导致了点云畸变。
4.激光雷达点云畸变矫正的原理为：利用系列的坐标变换将一帧中所有激光点都变换到第一个激光点的坐标系下。何磊等针对无人驾驶车辆，利用惯性导航系统和轮速传感器的信息，提出了一种激光雷达点云去畸变方法(专利申请号：201811479464.x)。该方法可以去除因激光雷达载体航向角变化和平面运动产生的误差，但忽略了载体俯仰角和侧倾角的影响。杨文武等针对无人船，综合考虑船身的三个姿态角和位置变化，以矫正激光雷达点云的畸变(专利申请号：201911222073.4)。
5.大型救援装备中最常使用的是履带式挖掘机，装备的上部转台和行走机构可独立工作，本发明以挖掘机为例介绍提出的方法。装备行驶时，不得做回转动作，激光雷达与装备的位置变化保持一致；装备进行回转操作时，装备的位置保持不变，而激光雷达进行圆周运动。然而现有方法认为激光雷达与其载体的位置变化相同，未考虑激光雷达与其载体的位置变化相异的场景。大型救援装备作业和在凹凸不平路面上行驶时，激光雷达安装处的俯仰角、侧倾角和航向角会持续变化，通过传感器测量存在一定的误差。然而现有方法未考虑如何保障三个角度测量的准确性。


技术实现要素：

6.针对以上问题，本发明提出一种面向大型救援装备的激光雷达点云畸变矫正方法，该方法兼顾激光雷达处旋转的角度和角速度，保证旋转变换矩阵的准确性；分析大型救援装备灾后施救的操作，将激光雷达处的位置变化分为两种，提高平移变换矩阵使用场景的适应性；利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声，进而提高激光雷达点云畸变矫正的准确性。
7.本发明提供一种面向大型救援装备的激光雷达点云畸变矫正方法，具体步骤如
下：
8.步骤一：利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵；
9.激光雷达固定在装备驾驶舱的顶部，坐标系满足右手准则，z轴正方向指向驾驶舱的正上方，y轴正方向指向驾驶舱的正前方，x轴正方向指向驾驶舱的正右方；
10.在一帧点云中，第i个激光点的坐标系需要依次绕x,y,z轴旋转a
xi
,a
yi
,a
zi
度才能与第1个激光点的坐标系保持对应坐标轴同向且平行，旋转方向满足右手法则记为正值，反之记为负值；
[0011]11.分别为第i个激光点对应的激光雷达处俯仰角、侧倾角、航向角，角度增大方向满足右手法则；分别为第j个激光点时三个角度对应的角速度，j＝1,2,
…
,i-1；f为同一帧点云中相邻两个激光点的间隔时间；
[0012]
第i个激光点的坐标系绕x轴旋转a
xi
度，旋转变换矩阵为第i个激光点的坐标系绕y轴旋转a
yi
度，旋转变换矩阵为第i个激光点的坐标系绕z轴旋转a
zi
度，旋转变换矩阵为
[0013]
第i个激光点的旋转变换矩阵f为：
[0014][0015]
步骤二：利用激光雷达处位置变化确定点云的平移变换矩阵；
[0016]
装备行驶时，以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向；在坐标系中，第i个激光点对应的激光雷达位置坐标为sj为第j个激光点时装备的速度；bj为第j个激光点时装备行驶方向与正东方向的夹角，为装备行走机构中心处的航向角；坐标系逆时针旋转度得到第1个激光点的xy坐标系，在第1个激光点的xy坐标系中，第i个激光点的平移变换矩阵为
[0017]
装备执行回转操作时，以上部转台的旋转中心为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向；在坐标系中，第1个激光点对应的激
光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离；ei为在坐标系中第i个激光点对应的激光雷达位置的极角，在坐标系中，第i个激光点对应的激光雷达位置坐标为在第1个激光点的xy坐标系中，第i个激光点的平移变换矩阵为
[0018]
第i个激光点的平移变换矩阵为：
[0019][0020]
步骤三：利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性；
[0021]
针对装备救援过程，取状态量为其中，分别为激光雷达处的俯仰角、侧倾角和航向角，分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度，分别为装备行走机构中心处的航向角和航向角速度；以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量，即其中，分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角，为装备行走机构中心处惯性传感器输出的航向角；
[0022]
离散化后的卡尔曼滤波的状态方程和观测方程为：
[0023][0024]
式中，t表示离散化时刻；a表示状态转移矩阵，c
表示离散周期；h表示观测矩阵，q和r是互不相关的零均值系统白噪声和观测白噪声向量，q对应的系统噪声协方差阵为q，r对应的观测噪声协方差阵为r；
[0025]
递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新；
[0026]
时间更新：
[0027][0028][0029]
测量更新：
[0030][0031][0032][0033]
利用卡尔曼滤波状态量优化后的旋转变换矩阵为平移变换矩阵为定义激光雷达一帧点云数据中第i个激光点的原始坐标为则畸变矫正后的第i个激光点的坐标为计算公式如下；
[0034][0035]
有益效果：
[0036]
1.本发明兼顾激光雷达坐标系旋转的角度和角速度，从而保证旋转变换矩阵的准确性；
[0037]
2.本发明分析大型救援装备灾后施救的操作，将激光雷达处的位置变化分为两种，提高平移变换矩阵使用场景的适应性；
[0038]
3.本发明利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声，进而提高激光雷达点云畸变矫正的准确性。
附图说明
[0039]
图1为用于大型救援装备的激光雷达点云畸变矫正方法总体设计方案图；
[0040]
图2为激光雷达坐标系旋转方向的示意图。
具体实施方式
[0041]
以下将结合具体实例对本发明提供的技术方案进行详细说明，应理解下述具体实
施方式仅用于说明本发明而不用于限制本发明的范围。
[0042]
本发明提出了一种面向大型救援装备的激光雷达点云畸变矫正方法。首先利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵，然后利用激光雷达处位置变化确定点云的平移变换矩阵，最后利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性。该方法兼顾激光雷达处旋转的角度和角速度，保证旋转变换矩阵的准确性；分析大型救援装备灾后施救的操作，将激光雷达处的位置变化分为两种，提高平移变换矩阵使用场景的适应性；利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声，进而提高激光雷达点云畸变矫正的准确性。
[0043]
总体设计方案如图1所示，具体步骤包括：
[0044]
步骤一：利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵；
[0045]
激光雷达固定在装备驾驶舱的顶部，坐标系满足右手准则，z轴正方向指向驾驶舱的正上方，y轴正方向指向驾驶舱的正前方，x轴正方向指向驾驶舱的正右方。激光雷达在垂直方向上有多个激光器，同一时刻发出多束激光。由于每束激光的点云矫正方法相同，故本发明对单束激光的点云矫正方法进行介绍。
[0046]
在一帧点云中，第i个激光点的坐标系需要依次绕x,y,z轴旋转a
xi
,a
yi
,a
zi
度才能与第1个激光点的坐标系保持对应坐标轴同向且平行，旋转方向满足右手法则记为正值，反之记为负值，如图2所示。
[0047]047]
分别为第i个激光点对应的激光雷达处俯仰角、侧倾角、航向角，角度增大方向满足右手法则，在本发明中航向角定义为与正北方向的夹角；分别为第j个激光点时三个角度对应的角速度，j＝1,2,
…
,i-1；f为同一帧点云中相邻两个激光点的间隔时间，通过激光雷达的产品手册获得。
[0048]
第i个激光点的坐标系绕x轴旋转a
xi
度，旋转变换矩阵为第i个激光点的坐标系绕y轴旋转a
yi
度，旋转变换矩阵为第i个激光点的坐标系绕z轴旋转a
zi
度，旋转变换矩阵为
[0049]
因此，第i个激光点的旋转变换矩阵f为：
[0050][0051]
步骤二：利用激光雷达处位置变化确定点云的平移变换矩阵；
[0052]
灾后现场施救时，在激光雷达旋转一周的时间里，大型救援装备可看成做平面运动，即认为激光雷达位置在垂直方向上保持不变。装备行驶时，由于不得做回转动作，激光
雷达与装备的位置变化保持一致；装备进行回转操作时，装备位置保持不变，而激光雷达进行圆周运动。
[0053]
装备行驶时，以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向。在坐标系中，第i个激光点对应的激光雷达位置坐标为sj为第j个激光点时装备的速度，通过行走马达信息换算获得；bj为第j个激光点时装备行驶方向与正东方向的夹角，为装备行走机构中心处的航向角。坐标系逆时针旋转度得到第1个激光点的xy坐标系，在第1个激光点的xy坐标系中，第i个激光点的平移变换矩阵为
[0054]
装备执行回转操作时，以上部转台的旋转中心为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向。在坐标系中，第1个激光点对应的激光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离，事先静态测量获得；ei为在坐标系中第i个激光点对应的激光雷达位置的极角，在坐标系中，第i个激光点对应的激光雷达位置坐标为在第1个激光点的xy坐标系中，第i个激光点的平移变换矩阵为
[0055]
因此，第i个激光点的平移变换矩阵为：
[0056][0057]
步骤三：利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性
[0058]
旋转变换矩阵f和平移变换矩阵b中的参数涉及到激光雷达处的三轴角度、角速度和行走机构中心处的航向角，考虑到灾后施救时大型救援装备处于动态，惯性传感器所获得的数据存在较大的随机误差，进而影响激光雷达点云畸变矫正的准确性。因此利用卡尔曼滤波提高上述参数的精度。
[0059]
针对装备救援过程，取状态量为其中，
分别为激光雷达处的俯仰角、侧倾角和航向角，分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度，分别为装备行走机构中心处的航向角和航向角速度。以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量，即其中，分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角，为装备行走机构中心处惯性传感器输出的航向角。
[0060]
离散化后的卡尔曼滤波的状态方程和观测方程为：
[0061][0062]
式中，t表示离散化时刻；a表示状态转移矩阵，c表示离散周期；h表示观测矩阵，由于状态量的参数中包含观测量的参数，故表示离散周期；h表示观测矩阵，由于状态量的参数中包含观测量的参数，故q和r是互不相关的零均值系统白噪声和观测白噪声向量，q对应的系统噪声协方差阵为q，r对应的观测噪声协方差阵为r。
[0063]
递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新；
[0064]
时间更新：
[0065][0066][0067]
测量更新：
[0068][0069][0070][0071]
利用卡尔曼滤波状态量优化后的旋转变换矩阵为平移变换矩阵为定义激光雷达一帧点云数据中第i个激光点的原始坐标为则畸变矫正后的第i个激光点的坐标为计算公式如下；
[0072]