[发明专利]一种激光雷达与IMU紧耦合的定位方法

权利要求书

1.一种基于激光雷达和IMU紧耦合的机器人定位方法，包括如下步骤：

(1)读取IMU的输出数据，通过运动模型对机器人的实时运动状态进行估计；

(2)读取激光雷达的输出数据，利用观测模型对由IMU估计得到的机器人运动状态进行纠正；具体地：激光雷达的输出数据为点云数据，通过提取点云数据中的平面特征并对其进行跟踪，通过跟踪在滑动窗口中不同激光雷达状态下的多个观测约束来对由IMU估计得到的机器人运动状态进行纠正；

提取点云数据中平面特征的方法为：首先利用随机霍夫变换计算点云中每个点的法向量，根据点与点之间的欧式距离差值以及法向量夹角差值进行区域生长聚类；然后，对于每个聚类通过PCA计算关于其点云集合位置的特征向量和特征值，若最小特征值与第二大特征值的比值大于一定阈值，则认为该聚类不是平面，否则计算最大的两个特征值所对应的特征向量的叉乘得到平面的法向量，计算聚类中点到该平面的平均距离，若平均距离大于一定阈值，说明该平面不够平整，丢弃该聚类，否则将该聚类对应的平面作为平面特征提取出来；

通过随机霍夫变换进行平面特征提取时，首先使用KD树查找每个点附近的K个最近邻点，然后在近邻点中多次随机选点进行霍夫投票得到该点的法向量；其中近邻点的查找过程具体为：将点云数据通过球面投影得到其前视图，以每个点在前视图中的坐标为矩形框中心，从固定尺寸的矩形框内提取像素点所对应的点云作为潜在近邻点，并度量潜在近邻点到中心点的距离，将距离小于一定阈值的作为近邻点进行后续的随机霍夫变换；

对平面特征进行跟踪的方法为：首先将上一帧的点云数据经过球面投影得到其前视图，计算上一帧平面特征对应的聚类在该前视图中的2D包围盒；然后，在当前帧的点云数据中依次计算包围盒中的每个点到该包围盒所对应的上一帧平面特征的距离，将距离小于一定阈值的点作为inlier点，当inlier点数量大于一定阈值且inlier点数量占包围盒中所有点数量的比例大于一定阈值时，认为平面特征跟踪成功，并根据inlier点计算平面特征在下一帧点云数据中的参数方程，否则认为平面特征跟踪失败；

(3)反复迭代执行步骤(1)和(2)，使用步骤(2)纠正后的机器人运动状态作为下次迭代时步骤(1)中所需的初始状态信息。

2.根据权利要求1所述的机器人定位方法，其特征在于：所述IMU的输出数据包括机器人的加速度和角速度，机器人的运动状态包括速度、位置以及朝向；步骤(1)通过维护一个固定大小的滑动窗口以保存机器人运动轨迹中最新的若干个激光雷达状态，从而实现对机器人实时运动状态的估计，滑动窗口中的激光雷达状态整体表示为其中：m为滑动窗口大小，T表示转置，为单位四元数且表示全局坐标系G到IMU坐标系I的旋转，b_g和b_a均为3×1大小的向量且分别表示影响IMU中陀螺仪和加速度计的测量偏差，^Gp_I和^Gv_I均为3×1大小的向量且分别表示IMU相对于全局坐标系G的位置和速度，为滑动窗口中的第i个激光雷达状态，表示全局坐标系G到第i个激光雷达状态的旋转，表示第i个激光雷达状态在全局坐标系G中的位置。

3.根据权利要求2所述的机器人定位方法，其特征在于：所述步骤(1)中的运动模型表达式如下：

其中：表示当前时刻全局坐标系G到IMU坐标系I的旋转，表示关于时间的导数，ω表示当前时刻机器人的角速度，表示当前时刻机器人的加速度，表示当前时刻机器人相对于全局坐标系G的速度，[ω_×]为ω的偏置矩阵，表示^Gp_I关于时间的导数，表示关于时间的导数，C()表示用于将四元数转换为对应旋转矩阵的函数，^Gg为全局坐标系G下的重力加速度，和分别为b_g和b_a关于时间的导数，0_3×1为3×1大小且元素值全为0的向量。