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

(12)发明专利申请

(10)申请公布号 CN 111366153 A(43)申请公布日 2020.07.03

(21)申请号 202010197086.7(22)申请日 2020.03.19

(71)申请人 浙江大学

地址 310013 浙江省杭州市西湖区余杭塘

路866号(72)发明人 吕攀　张恒　杨国青　李红　

潘之杰　吴朝晖　(74)专利代理机构 杭州天勤知识产权代理有限

公司 33224

代理人 王琛(51)Int.Cl.

G01C 21/16(2006.01)G01S 17/02(2020.01)G01S 17/87(2020.01)

权利要求书3页 说明书6页 附图2页

()发明名称

一种激光雷达与IMU紧耦合的定位方法(57)摘要

本发明公开了一种基于激光雷达和IMU紧耦合的机器人定位方法，通过IMU对系统实时运动状态进行估计，通过激光雷达的点云数据提取平面特征进行跟踪，并利用平面特征的观测模型对IMU估计的运动状态进行纠正。本发明方法利用MSCKF框架对激光雷达和IMU两种传感器实现了优势互补，一方面IMU高频的运动估计能够对系统的实时状态进行估计，另一方面激光雷达点云提取的平面特征能够鲁棒地在环境中被跟踪，从而能够定时对系统状态进行纠正，且本发明在降低计算复杂度的同时，也保持了定位的鲁棒性和准确性。CN 111366153 ACN 111366153 A

权　利　要　求　书

1/3页

1.一种基于激光雷达和IMU紧耦合的机器人定位方法，包括如下步骤：(1)读取IMU的输出数据，通过运动模型对机器人的实时运动状态进行估计；(2)读取激光雷达的输出数据，利用观测模型对由IMU估计得到的机器人运动状态进行纠正；

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

2.根据权利要求1所述的机器人定位方法，其特征在于：所述IMU的输出数据包括机器人的加速度和角速度，机器人的运动状态包括速度、位置以及朝向；步骤(1)通过维护一个固定大小的滑动窗口以保存机器人运动轨迹中最新的若干个激光雷达状态，从而实现对机器人实时运动状态的估计，所述激光雷达状态表示为

T

其中：m为滑动窗口大小，表示转置，为单位四元数且表示全局坐标

系G到IMU坐标系I的旋转，bg和ba均为3×1大小的向量且分别表示影响IMU中陀螺仪和加速G

度计的测量偏差，pI和GvI均为3×1大小的向量且分别表示IMU相对于全局坐标系G的位置和速度，为滑动窗口中的第i个激光雷达状态，表示全局坐标系G到第i个激光雷达状态的旋转，

表示第i个激光雷达状态在全局坐标系G中的位置。

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

其中：表示当前时刻全局坐标系G到IMU坐标系I的旋转，表示数，度，

关于时间的导

表示当前时刻机器人的角速度，表示当前时刻机器人的加速

为的偏置矩阵，

表示GpI关于时间的导数，表示GvI关于时间的导数，C()表

分

G

示用于将四元数转换为对应旋转矩阵的函数，g为全局坐标系G下的重力加速度，和

别为bg和ba关于时间的导数，03×1为3×1大小且元素值全为0的向量。

4.根据权利要求1所述的机器人定位方法，其特征在于：所述激光雷达的输出数据为点云数据，步骤(2)通过提取点云数据中的平面特征并对其进行跟踪，通过跟踪在滑动窗口中不同激光雷达状态下的多个观测约束来对由IMU估计得到的机器人运动状态进行纠正。

5.根据权利要求4所述的机器人定位方法，其特征在于：提取点云数据中平面特征的方法为：首先利用随机霍夫变换计算点云中每个点的法向量，根据点与点之间的欧式距离差

2

CN 111366153 A

权　利　要　求　书

2/3页

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

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

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

8.根据权利要求4所述的机器人定位方法，其特征在于：跟踪成功的平面特征都有滑动窗口中多个不同激光雷达状态所对应平面特征的观测，根据多个观测计算平面在全局坐标系中的参数方程，进而通过平面在不同坐标系的参数变换得到平面的在局部坐标系下的观测模型，累计跟踪成功的平面特征在不同激光雷达状态下的观测模型得到平面特征对滑动窗口中多个激光雷达状态的观测约束；最后根据平面特征的观测模型以及MSCKF的状态更新方法对由IMU估计得到的机器人运动状态进行纠正。

9.根据权利要求8所述的机器人定位方法，其特征在于：所述平面特征的表达式为

为滑动窗口中第i个激光雷达状态观测到的第j个平面特征，

平面特征的单位法向量，

为该

为第i个激光雷达状态到该平面的距离，则第i个激光雷达状态

对全局坐标系下第j个平面特征GΠj的观测方程如下：

进而根据上式得到观测误差函数如下：

i其中：zj为滑动窗口中第i个激光雷达状态对平面特征GΠj的观测，C()表示用于将四G

nj为元数转换为对应旋转矩阵的函数，表示全局坐标系G到第i个激光雷达状态的旋转，G

平面特征GΠj的单位法向量，dj为第i个激光雷达状态到GΠj对应平面的距离，

表示第i

Ti个激光雷达状态在全局坐标系G中的位置，表示转置，rj为滑动窗口中第i个激光雷达状态

对平面特征GΠj的观测误差，

为观测方程对滑动窗口中第i个激光雷达状态的雅可比矩

3

CN 111366153 A

权　利　要　求　书

3/3页

阵，为滑动窗口中第i个激光雷达状态的误差，数误差，

为全局坐标系下第j个平面特征的参

为全局

为观测方程对全局坐标系下第j个平面特征参数的雅可比矩阵，

坐标系下第j个平面特征的观测噪声；

将上述观测方程及观测误差函数组成观测模型加入MSCKF的状态更新框架中进行计算，即可同时对由IMU估计得到的机器人运动状态以及滑动窗口中激光雷达状态进行纠正。

4

CN 111366153 A

说　明　书

一种激光雷达与IMU紧耦合的定位方法

1/6页

技术领域

[0001]本发明属于机器人定位技术领域，具体涉及一种基于激光雷达和IMU紧耦合的机器人定位方法。

背景技术

[0002]自主导航是移动机器人自动运行的一种关键技术，目前最主流的导航技术是SLAM的方式，中文意思是“即时定位与地图构建”，其原理是通过传感器对周围环境进行扫描，然后构建一个和真实环境一致的地图，同时对机器人位置进行定位，并规划一条正确的路径，最终引导机器人安全到达指定的目的地。

[0003]定位技术是实现机器人自主导航的支撑技术，定位是否精准决定了机器人能否精准高效地完成更高层的规划导航等功能。定位技术中，可以使用的主要传感器有：GPS、激光雷达、相机、IMU(惯性测量单元)等，其中GPS能够提供全局定位，但精度不高，在遮挡环境中信号很差；激光雷达能够精准地对环境信息进行距离测量，得到机器人的相对位置，但点云数据一般比较稀疏，而点云数据稠密时又需要更高的计算复杂度以对点云进行处理；相机能够得到丰富的环境外观信息，但单目相机难以实现距离的感知；IMU能够直接观测到机器人的角速度和加速度等运动信息，而且频率较高，但IMU存在零偏问题，并且单纯对速度进行积分很容易出现累计误差问题。

[0004]因此为了解决机器人的自主定位问题，研究人员一般会使用多种传感器，利用不同传感器的优势互补来提高定位的精度和鲁棒性。

[0005]公开号为US2016327395A1的美国专利提出了一种基于滑动窗口的 VINS(vision-aided inertial system)系统，融合IMU和相机进行定位导航，其主要思想是利用IMU进行运动估计，利用图像的观测信息进行状态的更新，其中对跟踪短的图像特征点使用MSCKF进行状态更新，对跟踪长的图像特征点使用 SLAM进行状态更新。该专利技术一定程度上提高了VINS系统的定位性能，但该专利是基于相机与IMU融合的，当环境光照有明显变化或光照较暗时，系统容易定位失败。

[0006]公开号为CN110428467A的中国专利提出了一种相机、IMU和激光雷达联合的机器人定位方法，其主要思想是通过图优化方法对多种传感器进行融合，利用IMU进行预积分得到帧间约束，当光照稳定且点云纹理信息丰富时，利用激光雷达测距精准的特点得到图像特征点的深度信息对位姿进行优化，当光照不好或点云纹理信息较少，则利用激光雷达和局部点云匹配对位姿进行优化，同时利用图像的单词信息和IMU数据进行回环检测。该专利技术一定程度上弥补了相机在光照变化敏感时容易定位失败的情况，但在图像特征跟踪失效时依然单纯利用激光雷达的局部点云匹配进行位姿优化，计算量较大而且单纯的点云匹配会由于点云的稀疏特点容易出现匹配失败的情况。

发明内容

[0007]鉴于上述，本发明提供了一种基于激光雷达和IMU紧耦合的机器人定位方法，通过

5

CN 111366153 A

说　明　书

2/6页

快速且鲁棒地从激光雷达点云中检测环境中的平面特征并进行跟踪和匹配，将基于激光雷达平面特征的观测模型与IMU的运动模型通过 MSCKF(Multi-State Constraint Kalman Filter，多状态约束下的卡尔曼滤波器)紧耦合，最大程度地利用了激光雷达与IMU传感器的优势互补，在降低计算复杂度的同时也保证了定位的精准和鲁棒。[0008]一种基于激光雷达和IMU紧耦合的机器人定位方法，包括如下步骤：[0009](1)读取IMU的输出数据，通过运动模型对机器人的实时运动状态进行估计；[0010](2)读取激光雷达的输出数据，利用观测模型对由IMU估计得到的机器人运动状态进行纠正；[0011](3)反复迭代执行步骤(1)和(2)，使用步骤(2)纠正后的机器人运动状态作为下次迭代时步骤(1)中所需的初始状态信息。[0012]进一步地，所述IMU的输出数据包括机器人的加速度和角速度，机器人的运动状态包括速度、位置以及朝向；步骤(1)通过维护一个固定大小的滑动窗口以保存机器人运动轨迹中最新的若干个激光雷达状态，从而实现对机器人实时运动状态的估计，所述激光雷达状态表示为

T

其中：m为滑动窗口大小，表示转置，

为单位四元数且表示全局坐标系G到IMU坐标系I的旋转，bg和ba均为3×1大小的向量且

G

分别表示影响IMU中陀螺仪和加速度计的测量偏差，pI和GvI均为3×1大小的向量且分别表

示IMU相对于全局坐标系G的位置和速度，为滑动窗口中的第i个激光雷达状态，全局坐标系G到第i个激光雷达状态的旋转，

表示

表示第i个激光雷达状态在全局坐标系G中

的位置。

[0013]进一步地，所述步骤(1)中的运动模型表达式如下：

[0014][0015][0016][0017]

其中：表示当前时刻全局坐标系G到IMU坐标系I的旋转，表示关于时间的

导数，ω表示当前时刻机器人的角速度，表示当前时刻机器人的加速

表示GpI关于时间的导数，表示GvI关于时间的导数，C()表

度，[ω×]为ω的偏置矩阵，

G示用于将四元数转换为对应旋转矩阵的函数，g 为全局坐标系G下的重力加速度，和

分别为bg和ba关于时间的导数，03×1为 3×1大小且元素值全为0的向量。[0018]进一步地，所述激光雷达的输出数据为点云数据，步骤(2)通过提取点云数据中的平面特征并对其进行跟踪，通过跟踪在滑动窗口中不同激光雷达状态下的多个观测约束来

6

CN 111366153 A

说　明　书

3/6页

对由IMU估计得到的机器人运动状态进行纠正。[0019]进一步地，提取点云数据中平面特征的方法为：首先利用随机霍夫变换计算点云中每个点的法向量，根据点与点之间的欧式距离差值以及法向量夹角差值进行区域生长聚类；然后，对于每个聚类通过PCA(主成分分析)计算关于其点云集合位置的特征向量和特征值，若最小特征值与第二大特征值的比值大于一定阈值，则认为该聚类不是平面，否则计算最大的两个特征值所对应的特征向量的叉乘得到平面的法向量，计算聚类中点到该平面的平均距离，若平均距离大于一定阈值，说明该平面不够平整，丢弃该聚类，否则将该聚类对应的平面作为平面特征提取出来。[0020]进一步地，通过随机霍夫变换进行平面特征提取时，首先使用KD树查找每个点附近的K个最近邻点，然后在近邻点中多次随机选点进行霍夫投票得到该点的法向量；其中近邻点的查找过程具体为：将点云数据通过球面投影得到其前视图，以每个点在前视图中的坐标为矩形框中心，从固定尺寸的矩形框内提取像素点所对应的点云作为潜在近邻点，并度量潜在近邻点到中心点的距离，将距离小于一定阈值的作为近邻点进行后续的随机霍夫变换。该方法在极大提升近邻查找速度进而提升整体平面检测速度的同时，仅损失了稍微的法向量计算准确度，但对整体的平面检测准确度无明显影响。[0021]进一步地，对平面特征进行跟踪的方法为：首先将上一帧的点云数据经过球面投影得到其前视图，计算上一帧平面特征对应的聚类在该前视图中的2D包围盒(bounding box)；然后，在当前帧的点云数据中依次计算包围盒中的每个点到该包围盒所对应的上一帧平面特征的距离，将距离小于一定阈值的点作为 inlier点，当inlier点数量大于一定阈值且inlier点数量占包围盒中所有点数量的比例大于一定阈值时，认为平面特征跟踪成功，并根据inlier点计算平面特征在下一帧点云数据中的参数方程，否则认为平面特征跟踪失败。

[0022]进一步地，跟踪成功的平面特征都有滑动窗口中多个不同激光雷达状态所对应平面特征的观测，根据多个观测计算平面在全局坐标系中的参数方程，进而通过平面在不同坐标系的参数变换得到平面的在局部坐标系下的观测模型，累计跟踪成功的平面特征在不同激光雷达状态下的观测模型得到平面特征对滑动窗口中多个激光雷达状态的观测约束；最后根据平面特征的观测模型以及MSCKF的状态更新方法对由IMU估计得到的机器人运动状态进行纠正。

[0023]

进一步地，所述平面特征的表达式为为滑动窗口中第i 个

为第i个激光

激光雷达状态观测到的第j个平面特征，为该平面特征的单位法向量，

雷达状态到该平面的距离，则第i个激光雷达状态对全局坐标系下第j个平面特征GΠj的观

测方程如下：

[0024][0025][0026][0027]

i

其中：zj为滑动窗口中第i个激光雷达状态对平面特征GΠj的观测，C()表示用于

进而根据上式得到观测误差函数如下：

将四元数转换为对应旋转矩阵的函数，表示全局坐标系G到第i个激光雷达状态的旋转

7

CN 111366153 A

说　明　书

4/6页

GG，nj为平面特征GΠj的单位法向量，dj为第i个激光雷达状态到GΠj对应平面的距离，

表

Ti示第i个激光雷达状态在全局坐标系G中的位置，表示转置，rj为滑动窗口中第i个激光雷

达状态对平面特征GΠj的观测误差，

为观测方程对滑动窗口中第i个激光雷达状态的雅

为全局坐标系下第j个平面特

可比矩阵，为滑动窗口中第i个激光雷达状态的误差，征的参数误差，

为观测方程对全局坐标系下第j个平面特征参数的雅可比矩阵，

为全局坐标系下第j个平面特征的观测噪声；

[0028]将上述观测方程及观测误差函数组成观测模型加入MSCKF的状态更新框架中进行计算，即可同时对由IMU估计得到的机器人运动状态以及滑动窗口中激光雷达状态进行纠正。

[0029]本发明的有益技术效果在于：提出了一种激光雷达平面特征的提取、跟踪、匹配方法，能够快速且鲁棒地检测环境中的平面特征；提出了一种基于激光雷达平面特征的观测模型，能够整合到MSCKF融合框架中并有效地实现激光雷达与IMU的紧耦合定位，实现激光雷达和IMU的优势互补，在保持定位精度和鲁棒性的同时降低了计算复杂度。附图说明

[0030]图1为本发明机器人定位方法的流程示意图。[0031]图2为点云数据的平面特征提取示意图。

[0032]图3为本发明的平面特征提取与跟踪流程示意图。

具体实施方式

[0033]为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。

[0034]如图1所示，本发明基于激光雷达和IMU紧耦合的机器人定位方法，包括如下步骤：[0035]S1.读取IMU的输出数据，通过运动模型对系统的实时运动状态进行估计。[0036]系统的运动状态至少包含机器人的位置、朝向以及速度等信息，并维护一个关于激光雷达位姿的固定大小滑动窗口，滑动窗口中保存机器人运动轨迹中最新的m个激光雷达的位置、朝向等信息。IMU以100Hz的频率输出加速度和角速度数据，根据IMU的运动模型对系统的运动状态进行估计，得到高频但存在偏差的系统状态，系统状态向量为

其中m为滑动窗口大小， IMU运动模型如下：

[0037]

8

CN 111366153 A

说　明　书

5/6页

[0038]其中：

[ω×]为ω的偏置矩阵，C(·)为将四元数转换为对应

G

的旋转矩阵的函数，g为重力加速度。[0039]S2.读取激光雷达的输出数据，通过观测模型对IMU估计的运动状态进行纠正。[0040]激光雷达以10Hz的频率输出点云数据，通过平面特征提取和跟踪模块对原始点云进行处理，将跟踪的平面特征根据本发明提出的观测模型对估计的系统状态进行纠正，得到精准连续的系统状态。

[0041]平面特征提取与跟踪模块的流程如图3所示，首先将上一帧的激光雷达点云数据经过球面投影到得到其前视图，计算上一帧平面特征对应的聚类在该前视图中的2Dboundingbox；在当前帧的激光雷达点云中，依次计算每个bounding box中的每个点到该boundingbox所对应的上一帧平面特征的距离，将距离小于一定阈值的点作为inlier点，当inlier数量大于一定阈值且inlier数量占bounding box中所有点数量的比例大于一定阈值时，认为平面特征跟踪成功，根据inlier 计算平面特征在下一帧点云中的参数方程，否则平面特征跟踪失败。当跟踪平面特征数量小于数量N时，重新从原始点云进行平面特征提取，提取方法为：通过随机霍夫变换计算点云中每个点的法向量，根据点与点之间的欧式距离差值以及法向量夹角差值进行区域生长聚类，对每个聚类通过PCA计算关于其点云集合位置的特征向量和特征值，若最小特征值与第二大特征值的比值大于一定阈值，则认为该聚类不是平面，若小于一定阈值则计算最大的两个特征值所对应的特征向量的叉乘得到平面的法向量，计算聚类中点到该平面的平均距离，若距离大于一定阈值，则该平面不够平整，丢弃该聚类，否则将该聚类对应的平面作为平面特征提取出来，最终提取的平面特征如图2所示，其中白色点为原始点云，其他点为平面特征所对应的聚类点云，与点相连的灰色线段为该点对应的法向量，最终提取的平面特征通过灰色椭圆显示其位置和朝向。[0042]在通过随机霍夫变换进行平面特征提取时，一般是使用KD树查找每个点附近的K个最近邻点，然后在近邻点中多次随机选点进行霍夫投票得到该点的法向量。而在大规模点云中，对每个点使用KD树进行近邻点查找耗时较长，因此在本发明中，针对这一近邻查找步骤根据激光雷达的扫描特点进行了重新设计，具体方法为：将点云数据通过球面投影得到其前视图，以每个点在前视图中的坐标为中心，提取宽高为w、h矩形框内像素点所对应的点云作为潜在近邻点，并度量潜在近邻点到中心点的距离，将距离小于一定阈值的作为近似近邻点进行后续的随机霍夫变换。通过图2可以看到使用这一方法计算的法向量比较准确，并能辅助完成准确的平面特征检测，同时也提升了平面检测的速度。

[0043]每个跟踪的平面特征都有滑动窗口中多个不同激光雷达位姿所对应的该平面特征的观测，通过该平面的多个观测计算其在全局坐标系中的参数方程，根据平面在不同坐标系的参数变换，得到平面的在局部坐标系下的观测模型，累计跟踪平面在不同激光雷达位姿下的观测模型得到该特征对滑动窗口中多个激光雷达状态的观测约束，进而根据平面特征的观测模型以及MSCKF的状态更新方法对IMU估计的状态进行纠正。

[0044]平面特征参数为为滑动窗口中第Li个雷达状态观测到的第j

为第Li个雷达状态到该平面的距

个局部平面特征，为该平面特征的单位法向量，

9

CN 111366153 A

说　明　书

6/6页

离，则雷达状态Li对全局坐标系下的平面特征GΠj的观测方程为：

[0045]

[0046]

根据式(2)可得观测误差函数为：

[0047]

[0048]其中：为观测方程对的Jacobian矩阵，为观测方程对的

Jacobian 矩阵，为全局平面特征参数，为平面特征的观测噪声；将式(2)和(3)作

为观测模型加入MSCKF的状态更新框架进行计算，即可同时对IMU状态和滑动窗口中激光雷达状态进行纠正。

[0049]S3.重复步骤S1～S2，使用S2纠正后的运动状态作为下次迭代时S1的初始状态信息。

[0050]上述对实施例的描述是为便于本技术领域的普通技术人员能理解和应用本发明。熟悉本领域技术的人员显然可以容易地对上述实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于上述实施例，本领域技术人员根据本发明的揭示，对于本发明做出的改进和修改都应该在本发明的保护范围之内。

10

CN 111366153 A

说　明　书　附　图

1/2页

图1

图2

11

CN 111366153 A

说　明　书　附　图

2/2页

图3

12