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

本发明专利技术公开了一种基于IMU预积分的激光SLAM定位方法，包括步骤1：在获取当前IMU与激光雷达测量数据的基础上，对当前时刻IMU进行状态预测与预积分；步骤2：利用IMU状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；步骤6：以LiDAR相对测量残差和IMU预积分残差为约束条件，建立非线性最小二乘目标函数，并联合优化求解，将结果用于更新IMU预测的状态变量，消除IMU在传递过程中的误差。的误差。的误差。

【技术实现步骤摘要】
一种基于IMU预积分的激光SLAM定位方法


[0001]本专利技术涉及激光雷达建图与定位领域，特别涉及一种基于IMU预积分的激光SLAM定位方法。

技术介绍

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

技术实现思路

[0005]为了克服现有技术中的不足，本专利技术提供一种基于IMU预积分的激光SLAM定位方法，通过多传感器数据融合，解决单个传感器无法满足长时定位需求和定位精度日益增长的高要求，以及有效处理位姿累积误差的问题，弥补激光雷达和IMU各自的缺点，进而提高整个激光SLAM定位部分的位姿求解精度和速度。
[0006]为了达到上述专利技术目的，解决其技术问题所采用的技术方案如下：
[0007]一种基于IMU预积分的激光SLAM定位方法，包括以下步骤：
[0008]步骤1：在获取当前IMU与激光雷达测量数据的基础上，对当前时刻IMU进行状态预测与预积分；
[0009]步骤2：利用IMU状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；
[0010]步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；
[0011]步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；
[0012]步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；
[0013]步骤6：以LiDAR相对测量残差和IMU预积分残差为约束条件，建立非线性最小二乘目标函数，并联合优化求解，将结果用于更新IMU预测的状态变量，消除IMU在传递过程中的误差。
[0014]进一步的，步骤1包括以下内容：
[0015]在新的激光雷达数据帧S
j
到来前，进行IMU状态预测和预积分，假设当前时间戳为i，因IMU的频率普遍远高于雷达点云数据的更新频率，在下一帧点云即时间戳j到来前，会
有大量的IMU数据读入，可以根据从i到j这段时间间隔内的IMU数据对IMU的状态进行估计，同时利用这段时间间隔内的IMU测量数据进行预积分操作；
[0016]IMU状态预测为：
[0017][0018]上式中，Δt为采样频率，p为IMU在世界坐标系下的位置，v为IMU的速度，R为载体坐标系向世界坐标系的转换矩阵，a
m
,a0分别为m时刻和初始时刻的加速度观测值，q为载体坐标系到世界坐标系的姿态变换四元数，w
m
,w0分别为m时刻和初始时刻的加速度观测值；
[0019]通过上式离散化变换后，得到误差状态传播系统，如下式所示：
[0020]δ
x
＝f(x,δ
x
,u
m
,i)＝F
x
(x,u
m
)
·
δ
x
+F
i
·
i
ꢀꢀꢀꢀꢀꢀ
(0.12)
[0021]上式中，x为预测状态，δ
x
为误差状态向量，u
m
为输入向量(角速度、加速度测量值)，i为随机扰动脉冲向量；
[0022]IMU数据积分值为：
[0023][0024]上式中，假定Δt恒定，即采样频率不变，表示j时刻IMU在世界坐标系下的位置，表示i时刻IMU在世界坐标系下的位置，v
k
表示i时刻到j时刻中间离散的k时刻的速度，g
W
表示世界坐标系下的重力加速度，表示k时刻的载体坐标系向世界坐标系的转换矩阵，和w
k
表示k时刻的IMU测量得到的加速度和陀螺仪的观测值，表示j时刻的IMU的速度，表示i时刻的IMU速度，表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
[0025]对应IMU预积分项为：
[0026][0027]上式中，Δp
ij
，Δv
ij
，Δq
ij
分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量，表示i时刻的载体坐标系向世界坐标系的转换矩阵，p
i
,p
j
分别为i时刻和j时刻IMU在世界坐标系下的位置，v
i
,v
j
分别为i时刻和j时刻IMU的速度，q
i
,q
j
分别为i时刻和j时刻IMU载体坐标系到世界坐标系的姿态变换四元数，Δt
ij
，Δt为恒定采样频率，g
W
表示世界坐标系下的重力加速度，Δv
ik
,Δq
ik
分别为IMU在i时刻到k时刻的相对的速度和姿态变化量，R表示IMU载体坐标系向世界坐标系的转换矩阵，和w
k
表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
[0028]进一步的，步骤2包括以下内容：
[0029]利用IMU状态预测得到的预测值，消除雷达点云畸变，将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，即：
[0030][0031]为点云初始时刻的位置和速度，为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，为第i帧雷达数据转换到世界坐标系下的矩阵，为点云i时刻的位置和速度，为以O时刻为起始点建立的局部坐标系下的畸变位移量。
[0032]进一步的，步骤3包括以下内容：
[0033]提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
[0034][0035]上式中，S＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，表示点云q的坐标，为左右相邻的坐标。
[0036]进一步的，步骤4包括以下内容：
[0037]建立局部地图，地图中包含N
m
(o,
…
p,
…
,i)离散时刻，其中o为开始第一帧雷达扫本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于IMU预积分的激光SLAM定位方法，其特征在于，包括以下步骤：步骤1：在获取当前IMU与激光雷达测量数据的基础上，对当前时刻IMU进行状态预测与预积分；步骤2：利用IMU状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；步骤6：以LiDAR相对测量残差和IMU预积分残差为约束条件，建立非线性最小二乘目标函数，并联合优化求解，将结果用于更新IMU预测的状态变量，消除IMU在传递过程中的误差。2.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤1包括以下内容：在新的激光雷达数据帧S
j
到来前，进行IMU状态预测和预积分，假设当前时间戳为i，因IMU的频率普遍远高于雷达点云数据的更新频率，在下一帧点云即时间戳j到来前，会有大量的IMU数据读入，可以根据从i到j这段时间间隔内的IMU数据对IMU的状态进行估计，同时利用这段时间间隔内的IMU测量数据进行预积分操作；IMU状态预测为：上式中，Δt为采样频率，p为IMU在世界坐标系下的位置，v为IMU的速度，R为载体坐标系向世界坐标系的转换矩阵，a
m
,a0分别为m时刻和初始时刻的加速度观测值，q为载体坐标系到世界坐标系的姿态变换四元数，w
m
,w0分别为m时刻和初始时刻的加速度观测值；通过上式离散化变换后，得到误差状态传播系统，如下式所示：δ
x
＝f(x,δ
x
,u
m
,i)＝F
x
(x,u
m
)
·
δ
x
+F
i
·
i
ꢀꢀꢀꢀ
(0.2)上式中，x为预测状态，δ
x
为误差状态向量，u
m
为输入向量(角速度、加速度测量值)，i为随机扰动脉冲向量；IMU数据积分值为：
上式中，假定Δt恒定，即采样频率不变，表示j时刻IMU在世界坐标系下的位置，表示i时刻IMU在世界坐标系下的位置，v
k
表示i时刻到j时刻中间离散的k时刻的速度，g
W
表示世界坐标系下的重力加速度，表示k时刻的载体坐标系向世界坐标系的转换矩阵，和w
k
表示k时刻的IMU测量得到的加速度和陀螺仪的观测值，表示j时刻的IMU的速度，表示i时刻的IMU速度，表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；对应IMU预积分项为：上式中，Δp
ij
，Δv
ij
，Δq
ij
分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量，表示i时刻的载体坐标系向世界坐标系的转换矩阵，p
i
,p
j
分别为i时刻和j时刻IMU在世界坐标系下的位置，v
i
,v
j
分别为i时刻和j时刻IMU的速度，q
i
,q
j
分别为i时刻和j时刻IMU载体坐标系到世界坐标系的姿态变换四元数，Δt
ij
，Δt为恒定采样频率，g
W
表示世界坐标系下的重力加速度，Δv
ik
,Δq
ik
分别为IMU在i时刻到k时刻的相对的速度和姿态变化量，R表示IMU载体坐标系向世界坐标系的转换矩阵，和w
k
表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。3.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤2包括以下内容：利用IMU状态预测得到的预测值，消除雷达点云畸变，将...

【专利技术属性】
技术研发人员：狄逸群，马向华，靳午煊，朱丽，
申请(专利权)人：上海应用技术大学，
类型：发明
国别省市：