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

权利要求书

1.一种面向大型救援装备的激光雷达点云畸变矫正方法，其特征在于，具体步骤如下：

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

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

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

分别为第i个激光点对应的激光雷达处俯仰角、侧倾角、航向角，角度增大方向满足右手法则；分别为第j个激光点时三个角度对应的角速度，j＝1,2,…,i-1；f为同一帧点云中相邻两个激光点的间隔时间；

第i个激光点的坐标系绕X轴旋转a_xi度，旋转变换矩阵为第i个激光点的坐标系绕Y轴旋转a_yi度，旋转变换矩阵为第i个激光点的坐标系绕Z轴旋转a_zi度，旋转变换矩阵为

第i个激光点的旋转变换矩阵F为：

步骤二：利用激光雷达处位置变化确定点云的平移变换矩阵；

装备行驶时，以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向；在坐标系中，第i个激光点对应的激光雷达位置坐标为s_j为第j个激光点时装备的速度；b_j为第j个激光点时装备行驶方向与正东方向的夹角，为装备行走机构中心处的航向角；坐标系逆时针旋转度得到第1个激光点的XY坐标系，在第1个激光点的XY坐标系中，第i个激光点的平移变换矩阵为

装备执行回转操作时，以上部转台的旋转中心为原点建立平面直角坐标系的正方向指向正东方向，的正方向指向正北方向；在坐标系中，第1个激光点对应的激光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离；e_i为在坐标系中第i个激光点对应的激光雷达位置的极角，在坐标系中，第i个激光点对应的激光雷达位置坐标为在第1个激光点的XY坐标系中，第i个激光点的平移变换矩阵为

第i个激光点的平移变换矩阵为：

步骤三：利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性；

针对装备救援过程，取状态量为其中，分别为激光雷达处的俯仰角、侧倾角和航向角，分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度，分别为装备行走机构中心处的航向角和航向角速度；以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量，即其中，分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角，为装备行走机构中心处惯性传感器输出的航向角；

离散化后的卡尔曼滤波的状态方程和观测方程为：

式中，t表示离散化时刻；A表示状态转移矩阵，c表示离散周期；H表示观测矩阵，q和r是互不相关的零均值系统白噪声和观测白噪声向量，q对应的系统噪声协方差阵为Q，r对应的观测噪声协方差阵为R；

递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新；

时间更新：

测量更新：

利用卡尔曼滤波状态量优化后的旋转变换矩阵为平移变换矩阵为定义激光雷达一帧点云数据中第i个激光点的原始坐标为则畸变矫正后的第i个激光点的坐标为计算公式如下；