CN114136311A - 一种基于imu预积分的激光slam定位方法 - Google Patents
一种基于imu预积分的激光slam定位方法 Download PDFInfo
- Publication number
- CN114136311A CN114136311A CN202111311578.5A CN202111311578A CN114136311A CN 114136311 A CN114136311 A CN 114136311A CN 202111311578 A CN202111311578 A CN 202111311578A CN 114136311 A CN114136311 A CN 114136311A
- Authority
- CN
- China
- Prior art keywords
- imu
- time
- coordinate system
- representing
- world coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000010354 integration Effects 0.000 title claims abstract description 52
- 238000000034 method Methods 0.000 title claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 44
- 238000005457 optimization Methods 0.000 claims abstract description 22
- 230000005540 biological transmission Effects 0.000 claims abstract description 7
- 230000009466 transformation Effects 0.000 claims description 44
- 230000001133 acceleration Effects 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000036544 posture Effects 0.000 claims description 20
- 230000033001 locomotion Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 230000007547 defect Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于IMU预积分的激光SLAM定位方法,包括步骤1:在获取当前IMU与激光雷达测量数据的基础上,对当前时刻IMU进行状态预测与预积分;步骤2:利用IMU状态预测得到的预测值,消除激光雷达数据测量产生的点云畸变;步骤3:从畸变后的点云中进行特征提取,提取边缘点与平面点;步骤4:将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下,形成一个点云局部地图;步骤5:根据局部地图,确定所提取特征与局部地图间的对应关系,得到相对激光雷达测量;步骤6:以LiDAR相对测量残差和IMU预积分残差为约束条件,建立非线性最小二乘目标函数,并联合优化求解,将结果用于更新IMU预测的状态变量,消除IMU在传递过程中的误差。
Description
技术领域
本发明涉及激光雷达建图与定位领域,特别涉及一种基于IMU预积分的激光SLAM定位方法。
背景技术
无人驾驶领域,作为未来的一大科技重点,实现高精准度自主导航是最终目标。在自主导航系统中,定位问题无疑是首先要解决的难题,准确的位姿估计是实现自主导航的关键技术,可以为建图、路径规划等技术提供精确位姿信息,是实现自主导航功能的第一步。
激光雷达可以通过点云形式对周围环境进行感知,但对于目前无人驾驶领域的长时定位需求和定位精度日益增长的高要求,依靠单一传感器已经无法满足。
IMU能够根据载体运动情况,监测加速度与角速度信息,得到载体的位姿变化状态。但由于位姿状态通过积分获取,故位姿误差会随时间不断积累,长时间的运行会导致定位失准。
发明内容
为了克服现有技术中的不足,本发明提供一种基于IMU预积分的激光SLAM定位方法,通过多传感器数据融合,解决单个传感器无法满足长时定位需求和定位精度日益增长的高要求,以及有效处理位姿累积误差的问题,弥补激光雷达和IMU各自的缺点,进而提高整个激光SLAM定位部分的位姿求解精度和速度。
为了达到上述发明目的,解决其技术问题所采用的技术方案如下:
一种基于IMU预积分的激光SLAM定位方法,包括以下步骤:
步骤1:在获取当前IMU与激光雷达测量数据的基础上,对当前时刻IMU进行状态预测与预积分;
步骤2:利用IMU状态预测得到的预测值,消除激光雷达数据测量产生的点云畸变;
步骤3:从畸变后的点云中进行特征提取,提取边缘点与平面点;
步骤4:将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下,形成一个点云局部地图;
步骤5:根据局部地图,确定所提取特征与局部地图间的对应关系,得到相对激光雷达测量;
步骤6:以LiDAR相对测量残差和IMU预积分残差为约束条件,建立非线性最小二乘目标函数,并联合优化求解,将结果用于更新IMU预测的状态变量,消除IMU在传递过程中的误差。
进一步的,步骤1包括以下内容:
在新的激光雷达数据帧Sj到来前,进行IMU状态预测和预积分,假设当前时间戳为i,因IMU的频率普遍远高于雷达点云数据的更新频率,在下一帧点云即时间戳j到来前,会有大量的IMU数据读入,可以根据从i到j这段时间间隔内的IMU数据对IMU的状态进行估计,同时利用这段时间间隔内的IMU测量数据进行预积分操作;
IMU状态预测为:
上式中,Δt为采样频率,p为IMU在世界坐标系下的位置,v为IMU的速度,R为载体坐标系向世界坐标系的转换矩阵,am,a0分别为m时刻和初始时刻的加速度观测值,q为载体坐标系到世界坐标系的姿态变换四元数,wm,w0分别为m时刻和初始时刻的加速度观测值;
通过上式离散化变换后,得到误差状态传播系统,如下式所示:
δx=f(x,δx,um,i)=Fx(x,um)·δx+Fi·i (0.12)
上式中,x为预测状态,δx为误差状态向量,um为输入向量(角速度、加速度测量值),i为随机扰动脉冲向量;
IMU数据积分值为:
上式中,假定Δt恒定,即采样频率不变,表示j时刻IMU在世界坐标系下的位置,表示i时刻IMU在世界坐标系下的位置,vk表示i时刻到j时刻中间离散的k时刻的速度,gW表示世界坐标系下的重力加速度,表示k时刻的载体坐标系向世界坐标系的转换矩阵,和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值,表示j时刻的IMU的速度,表示i时刻的IMU速度,表示j时刻的载体坐标系到世界坐标系的姿态变换四元数,表示i时刻的载体坐标系到世界坐标系的姿态变换四元数,表示k时刻的载体坐标系到世界坐标系的姿态变换四元数;
对应IMU预积分项为:
上式中,Δpij,Δvij,Δqij分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量,表示i时刻的载体坐标系向世界坐标系的转换矩阵,pi,pj分别为i时刻和j时刻IMU在世界坐标系下的位置,vi,vj分别为i时刻和j时刻IMU的速度,qi,qj分别为i时刻和j时刻IMU载体坐标系到世界坐标系的姿态变换四元数,Δtij,Δt为恒定采样频率,gW表示世界坐标系下的重力加速度,Δvik,Δqik分别为IMU在i时刻到k时刻的相对的速度和姿态变化量,R表示IMU载体坐标系向世界坐标系的转换矩阵,和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
进一步的,步骤2包括以下内容:
利用IMU状态预测得到的预测值,消除雷达点云畸变,将一帧雷达数据中的点先转移到世界坐标系下,然后再转移到以起始点为基准的局部坐标系下,最后减去非匀速运动造成的畸变,即:
进一步的,步骤3包括以下内容:
提取点云特征,计算得到扫描线点云曲率后,将每条扫描线均匀分成若干份,再将每份中的点云按曲率大小进行排序,并设定曲率的上下阈值,以提取两种几何特征:尖锐的边缘点和平滑的平面点,针对每条扫描线中除起始和末端的若干点云外的其他点,计算点云的曲率值:
进一步的,步骤4包括以下内容:
建立局部地图,地图中包含Nm(o,…p,…,i)离散时刻,其中o为开始第一帧雷达扫描时间,p为关键帧的雷达扫描时间,i表示结束帧的雷达扫描时间,将提取到的每一帧雷达扫描中的平面点FLγ,γ∈{o,…,p,…,i}的数据,根据优化得到的位姿变化量将其他时刻的特征点转换到以p时刻为基准建立的坐标系中,记为以所有雷达帧的特征点建立一个局部地图需要估计的状态为时间段的状态,其中p+1表示关键帧p的下一时刻,j表示新的雷达扫描时刻。
进一步的,步骤5包括以下内容:
相对雷达测量,利用步骤4中构建的局部地图,找到和原始的特征点之间的对应关系,二者都是相对于关键帧p的位姿运行,关键帧p的位姿会随着测量值的加入而发生变化,利用KNN算法可以找到的K个最近点将上述最近点拟合至坐标系FLp上的平面上,平面方程的系数为:
wTx'+d=0,x'∈π(xLp) (0.17)
式中,w是平面的法向量,d是距离坐标系FLp原点的距离,两者均定义在坐标系FLp中,对于每个平面特征点x∈FLa,m=[x,w,d]∈mLa是特征点的相对雷达运动,x定义在坐标系FLa中。
进一步的,步骤6包括以下内容:
以相对雷达测量残差和IMU预积分残差为约束条件,建立非线性最小二乘的目标函数并进行联合优化求解,将优化所得到的结果用于更新IMU预测的状态变量,以消除IMU在传递过程中产生的误差;
IMU预积分残差为:
其中,为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时刻的载体坐标系到世界坐标系的姿态变换四元数;
相对雷达测量残差为:
通过滑窗优化方法可以有效限制传感器数据优化的计算量,滑窗能限制关键帧的数量,防止状态的个数不会随时间不断增加,使得优化问题始终在一个有限的复杂度内,不会随时间不断增长;
非线性最小二乘的代价函数为:
本发明由于采用以上技术方案,使之与现有技术相比,具有以下的优点和积极效果:
本发明提供的一种基于IMU预积分的激光SLAM定位方法,在一定程度上能够解决GNSS信号遮挡环境下,移动机器人定位不准确的问题,弥补激光雷达和IMU各自的缺点,进而提高整个激光SLAM定位部分的位姿求解精度和速度。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见,下面描述中的附图仅仅是本发明的一些实施例,对于本领域技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。附图中:
图1是本发明一种基于IMU预积分的激光SLAM定位方法中的激光定位流程图;
图2是本发明一种基于IMU预积分的激光SLAM定位方法中的点到平面距离的示意图;
图3是本发明一种基于IMU预积分的激光SLAM定位方法的整体操作步骤图。
具体实施方式
下面将结合附图对本发明的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
如图1-3所示,本实施例公开了一种基于IMU(Inertial Measurement Unit,惯性测量单元)预积分的激光SLAM(simultaneous localization and mapping,即时定位与地图构建)定位方法,包括以下步骤:
步骤1:在获取当前IMU与激光雷达测量数据的基础上,对当前时刻IMU进行状态预测与预积分;
具体的,步骤1包括以下内容:
在新的激光雷达数据帧Sj到来前,进行IMU状态预测和预积分,假设当前时间戳为i,因IMU的频率普遍远高于雷达点云数据的更新频率,在下一帧点云即时间戳j到来前,会有大量的IMU数据读入,可以根据从i到j这段时间间隔内的IMU数据对IMU的状态进行估计,同时利用这段时间间隔内的IMU测量数据进行预积分操作;
IMU状态预测,通过IMU测量值的连续时间积分得到预测值。预测值包括IMU的位置、速度、姿态、加速度偏差、角速度偏差。在实际应用中,往往要将连续时间的积分离散化。将运动学的预测状态离散化处理,IMU状态预测为:
上式中,Δt为采样频率,p为IMU在世界坐标系下的位置,v为IMU的速度,R为载体坐标系向世界坐标系的转换矩阵,am,a0分别为m时刻和初始时刻的加速度观测值,q为载体坐标系到世界坐标系的姿态变换四元数,wm,w0分别为m时刻和初始时刻的加速度观测值;
通过上式离散化变换后,得到误差状态传播系统,如下式所示:
δx=f(x,δx,um,i)=Fx(x,um)·δx+Fi·i (0.22)
上式中,x为预测状态,δx为误差状态向量,um为输入向量(角速度、加速度测量值),i为随机扰动脉冲向量;
IMU预积分,一个三轴正交的IMU可以测量三轴的加速度以及角速度,在已知i时刻的IMU状态,可以根据加速度以及角速度的测量值求解j时刻的IMU的状态,即IMU数据积分值为:
上式中,假定采样频率Δt恒定,表示j时刻IMU在世界坐标系下的位置,表示i时刻IMU在世界坐标系下的位置,vk表示i时刻到j时刻中间离散的k时刻的速度,gW表示世界坐标系下的重力加速度,表示k时刻的载体坐标系向世界坐标系的转换矩阵,和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值,表示j时刻的IMU的速度,表示i时刻的IMU速度,表示j时刻的载体坐标系到世界坐标系的姿态变换四元数,表示i时刻的载体坐标系到世界坐标系的姿态变换四元数,表示k时刻的载体坐标系到世界坐标系的姿态变换四元数;
在更新位置、速度和姿态时需知道前一时刻的IMU位置,且每次优化更新后,都需要重新对之前的数据进行积分,再更新所有状态量,会导致对数据的重复积分,预积分能够将一段时间内的IMU数据直接积分起来得到给定时间段的IMU相对位置、姿态、速度的变化,对应IMU预积分项为:
上式中,Δpij,Δvij,Δqij分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量,表示i时刻的载体坐标系向世界坐标系的转换矩阵,pi,pj分别为i时刻和j时刻IMU在世界坐标系下的位置,vi,vj分别为i时刻和j时刻IMU的速度,qi,qj分别为i时刻和j时刻IMU载体坐标系到世界坐标系的姿态变换四元数,Δtij,Δt为恒定采样频率,gW表示世界坐标系下的重力加速度,Δvik,Δqik分别为IMU在i时刻到k时刻的相对的速度和姿态变化量,R表示IMU载体坐标系向世界坐标系的转换矩阵,和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
步骤2:利用IMU状态预测得到的预测值,消除激光雷达数据测量产生的点云畸变;
因载体在行驶过程中不能保证做匀速运动,所以需要考虑非匀速运动下产生的点云畸变的影响。假设tk和tk+1时刻为一次雷达扫描的开始和结束时刻,将tk+1时刻相对于tk时刻的雷达的相对位姿变换矩阵表示为可以计算出每个激光脚点i的补偿变换矩阵:
首先,需要寻找在IMU两帧tk和tk+1之间的i时刻的激光脚点,利用IMU可知载体在tk和tk+1时刻的位置和角度。载体在此时间段内作非匀速运动,利用时间距离计算权重分配比率,即线性插值,对应权重模型为:
则i时刻的角度、位置、速度变化为:
以点云第一帧的时刻为基准,建立一个局部坐标系,计算其他时刻相对于初始时刻由于非匀速运动产生的运动畸变,设点云初始时刻的位置和速度为Δt为其他激光脚点相对与第一个点的相对时间,表示其他时间点相对于第一点的畸变位移量。通过下式可得到每个点相对于初始点在世界坐标系下的畸变位移量:
将所得畸变位移量再通过坐标转换到以初始点为基准的运动畸变。
将一帧雷达数据中的点先转移到世界坐标系下,然后再转移到以起始点为基准的局部坐标系下,最后减去非匀速运动造成的畸变,则:
步骤3:从畸变后的点云中进行特征提取,提取边缘点与平面点;
具体的,步骤3包括以下内容:
提取点云特征,计算得到扫描线点云曲率后,将每条扫描线均匀分成若干份,再将每份中的点云按曲率大小进行排序,并设定曲率的上下阈值,以提取两种几何特征:尖锐的边缘点和平滑的平面点,针对每条扫描线中除起始和末端的若干点云外的其他点,计算点云的曲率值:
为了让特征点云均匀分布,先将每条扫描线均匀分成若干份,再将每份中的点按照曲率大小进行排序,曲率反映了点云点表面的几个结构平滑度,平面上的曲率小,平面交界线上的曲率大。通过设定上下两个阈值,可提取两种几何特征:尖锐的边缘点和平滑的平面点。
步骤4:将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下,形成一个点云局部地图;
具体的,步骤4包括以下内容:
建立局部地图,地图中包含Nm(o,…p,…,i)离散时刻,其中o为开始第一帧雷达扫描时间,p为关键帧的雷达扫描时间,i表示结束帧的雷达扫描时间。将提取到的每一帧雷达扫描中的平面点FLγ,γ∈{o,…,p,…,i}的数据,根据优化得到的位姿变化量将其他时刻的特征点转换到以p时刻为基准建立的坐标系中,记为以所有雷达帧的特征点建立一个局部地图需要估计的状态为时间段的状态,其中p+1表示关键帧p的下一时刻,j表示新的雷达扫描时刻。
步骤5:根据局部地图,确定所提取特征与局部地图间的对应关系,得到相对激光雷达测量;
具体的,步骤5包括以下内容:
相对雷达测量,利用步骤4中构建的局部地图,找到和原始的特征点之间的对应关系,二者都是相对于关键帧p的位姿运行,关键帧p的位姿会随着测量值的加入而发生变化,利用KNN(k-nearest neighbor)算法可以找到的K个最近点将上述最近点拟合至坐标系FLp上的平面上,如图2所示,平面方程的系数为:
wTx'+d=0,x'∈π(xLp) (0.26)
式中,w是平面的法向量,d是距离坐标系FLp原点的距离,两者均定义在坐标系FLp中,对于每个平面特征点x∈FLa,m=[x,w,d]∈mLa是特征点的相对雷达运动,x定义在坐标系FLa中。
步骤6:以LiDAR(Light Detection and Ranging,激光探测及测距系统)相对测量残差和IMU预积分残差为约束条件,建立非线性最小二乘目标函数,并联合优化求解,将结果用于更新IMU预测的状态变量,消除IMU在传递过程中的误差。
具体的,步骤6包括以下内容:
以相对雷达测量残差和IMU预积分残差为约束条件,建立非线性最小二乘的目标函数并进行联合优化求解,将优化所得到的结果用于更新IMU预测的状态变量,以消除IMU在传递过程中产生的误差;
IMU预积分残差为:
其中,为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时刻的载体坐标系到世界坐标系的姿态变换四元数;
每个相对激光雷达测量的残差m=[x,w,d]∈mLa,a∈{p+1,…,j}可以用点到平面的距离来表示,相对雷达测量残差为:
通过滑窗优化方法可以有效限制传感器数据优化的计算量,滑窗能限制关键帧的数量,防止状态的个数不会随时间不断增加,使得优化问题始终在一个有限的复杂度内,不会随时间不断增长;
再求下列马氏距离代价函数的最小值,以获取X的最大后验估计,非线性最小二乘的代价函数为:
上述非线性最小二乘的代价函数可通过Gauss-Newton法求解:
HδX=b
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求的保护范围为准。
Claims (7)
1.一种基于IMU预积分的激光SLAM定位方法,其特征在于,包括以下步骤:
步骤1:在获取当前IMU与激光雷达测量数据的基础上,对当前时刻IMU进行状态预测与预积分;
步骤2:利用IMU状态预测得到的预测值,消除激光雷达数据测量产生的点云畸变;
步骤3:从畸变后的点云中进行特征提取,提取边缘点与平面点;
步骤4:将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下,形成一个点云局部地图;
步骤5:根据局部地图,确定所提取特征与局部地图间的对应关系,得到相对激光雷达测量;
步骤6:以LiDAR相对测量残差和IMU预积分残差为约束条件,建立非线性最小二乘目标函数,并联合优化求解,将结果用于更新IMU预测的状态变量,消除IMU在传递过程中的误差。
2.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法,其特征在于,步骤1包括以下内容:
在新的激光雷达数据帧Sj到来前,进行IMU状态预测和预积分,假设当前时间戳为i,因IMU的频率普遍远高于雷达点云数据的更新频率,在下一帧点云即时间戳j到来前,会有大量的IMU数据读入,可以根据从i到j这段时间间隔内的IMU数据对IMU的状态进行估计,同时利用这段时间间隔内的IMU测量数据进行预积分操作;
IMU状态预测为:
上式中,Δt为采样频率,p为IMU在世界坐标系下的位置,v为IMU的速度,R为载体坐标系向世界坐标系的转换矩阵,am,a0分别为m时刻和初始时刻的加速度观测值,q为载体坐标系到世界坐标系的姿态变换四元数,wm,w0分别为m时刻和初始时刻的加速度观测值;
通过上式离散化变换后,得到误差状态传播系统,如下式所示:
δx=f(x,δx,um,i)=Fx(x,um)·δx+Fi·i (0.2)
上式中,x为预测状态,δx为误差状态向量,um为输入向量(角速度、加速度测量值),i为随机扰动脉冲向量;
IMU数据积分值为:
上式中,假定Δt恒定,即采样频率不变,表示j时刻IMU在世界坐标系下的位置,表示i时刻IMU在世界坐标系下的位置,vk表示i时刻到j时刻中间离散的k时刻的速度,gW表示世界坐标系下的重力加速度,表示k时刻的载体坐标系向世界坐标系的转换矩阵,和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值,表示j时刻的IMU的速度,表示i时刻的IMU速度,表示j时刻的载体坐标系到世界坐标系的姿态变换四元数,表示i时刻的载体坐标系到世界坐标系的姿态变换四元数,表示k时刻的载体坐标系到世界坐标系的姿态变换四元数;
对应IMU预积分项为:
7.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法,其特征在于,步骤6包括以下内容:
以相对雷达测量残差和IMU预积分残差为约束条件,建立非线性最小二乘的目标函数并进行联合优化求解,将优化所得到的结果用于更新IMU预测的状态变量,以消除IMU在传递过程中产生的误差;
IMU预积分残差为:
其中,为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时刻的载体坐标系到世界坐标系的姿态变换四元数;
相对雷达测量残差为:
通过滑窗优化方法可以有效限制传感器数据优化的计算量,滑窗能限制关键帧的数量,防止状态的个数不会随时间不断增加,使得优化问题始终在一个有限的复杂度内,不会随时间不断增长;
非线性最小二乘的代价函数为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111311578.5A CN114136311B (zh) | 2021-11-08 | 2021-11-08 | 一种基于imu预积分的激光slam定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111311578.5A CN114136311B (zh) | 2021-11-08 | 2021-11-08 | 一种基于imu预积分的激光slam定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114136311A true CN114136311A (zh) | 2022-03-04 |
CN114136311B CN114136311B (zh) | 2023-08-04 |
Family
ID=80393153
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111311578.5A Active CN114136311B (zh) | 2021-11-08 | 2021-11-08 | 一种基于imu预积分的激光slam定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114136311B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114660589A (zh) * | 2022-03-25 | 2022-06-24 | 中国铁建重工集团股份有限公司 | 一种地下隧道的定位方法、系统及装置 |
CN115407357A (zh) * | 2022-07-05 | 2022-11-29 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
CN115683122A (zh) * | 2022-10-25 | 2023-02-03 | 中国科学院自动化研究所 | 以世界为中心的定位与全局地图优化方法、装置及设备 |
CN115683170A (zh) * | 2023-01-04 | 2023-02-03 | 成都西物信安智能系统有限公司 | 基于雷达点云数据融合误差的校准方法 |
CN116772828A (zh) * | 2023-08-24 | 2023-09-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
CN116929338A (zh) * | 2023-09-15 | 2023-10-24 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
CN117387598A (zh) * | 2023-10-08 | 2024-01-12 | 北京理工大学 | 一种紧耦合轻量级的实时定位与建图方法 |
CN118466572A (zh) * | 2024-04-30 | 2024-08-09 | 武汉大学 | Gnss拒止环境梁底ndt任务驱动的uav自主循迹飞行控制方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190368879A1 (en) * | 2018-05-29 | 2019-12-05 | Regents Of The University Of Minnesota | Vision-aided inertial navigation system for ground vehicle localization |
CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
WO2021218620A1 (zh) * | 2020-04-30 | 2021-11-04 | 上海商汤临港智能科技有限公司 | 地图构建方法、装置、设备及存储介质 |
-
2021
- 2021-11-08 CN CN202111311578.5A patent/CN114136311B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190368879A1 (en) * | 2018-05-29 | 2019-12-05 | Regents Of The University Of Minnesota | Vision-aided inertial navigation system for ground vehicle localization |
CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
WO2021218620A1 (zh) * | 2020-04-30 | 2021-11-04 | 上海商汤临港智能科技有限公司 | 地图构建方法、装置、设备及存储介质 |
CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
Non-Patent Citations (3)
Title |
---|
HESS W ET AL.: "Real-time loop closure in 2D LiDAR SLAM", IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION * |
章弘凯 等: "一种多层次数据融合的SLAM定位算法", 机器人 * |
纪嘉文;杨明欣;: "一种基于多传感融合的室内建图和定位算法", 成都信息工程大学学报, no. 04 * |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114660589A (zh) * | 2022-03-25 | 2022-06-24 | 中国铁建重工集团股份有限公司 | 一种地下隧道的定位方法、系统及装置 |
CN114660589B (zh) * | 2022-03-25 | 2023-03-10 | 中国铁建重工集团股份有限公司 | 一种地下隧道的定位方法、系统及装置 |
CN115407357A (zh) * | 2022-07-05 | 2022-11-29 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
CN115407357B (zh) * | 2022-07-05 | 2024-05-31 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
CN115683122A (zh) * | 2022-10-25 | 2023-02-03 | 中国科学院自动化研究所 | 以世界为中心的定位与全局地图优化方法、装置及设备 |
CN115683170A (zh) * | 2023-01-04 | 2023-02-03 | 成都西物信安智能系统有限公司 | 基于雷达点云数据融合误差的校准方法 |
CN116772828A (zh) * | 2023-08-24 | 2023-09-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
CN116772828B (zh) * | 2023-08-24 | 2023-12-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
CN116929338A (zh) * | 2023-09-15 | 2023-10-24 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
CN116929338B (zh) * | 2023-09-15 | 2024-01-09 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
CN117387598A (zh) * | 2023-10-08 | 2024-01-12 | 北京理工大学 | 一种紧耦合轻量级的实时定位与建图方法 |
CN118466572A (zh) * | 2024-04-30 | 2024-08-09 | 武汉大学 | Gnss拒止环境梁底ndt任务驱动的uav自主循迹飞行控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114136311B (zh) | 2023-08-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114136311A (zh) | 一种基于imu预积分的激光slam定位方法 | |
CN111207774B (zh) | 一种用于激光-imu外参标定的方法及系统 | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN110926460B (zh) | 一种基于IMU的uwb定位异常值处理方法 | |
CN110243358A (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN113091738B (zh) | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 | |
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
KR20050025836A (ko) | 관성 항법 시스템의 자세 보상 방법 및 장치, 이를 이용한관성 항법 시스템의 위치 계산 방법 및 장치 | |
CN114608554B (zh) | 一种手持slam设备以及机器人即时定位与建图方法 | |
CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN113052855B (zh) | 一种基于视觉-imu-轮速计融合的语义slam方法 | |
CN110929402A (zh) | 一种基于不确定分析的概率地形估计方法 | |
CN109655059B (zh) | 一种基于θ-增量学习的视觉-惯性融合导航系统及方法 | |
CN103878770A (zh) | 基于速度估计的空间机器人视觉时延误差补偿方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
CN117685953A (zh) | 面向多无人机协同定位的uwb与视觉融合定位方法及系统 | |
CN116202509A (zh) | 一种面向室内多层建筑的可通行地图生成方法 | |
CN116399351A (zh) | 一种车辆位置估计方法 | |
RU2661446C1 (ru) | Способ определения навигационных параметров объекта и бесплатформенная инерциальная навигационная система для осуществления способа | |
CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
CN116295342A (zh) | 一种用于飞行器勘测的多传感状态估计器 | |
CN115183767A (zh) | 一种基于arkf的单目vio/uwb室内组合定位方法 | |
CN114690230A (zh) | 一种基于视觉惯性slam的自动驾驶车辆导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |