[发明专利]一种鲁棒的激光雷达-惯导标定方法

权利要求书

1.一种鲁棒的激光雷达-惯导联合标定方法，其特征在于，包括如下步骤：

1)对惯导和激光雷达采集的数据分别进行预处理后，分别获得惯导和激光雷达的初始位姿估计结果，再对惯导和激光雷达的位姿估计结果进行位姿对齐，获得激光雷达与惯导之间的初始外参矩阵；

2)基于惯导的初始位姿估计结果以及激光雷达与惯导之间的初始外参矩阵对激光雷达采集的数据进行去畸变处理，获得去畸变后的点云，再从去畸变后的点云中提取线特征点云，基于去畸变后的点云和线特征点云分别构建面片地图和线特征地图；

3)根据惯导的初始位姿估计结果以及激光雷达与惯导之间的初始外参矩阵分别对去畸变后的点云与面片地图、线特征点云与线特征地图进行迭代优化配准，优化估计激光雷达与惯导之间的外参以及惯导的内参。

2.根据权利要求1所述的一种鲁棒的激光雷达-惯导联合标定方法，其特征在于：所述步骤1)具体为：

1.1)对惯导采集的数据进行预积分处理，获得惯导的初始位姿估计结果；

1.2)利用正态分布变换NDT算法对激光雷达采集的原始点云进行逐帧匹配，获得激光雷达的初始位姿估计结果；

1.3)测量并获得激光雷达与惯导之间的平移向量和惯导的重力方向测量值，基于激光雷达与惯导之间的平移向量和惯导的重力方向测量值，再根据惯导和激光雷达的位姿估计结果进行位姿对齐，获得激光雷达与惯导之间的初始外参矩阵。

3.根据权利要求2所述的一种鲁棒的激光雷达-惯导联合标定方法，其特征在于：所述步骤1.3)中激光雷达与惯导之间的初始外参矩阵的计算公式如下：

其中，表示激光雷达与惯导之间的初始外参矩阵，表示激光雷达和惯导之间第k帧位姿对齐的残差，表示第k帧惯导的重力方向估计值与测量值之间的残差，r_t表示激光雷达与惯导之间平移向量的残差，表示激光雷达的初始位姿估计结果中第0帧到第k帧的位姿增量，K为总帧数，k为帧序号，表示惯导的初始位姿估计结果中第0帧到第k帧的位姿增量，()^-1表示矩阵求逆，和分别表示惯导的第k帧翻转角、俯仰角的估计值，和分别表示惯导第k帧翻转角、俯仰角的测量值，表示激光雷达与惯导之间平移向量的估计值，表示激光雷达与惯导之间平移向量的测量值，|| ||²表示二范数操作，||表示绝对值操作，arg min{*}表示取最小值操作。

4.根据权利要求1所述的一种鲁棒的激光雷达-惯导联合标定方法，其特征在于：所述步骤2)中基于去畸变后的点云和线特征点云分别构建初始面片地图和初始线特征地图，具体为：

S1：利用NDT算法对去畸变后的点云进行逐帧匹配，从而对激光雷达的初始位姿估计结果进行更新，获得激光雷达的更新后的位姿估计结果；

S2：根据激光雷达的更新后的位姿估计结果，将去畸变后的点云投影到地图坐标系，并基于随机采样一致性RANSAC方法提取出平面结构，从而构建出初始面片地图；

S3：根据激光雷达的更新后的位姿估计结果，将线特征点云投影到地图坐标系，从而构建出初始线特征地图。