一种基于IMU预积分的激光SLAM定位方法

一种基于imu预积分的激光slam定位方法
技术领域
1.本发明涉及激光雷达建图与定位领域，特别涉及一种基于imu预积分的激光slam定位方法。


背景技术：

2.无人驾驶领域，作为未来的一大科技重点，实现高精准度自主导航是最终目标。在自主导航系统中，定位问题无疑是首先要解决的难题，准确的位姿估计是实现自主导航的关键技术，可以为建图、路径规划等技术提供精确位姿信息，是实现自主导航功能的第一步。
3.激光雷达可以通过点云形式对周围环境进行感知，但对于目前无人驾驶领域的长时定位需求和定位精度日益增长的高要求，依靠单一传感器已经无法满足。
4.imu能够根据载体运动情况，监测加速度与角速度信息，得到载体的位姿变化状态。但由于位姿状态通过积分获取，故位姿误差会随时间不断积累，长时间的运行会导致定位失准。


技术实现要素：

5.为了克服现有技术中的不足，本发明提供一种基于imu预积分的激光slam定位方法，通过多传感器数据融合，解决单个传感器无法满足长时定位需求和定位精度日益增长的高要求，以及有效处理位姿累积误差的问题，弥补激光雷达和imu各自的缺点，进而提高整个激光slam定位部分的位姿求解精度和速度。
6.为了达到上述发明目的，解决其技术问题所采用的技术方案如下：
7.一种基于imu预积分的激光slam定位方法，包括以下步骤：
8.步骤1：在获取当前imu与激光雷达测量数据的基础上，对当前时刻imu进行状态预测与预积分；
9.步骤2：利用imu状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；
10.步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；
11.步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；
12.步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；
13.步骤6：以lidar相对测量残差和imu预积分残差为约束条件，建立非线性最小二乘目标函数，并联合优化求解，将结果用于更新imu预测的状态变量，消除imu在传递过程中的误差。
14.进一步的，步骤1包括以下内容：
15.在新的激光雷达数据帧sj到来前，进行imu状态预测和预积分，假设当前时间戳为i，因imu的频率普遍远高于雷达点云数据的更新频率，在下一帧点云即时间戳j到来前，会
有大量的imu数据读入，可以根据从i到j这段时间间隔内的imu数据对imu的状态进行估计，同时利用这段时间间隔内的imu测量数据进行预积分操作；
16.imu状态预测为：
[0017][0018]
上式中，δt为采样频率，p为imu在世界坐标系下的位置，v为imu的速度，r为载体坐标系向世界坐标系的转换矩阵，am,a0分别为m时刻和初始时刻的加速度观测值，q为载体坐标系到世界坐标系的姿态变换四元数，wm,w0分别为m时刻和初始时刻的加速度观测值；
[0019]
通过上式离散化变换后，得到误差状态传播系统，如下式所示：
[0020]
δ
x
＝f(x,δ
x
,um,i)＝f
x
(x,um)
·
δ
x
+fi·iꢀꢀꢀꢀꢀꢀ
(0.12)
[0021]
上式中，x为预测状态，δ
x
为误差状态向量，um为输入向量(角速度、加速度测量值)，i为随机扰动脉冲向量；
[0022]
imu数据积分值为：
[0023][0024]
上式中，假定δt恒定，即采样频率不变，表示j时刻imu在世界坐标系下的位置，表示i时刻imu在世界坐标系下的位置，vk表示i时刻到j时刻中间离散的k时刻的速度，gw表示世界坐标系下的重力加速度，表示k时刻的载体坐标系向世界坐标系的转换矩阵，和wk表示k时刻的imu测量得到的加速度和陀螺仪的观测值，表示j时刻的imu的速度，表示i时刻的imu速度，表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
[0025]
对应imu预积分项为：
[0026][0027]
上式中，δp
ij
，δv
ij
，δq
ij
分别为预积分得到的imu在i时刻到j时刻的相对的位置、姿态和速度变化量，表示i时刻的载体坐标系向世界坐标系的转换矩阵，pi,pj分别为i时刻和j时刻imu在世界坐标系下的位置，vi,vj分别为i时刻和j时刻imu的速度，qi,qj分别为i时刻和j时刻imu载体坐标系到世界坐标系的姿态变换四元数，δt
ij
，δt为恒定采样频率，gw表示世界坐标系下的重力加速度，δv
ik
,δq
ik
分别为imu在i时刻到k时刻的相对的速度和姿态变化量，r表示imu载体坐标系向世界坐标系的转换矩阵，和wk表示k时刻的imu测量得到的加速度和陀螺仪的观测值。
[0028]
进一步的，步骤2包括以下内容：
[0029]
利用imu状态预测得到的预测值，消除雷达点云畸变，将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，即：
[0030][0031]
为点云初始时刻的位置和速度，为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，为第i帧雷达数据转换到世界坐标系下的矩阵，为点云i时刻的位置和速度，为以o时刻为起始点建立的局部坐标系下的畸变位移量。
[0032]
进一步的，步骤3包括以下内容：
[0033]
提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
[0034][0035]
上式中，s＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，表示点云q的坐标，为左右相邻的坐标。
[0036]
进一步的，步骤4包括以下内容：
[0037]
建立局部地图，地图中包含nm(o,
…
p,
…
,i)离散时刻，其中o为开始第一帧雷达扫描时间，p为关键帧的雷达扫描时间，i表示结束帧的雷达扫描时间，将提取到的每一帧雷达扫描中的平面点f
lγ
,γ∈{o,
…
,p,
…
,i}的数据，根据优化得到的位姿变化量将其他时刻的特征点转换到以p时刻为基准建立的坐标系中，记为以所有雷达帧的特征点建立一个局部地图需要估计的状态为时间段的状态，其中p+1表示关键帧p的下一时刻，j表示新的雷达扫描时刻。
[0038]
进一步的，步骤5包括以下内容：
[0039]
相对雷达测量，利用步骤4中构建的局部地图，找到和原始的特征点之间的对应关系，二者都是相对于关键帧p的位姿运行，关键帧p的位姿会随着测量值的加入而发生变化，利用knn算法可以找到的k个最近点将上述最近点拟合至坐标系f
lp
上的平面上，平面方程的系数为：
[0040]wt
x'+d＝0,x'∈π(x
lp
)
ꢀꢀꢀꢀꢀꢀ
(0.17)
[0041]
式中，w是平面的法向量，d是距离坐标系f
lp
原点的距离，两者均定义在坐标系f
lp
中，对于每个平面特征点x∈f
la
，m＝[x,w,d]∈m
la
是特征点的相对雷达运动，x定义在坐标系f
la
中。
[0042]
进一步的，步骤6包括以下内容：
[0043]
以相对雷达测量残差和imu预积分残差为约束条件，建立非线性最小二乘的目标函数并进行联合优化求解，将优化所得到的结果用于更新imu预测的状态变量，以消除imu在传递过程中产生的误差；
[0044]
imu预积分残差为：
[0045][0046]
其中，为imu预积分测量bias修正值，表示k时刻imu在世界坐标系下的位置，表示k+1时刻imu在世界坐标系下的位置，gw表示世界坐标系下的重力加速度，δtk为k时刻的采样频率，表示i时刻到j时刻中间离散的k时刻的速度，表示k时刻的世界坐标系向载体坐标系的转换矩阵，和表示k时刻和k+1的imu测量得到的加速度的观测值，和表示k时刻和k+1时刻的imu测量得到的陀螺仪的观测值，表示k时刻的imu的速度，表示k+1时刻的imu速度，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k+1时刻的载体坐标系到世界坐标系的姿态变换四元数，表
示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
[0047]
相对雷达测量残差为：
[0048][0049]
其中，和分别为激光雷达扫描的相邻姿态，m为残差项，为关键帧姿态，d为点到平面间的距离；
[0050]
通过滑窗优化方法可以有效限制传感器数据优化的计算量，滑窗能限制关键帧的数量，防止状态的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长；
[0051]
非线性最小二乘的代价函数为：
[0052][0053]
其中，r
l
(m,x)是相对雷达测量残差，是imu测量残差，r
p
(x)是边缘化的先验项。
[0054]
本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：
[0055]
本发明提供的一种基于imu预积分的激光slam定位方法，在一定程度上能够解决gnss信号遮挡环境下，移动机器人定位不准确的问题，弥补激光雷达和imu各自的缺点，进而提高整个激光slam定位部分的位姿求解精度和速度。
附图说明
[0056]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：
[0057]
图1是本发明一种基于imu预积分的激光slam定位方法中的激光定位流程图；
[0058]
图2是本发明一种基于imu预积分的激光slam定位方法中的点到平面距离的示意图；
[0059]
图3是本发明一种基于imu预积分的激光slam定位方法的整体操作步骤图。
具体实施方式
[0060]
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0061]
如图1-3所示，本实施例公开了一种基于imu(inertial measurement unit，惯性测量单元)预积分的激光slam(simultaneous localization and mapping，即时定位与地图构建)定位方法，包括以下步骤：
[0062]
步骤1：在获取当前imu与激光雷达测量数据的基础上，对当前时刻imu进行状态预测与预积分；
[0063]
具体的，步骤1包括以下内容：
[0064]
在新的激光雷达数据帧sj到来前，进行imu状态预测和预积分，假设当前时间戳为i，因imu的频率普遍远高于雷达点云数据的更新频率，在下一帧点云即时间戳j到来前，会有大量的imu数据读入，可以根据从i到j这段时间间隔内的imu数据对imu的状态进行估计，同时利用这段时间间隔内的imu测量数据进行预积分操作；
[0065]
imu状态预测，通过imu测量值的连续时间积分得到预测值。预测值包括imu的位置、速度、姿态、加速度偏差、角速度偏差。在实际应用中，往往要将连续时间的积分离散化。将运动学的预测状态离散化处理，imu状态预测为：
[0066][0067]
上式中，δt为采样频率，p为imu在世界坐标系下的位置，v为imu的速度，r为载体坐标系向世界坐标系的转换矩阵，am,a0分别为m时刻和初始时刻的加速度观测值，q为载体坐标系到世界坐标系的姿态变换四元数，wm,w0分别为m时刻和初始时刻的加速度观测值；
[0068]
通过上式离散化变换后，得到误差状态传播系统，如下式所示：
[0069]
δ
x
＝f(x,δ
x
,um,i)＝f
x
(x,um)
·
δ
x
+fi·iꢀꢀꢀꢀꢀ
(0.22)
[0070]
上式中，x为预测状态，δ
x
为误差状态向量，um为输入向量(角速度、加速度测量值)，i为随机扰动脉冲向量；
[0071]
imu预积分，一个三轴正交的imu可以测量三轴的加速度以及角速度，在已知i时刻的imu状态，可以根据加速度以及角速度的测量值求解j时刻的imu的状态，即imu数据积分值为：
[0072][0073]
上式中，假定采样频率δt恒定，表示j时刻imu在世界坐标系下的位置，表示i时刻imu在世界坐标系下的位置，vk表示i时刻到j时刻中间离散的k时刻的速度，gw表示世界坐标系下的重力加速度，表示k时刻的载体坐标系向世界坐标系的转换矩阵，和wk表示k时刻的imu测量得到的加速度和陀螺仪的观测值，表示j时刻的imu的速度，表
示i时刻的imu速度，表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
[0074]
在更新位置、速度和姿态时需知道前一时刻的imu位置，且每次优化更新后，都需要重新对之前的数据进行积分，再更新所有状态量，会导致对数据的重复积分，预积分能够将一段时间内的imu数据直接积分起来得到给定时间段的imu相对位置、姿态、速度的变化，对应imu预积分项为：
[0075][0076]
上式中，δp
ij
，δv
ij
，δq
ij
分别为预积分得到的imu在i时刻到j时刻的相对的位置、姿态和速度变化量，表示i时刻的载体坐标系向世界坐标系的转换矩阵，pi,pj分别为i时刻和j时刻imu在世界坐标系下的位置，vi,vj分别为i时刻和j时刻imu的速度，qi,qj分别为i时刻和j时刻imu载体坐标系到世界坐标系的姿态变换四元数，δt
ij
，δt为恒定采样频率，gw表示世界坐标系下的重力加速度，δv
ik
,δq
ik
分别为imu在i时刻到k时刻的相对的速度和姿态变化量，r表示imu载体坐标系向世界坐标系的转换矩阵，和wk表示k时刻的imu测量得到的加速度和陀螺仪的观测值。
[0077]
步骤2：利用imu状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；
[0078]
因载体在行驶过程中不能保证做匀速运动，所以需要考虑非匀速运动下产生的点云畸变的影响。假设tk和t
k+1
时刻为一次雷达扫描的开始和结束时刻，将t
k+1
时刻相对于tk时刻的雷达的相对位姿变换矩阵表示为可以计算出每个激光脚点i的补偿变换矩阵：
[0079][0080]
首先，需要寻找在imu两帧tk和t
k+1
之间的i时刻的激光脚点，利用imu可知载体在tk和t
k+1
时刻的位置和角度。载体在此时间段内作非匀速运动，利用时间距离计算权重分配比率，即线性插值，对应权重模型为：
[0081][0082]
则i时刻的角度、位置、速度变化为：
[0083]
[0084][0085][0086]
以点云第一帧的时刻为基准，建立一个局部坐标系，计算其他时刻相对于初始时刻由于非匀速运动产生的运动畸变，设点云初始时刻的位置和速度为δt为其他激光脚点相对与第一个点的相对时间，表示其他时间点相对于第一点的畸变位移量。通过下式可得到每个点相对于初始点在世界坐标系下的畸变位移量：
[0087][0088]
将所得畸变位移量再通过坐标转换到以初始点为基准的运动畸变。
[0089][0090]
表示以k时刻为起始点的载体坐标系到世界坐标系的矩阵，可以根据起始时刻载体的相对于世界坐标系的姿态获得。表示以o时刻为起始点建立的局部坐标系下的畸变位移量。
[0091]
将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，则：
[0092][0093]
为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，为第i帧雷达数据转换到世界坐标系下的矩阵，为点云i时刻的位置和速度。
[0094]
步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；
[0095]
具体的，步骤3包括以下内容：
[0096]
提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
[0097][0098]
上式中，s＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，表示点云q的坐标，为左右相邻的坐标。
[0099]
为了让特征点云均匀分布，先将每条扫描线均匀分成若干份，再将每份中的点按照曲率大小进行排序，曲率反映了点云点表面的几个结构平滑度，平面上的曲率小，平面交界线上的曲率大。通过设定上下两个阈值，可提取两种几何特征：尖锐的边缘点和平滑的平面点。
[0100]
步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；
[0101]
具体的，步骤4包括以下内容：
[0102]
建立局部地图，地图中包含nm(o,
…
p,
…
,i)离散时刻，其中o为开始第一帧雷达扫描时间，p为关键帧的雷达扫描时间，i表示结束帧的雷达扫描时间。将提取到的每一帧雷达扫描中的平面点f
lγ
,γ∈{o,
…
,p,
…
,i}的数据，根据优化得到的位姿变化量将其他时刻的特征点转换到以p时刻为基准建立的坐标系中，记为以所有雷达帧的特征点建立一个局部地图需要估计的状态为时间段的状态，其中p+1表示关键帧p的下一时刻，j表示新的雷达扫描时刻。
[0103]
步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；
[0104]
具体的，步骤5包括以下内容：
[0105]
相对雷达测量，利用步骤4中构建的局部地图，找到和原始的特征点之间的对应关系，二者都是相对于关键帧p的位姿运行，关键帧p的位姿会随着测量值的加入而发生变化，利用knn(k-nearest neighbor)算法可以找到的k个最近点将上述最近点拟合至坐标系f
lp
上的平面上，如图2所示，平面方程的系数为：
[0106]wt
x'+d＝0,x'∈π(x
lp
)
ꢀꢀꢀꢀꢀꢀꢀ
(0.26)
[0107]
式中，w是平面的法向量，d是距离坐标系f
lp
原点的距离，两者均定义在坐标系f
lp
中，对于每个平面特征点x∈f
la
，m＝[x,w,d]∈m
la
是特征点的相对雷达运动，x定义在坐标系f
la
中。
[0108]
步骤6：以lidar(light detection and ranging，激光探测及测距系统)相对测量残差和imu预积分残差为约束条件，建立非线性最小二乘目标函数，并联合优化求解，将结果用于更新imu预测的状态变量，消除imu在传递过程中的误差。
[0109]
具体的，步骤6包括以下内容：
[0110]
以相对雷达测量残差和imu预积分残差为约束条件，建立非线性最小二乘的目标函数并进行联合优化求解，将优化所得到的结果用于更新imu预测的状态变量，以消除imu
在传递过程中产生的误差；
[0111]
imu预积分残差为：
[0112][0113]
其中，为imu预积分测量bias修正值，表示k时刻imu在世界坐标系下的位置，表示k+1时刻imu在世界坐标系下的位置，gw表示世界坐标系下的重力加速度，δtk为k时刻的采样频率，表示i时刻到j时刻中间离散的k时刻的速度，表示k时刻的世界坐标系向载体坐标系的转换矩阵，和表示k时刻和k+1的imu测量得到的加速度的观测值，和表示k时刻和k+1时刻的imu测量得到的陀螺仪的观测值，表示k时刻的imu的速度，表示k+1时刻的imu速度，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k+1时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
[0114]
每个相对激光雷达测量的残差m＝[x,w,d]∈m
la
,a∈{p+1,
…
,j}可以用点到平面的距离来表示，相对雷达测量残差为：
[0115][0116]
其中，和分别为激光雷达扫描的相邻姿态，m为残差项，为关键帧姿态，d为点到平面间的距离；
[0117]
通过滑窗优化方法可以有效限制传感器数据优化的计算量，滑窗能限制关键帧的数量，防止状态的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长；
[0118]
在滑动窗口中一直保持ns个imu状态，即到滑动窗口有助于限制计算量。当出现新的测量约束时，平滑器将包括新状态并边缘化窗口中的最旧状态，要估算的整个状态是：
[0119][0120]
再求下列马氏距离代价函数的最小值，以获取x的最大后验估计，非线性最小二乘的代价函数为：
[0121][0122]
其中，r
l
(m,x)是相对雷达测量残差，是imu测量残差，r
p
(x)是边缘化的先验项。
[0123]
上述非线性最小二乘的代价函数可通过gauss-newton法求解：
[0124]
hδx＝b
[0125]
其中，h＝∑j
t
c-1
j,b＝∑j
t
c-1
r，j表示剩余状态的雅可比行列式，c表示状态的协方差矩阵，δx代表将在优化中实际估算的误差状态，通过该误差状态，可以更新过该误差状态，可以更新表示四元数的小角度更新。
[0126]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。