CN114136311A - 一种基于imu预积分的激光slam定位方法 - Google Patents

一种基于imu预积分的激光slam定位方法 Download PDF

Info

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
Application number
CN202111311578.5A
Other languages
English (en)
Other versions
CN114136311B (zh
Inventor
狄逸群
马向华
靳午煊
朱丽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Institute of Technology
Original Assignee
Shanghai Institute of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanghai Institute of Technology filed Critical Shanghai Institute of Technology
Priority to CN202111311578.5A priority Critical patent/CN114136311B/zh
Publication of CN114136311A publication Critical patent/CN114136311A/zh
Application granted granted Critical
Publication of CN114136311B publication Critical patent/CN114136311B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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预积分的激光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状态预测为：
Figure BDA0003342119880000021
上式中，Δ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数据积分值为：
Figure BDA0003342119880000031
上式中，假定Δt恒定，即采样频率不变，
Figure BDA0003342119880000032
表示j时刻IMU在世界坐标系下的位置，
Figure BDA0003342119880000033
表示i时刻IMU在世界坐标系下的位置，vk表示i时刻到j时刻中间离散的k时刻的速度，gW表示世界坐标系下的重力加速度，
Figure BDA0003342119880000034
表示k时刻的载体坐标系向世界坐标系的转换矩阵，
Figure BDA0003342119880000035
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值，
Figure BDA0003342119880000036
表示j时刻的IMU的速度，
Figure BDA0003342119880000037
表示i时刻的IMU速度，
Figure BDA0003342119880000038
表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA0003342119880000039
表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800000310
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
对应IMU预积分项为：
Figure BDA0003342119880000041
上式中，Δpij，Δvij，Δqij分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量，
Figure BDA0003342119880000042
表示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载体坐标系向世界坐标系的转换矩阵，
Figure BDA0003342119880000043
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
进一步的，步骤2包括以下内容：
利用IMU状态预测得到的预测值，消除雷达点云畸变，将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，即：
Figure BDA0003342119880000044
Figure BDA0003342119880000045
为点云初始时刻的位置和速度，
Figure BDA0003342119880000046
为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，
Figure BDA0003342119880000047
为第i帧雷达数据转换到世界坐标系下的矩阵，
Figure BDA0003342119880000048
为点云i时刻的位置和速度，
Figure BDA0003342119880000049
为以O时刻为起始点建立的局部坐标系下的畸变位移量。
进一步的，步骤3包括以下内容：
提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
Figure BDA0003342119880000051
上式中，S＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，
Figure BDA0003342119880000052
表示点云q的坐标，
Figure BDA0003342119880000053
为左右相邻的坐标。
进一步的，步骤4包括以下内容：
建立局部地图，地图中包含Nm(o,…p,…,i)离散时刻，其中o为开始第一帧雷达扫描时间，p为关键帧的雷达扫描时间，i表示结束帧的雷达扫描时间，将提取到的每一帧雷达扫描中的平面点FLγ,γ∈{o,…,p,…,i}的数据，根据优化得到的位姿变化量
Figure BDA0003342119880000054
将其他时刻的特征点转换到以p时刻为基准建立的坐标系中，记为
Figure BDA0003342119880000055
以所有雷达帧的特征点
Figure BDA0003342119880000056
建立一个局部地图
Figure BDA0003342119880000057
需要估计的状态为
Figure BDA0003342119880000058
时间段的状态，其中p+1表示关键帧p的下一时刻，j表示新的雷达扫描时刻。
进一步的，步骤5包括以下内容：
相对雷达测量，利用步骤4中构建的局部地图，找到
Figure BDA0003342119880000059
和原始的特征点
Figure BDA00033421198800000510
之间的对应关系，二者都是相对于关键帧p的位姿运行，关键帧p的位姿会随着测量值的加入而发生变化，利用KNN算法可以找到
Figure BDA00033421198800000511
的K个最近点
Figure BDA00033421198800000512
将上述最近点拟合至坐标系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预积分残差为：
Figure BDA0003342119880000061
其中，
Figure BDA0003342119880000062
为IMU预积分测量Bias修正值，
Figure BDA0003342119880000063
表示k时刻IMU在世界坐标系下的位置，
Figure BDA0003342119880000064
表示k+1时刻IMU在世界坐标系下的位置，gW表示世界坐标系下的重力加速度，Δtk为k时刻的采样频率，
Figure BDA0003342119880000065
表示i时刻到j时刻中间离散的k时刻的速度，
Figure BDA0003342119880000066
表示k时刻的世界坐标系向载体坐标系的转换矩阵，
Figure BDA0003342119880000067
和
Figure BDA0003342119880000068
表示k时刻和k+1的IMU测量得到的加速度的观测值，
Figure BDA0003342119880000069
和
Figure BDA00033421198800000610
表示k时刻和k+1时刻的IMU测量得到的陀螺仪的观测值，
Figure BDA00033421198800000611
表示k时刻的IMU的速度，
Figure BDA00033421198800000612
表示k+1时刻的IMU速度，
Figure BDA00033421198800000613
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800000614
表示k+1时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800000615
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
相对雷达测量残差为：
Figure BDA00033421198800000616
其中，
Figure BDA00033421198800000617
和
Figure BDA00033421198800000618
分别为激光雷达扫描的相邻姿态，m为残差项，
Figure BDA00033421198800000619
为关键帧姿态，d为点到平面间的距离；
通过滑窗优化方法可以有效限制传感器数据优化的计算量，滑窗能限制关键帧的数量，防止状态的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长；
非线性最小二乘的代价函数为：
Figure BDA0003342119880000071
其中，rL(m,X)是相对雷达测量残差，
Figure BDA0003342119880000072
是IMU测量残差，rp(X)是边缘化的先验项。
本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：
本发明提供的一种基于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状态预测为：
Figure BDA0003342119880000081
上式中，Δ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数据积分值为：
Figure BDA0003342119880000091
上式中，假定采样频率Δt恒定，
Figure BDA0003342119880000092
表示j时刻IMU在世界坐标系下的位置，
Figure BDA0003342119880000093
表示i时刻IMU在世界坐标系下的位置，vk表示i时刻到j时刻中间离散的k时刻的速度，gW表示世界坐标系下的重力加速度，
Figure BDA0003342119880000094
表示k时刻的载体坐标系向世界坐标系的转换矩阵，
Figure BDA0003342119880000095
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值，
Figure BDA0003342119880000096
表示j时刻的IMU的速度，
Figure BDA0003342119880000097
表示i时刻的IMU速度，
Figure BDA0003342119880000098
表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA0003342119880000099
表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800000910
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
在更新位置、速度和姿态时需知道前一时刻的IMU位置，且每次优化更新后，都需要重新对之前的数据进行积分，再更新所有状态量，会导致对数据的重复积分，预积分能够将一段时间内的IMU数据直接积分起来得到给定时间段的IMU相对位置、姿态、速度的变化，对应IMU预积分项为：
Figure BDA0003342119880000101
上式中，Δpij，Δvij，Δqij分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量，
Figure BDA0003342119880000102
表示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载体坐标系向世界坐标系的转换矩阵，
Figure BDA0003342119880000103
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
步骤2：利用IMU状态预测得到的预测值，消除激光雷达数据测量产生的点云畸变；
因载体在行驶过程中不能保证做匀速运动，所以需要考虑非匀速运动下产生的点云畸变的影响。假设tk和tk+1时刻为一次雷达扫描的开始和结束时刻，将tk+1时刻相对于tk时刻的雷达的相对位姿变换矩阵表示为
Figure BDA0003342119880000104
可以计算出每个激光脚点i的补偿变换矩阵：
Figure BDA0003342119880000105
首先，需要寻找在IMU两帧tk和tk+1之间的i时刻的激光脚点，利用IMU可知载体在tk和tk+1时刻的位置和角度。载体在此时间段内作非匀速运动，利用时间距离计算权重分配比率，即线性插值，对应权重模型为：
Figure BDA0003342119880000111
则i时刻的角度、位置、速度变化为：
Figure BDA0003342119880000112
Figure BDA0003342119880000113
Figure BDA0003342119880000114
以点云第一帧的时刻为基准，建立一个局部坐标系，计算其他时刻相对于初始时刻由于非匀速运动产生的运动畸变，设点云初始时刻的位置和速度为
Figure BDA0003342119880000115
Δt为其他激光脚点相对与第一个点的相对时间，
Figure BDA0003342119880000116
表示其他时间点相对于第一点的畸变位移量。通过下式可得到每个点相对于初始点在世界坐标系下的畸变位移量：
Figure BDA0003342119880000117
将所得畸变位移量再通过坐标转换到以初始点为基准的运动畸变。
Figure BDA0003342119880000118
Figure BDA0003342119880000119
表示以k时刻为起始点的载体坐标系到世界坐标系的矩阵，可以根据起始时刻载体的相对于世界坐标系的姿态获得。
Figure BDA00033421198800001110
表示以O时刻为起始点建立的局部坐标系下的畸变位移量。
将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，则：
Figure BDA0003342119880000121
Figure BDA0003342119880000122
为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，
Figure BDA0003342119880000123
为第i帧雷达数据转换到世界坐标系下的矩阵，
Figure BDA0003342119880000124
为点云i时刻的位置和速度。
步骤3：从畸变后的点云中进行特征提取，提取边缘点与平面点；
具体的，步骤3包括以下内容：
提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
Figure BDA0003342119880000125
上式中，S＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，
Figure BDA0003342119880000126
表示点云q的坐标，
Figure BDA0003342119880000127
为左右相邻的坐标。
为了让特征点云均匀分布，先将每条扫描线均匀分成若干份，再将每份中的点按照曲率大小进行排序，曲率反映了点云点表面的几个结构平滑度，平面上的曲率小，平面交界线上的曲率大。通过设定上下两个阈值，可提取两种几何特征：尖锐的边缘点和平滑的平面点。
步骤4：将一段时间内不同时间戳下所提取的特征点投影至同一坐标系下，形成一个点云局部地图；
具体的，步骤4包括以下内容：
建立局部地图，地图中包含Nm(o,…p,…,i)离散时刻，其中o为开始第一帧雷达扫描时间，p为关键帧的雷达扫描时间，i表示结束帧的雷达扫描时间。将提取到的每一帧雷达扫描中的平面点FLγ,γ∈{o,…,p,…,i}的数据，根据优化得到的位姿变化量
Figure BDA0003342119880000131
将其他时刻的特征点转换到以p时刻为基准建立的坐标系中，记为
Figure BDA0003342119880000132
以所有雷达帧的特征点
Figure BDA0003342119880000133
建立一个局部地图
Figure BDA0003342119880000134
需要估计的状态为
Figure BDA0003342119880000135
时间段的状态，其中p+1表示关键帧p的下一时刻，j表示新的雷达扫描时刻。
步骤5：根据局部地图，确定所提取特征与局部地图间的对应关系，得到相对激光雷达测量；
具体的，步骤5包括以下内容：
相对雷达测量，利用步骤4中构建的局部地图，找到
Figure BDA0003342119880000136
和原始的特征点
Figure BDA0003342119880000137
之间的对应关系，二者都是相对于关键帧p的位姿运行，关键帧p的位姿会随着测量值的加入而发生变化，利用KNN(k-nearest neighbor)算法可以找到
Figure BDA0003342119880000138
的K个最近点
Figure BDA0003342119880000139
将上述最近点拟合至坐标系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预积分残差为：
Figure BDA0003342119880000141
其中，
Figure BDA0003342119880000142
为IMU预积分测量Bias修正值，
Figure BDA0003342119880000143
表示k时刻IMU在世界坐标系下的位置，
Figure BDA0003342119880000144
表示k+1时刻IMU在世界坐标系下的位置，gW表示世界坐标系下的重力加速度，Δtk为k时刻的采样频率，
Figure BDA0003342119880000145
表示i时刻到j时刻中间离散的k时刻的速度，
Figure BDA0003342119880000146
表示k时刻的世界坐标系向载体坐标系的转换矩阵，
Figure BDA0003342119880000147
和
Figure BDA0003342119880000148
表示k时刻和k+1的IMU测量得到的加速度的观测值，
Figure BDA0003342119880000149
和
Figure BDA00033421198800001410
表示k时刻和k+1时刻的IMU测量得到的陀螺仪的观测值，
Figure BDA00033421198800001411
表示k时刻的IMU的速度，
Figure BDA00033421198800001412
表示k+1时刻的IMU速度，
Figure BDA00033421198800001413
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800001414
表示k+1时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure BDA00033421198800001415
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
每个相对激光雷达测量的残差m＝[x,w,d]∈mLa,a∈{p+1,…,j}可以用点到平面的距离来表示，相对雷达测量残差为：
Figure BDA00033421198800001416
其中，
Figure BDA00033421198800001417
和
Figure BDA00033421198800001418
分别为激光雷达扫描的相邻姿态，m为残差项，
Figure BDA00033421198800001419
为关键帧姿态，d为点到平面间的距离；
通过滑窗优化方法可以有效限制传感器数据优化的计算量，滑窗能限制关键帧的数量，防止状态的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长；
在滑动窗口中一直保持Ns个IMU状态，即
Figure BDA0003342119880000151
到
Figure BDA0003342119880000152
滑动窗口有助于限制计算量。当出现新的测量约束时，平滑器将包括新状态并边缘化窗口中的最旧状态，要估算的整个状态是：
Figure BDA0003342119880000153
再求下列马氏距离代价函数的最小值，以获取X的最大后验估计，非线性最小二乘的代价函数为：
Figure BDA0003342119880000154
其中，rL(m,X)是相对雷达测量残差，
Figure BDA0003342119880000155
是IMU测量残差，rp(X)是边缘化的先验项。
上述非线性最小二乘的代价函数可通过Gauss-Newton法求解：
HδX＝b
其中，H＝∑JTC-1J,b＝∑JTC-1r，J表示剩余状态的雅可比行列式，C表示状态的协方差矩阵，δX代表将在优化中实际估算的误差状态，通过该误差状态，可以更新
Figure BDA0003342119880000156
Figure BDA0003342119880000157
表示四元数的小角度更新。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

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状态预测为：
Figure FDA0003342119870000021
上式中，Δ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数据积分值为：
Figure FDA0003342119870000022
上式中，假定Δt恒定，即采样频率不变，
Figure FDA0003342119870000023
表示j时刻IMU在世界坐标系下的位置，
Figure FDA0003342119870000024
表示i时刻IMU在世界坐标系下的位置，vk表示i时刻到j时刻中间离散的k时刻的速度，gW表示世界坐标系下的重力加速度，
Figure FDA0003342119870000025
表示k时刻的载体坐标系向世界坐标系的转换矩阵，
Figure FDA0003342119870000026
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值，
Figure FDA0003342119870000027
表示j时刻的IMU的速度，
Figure FDA0003342119870000028
表示i时刻的IMU速度，
Figure FDA0003342119870000029
表示j时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure FDA00033421198700000210
表示i时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure FDA0003342119870000031
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
对应IMU预积分项为：
Figure FDA0003342119870000032
上式中，Δpij，Δvij，Δqij分别为预积分得到的IMU在i时刻到j时刻的相对的位置、姿态和速度变化量，
Figure FDA0003342119870000033
表示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载体坐标系向世界坐标系的转换矩阵，
Figure FDA0003342119870000034
和wk表示k时刻的IMU测量得到的加速度和陀螺仪的观测值。
3.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤2包括以下内容：
利用IMU状态预测得到的预测值，消除雷达点云畸变，将一帧雷达数据中的点先转移到世界坐标系下，然后再转移到以起始点为基准的局部坐标系下，最后减去非匀速运动造成的畸变，即：
Figure FDA0003342119870000035
Figure FDA0003342119870000036
为点云初始时刻的位置和速度，
Figure FDA0003342119870000037
为世界坐标系转换到以起始点为基准的局部坐标系的矩阵，
Figure FDA0003342119870000038
为第i帧雷达数据转换到世界坐标系下的矩阵，
Figure FDA0003342119870000041
为点云i时刻的位置和速度，
Figure FDA0003342119870000042
为以O时刻为起始点建立的局部坐标系下的畸变位移量。
4.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤3包括以下内容：
提取点云特征，计算得到扫描线点云曲率后，将每条扫描线均匀分成若干份，再将每份中的点云按曲率大小进行排序，并设定曲率的上下阈值，以提取两种几何特征：尖锐的边缘点和平滑的平面点，针对每条扫描线中除起始和末端的若干点云外的其他点，计算点云的曲率值：
Figure FDA0003342119870000043
上式中，S＝n，为与点云q呈连续排列的点云数，均匀分布在点云q的左右两侧，
Figure FDA0003342119870000044
表示点云q的坐标，
Figure FDA0003342119870000045
为左右相邻的坐标。
5.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤4包括以下内容：
建立局部地图，地图中包含Nm(o,…p,…,i)离散时刻，其中o为开始第一帧雷达扫描时间，p为关键帧的雷达扫描时间，i表示结束帧的雷达扫描时间，将提取到的每一帧雷达扫描中的平面点FLγ,γ∈{o,…,p,…,i}的数据，根据优化得到的位姿变化量
Figure FDA0003342119870000046
将其他时刻的特征点转换到以p时刻为基准建立的坐标系中，记为
Figure FDA0003342119870000047
以所有雷达帧的特征点
Figure FDA0003342119870000048
建立一个局部地图
Figure FDA0003342119870000049
需要估计的状态为
Figure FDA00033421198700000410
时间段的状态，其中p+1表示关键帧p的下一时刻，j表示新的雷达扫描时刻。
6.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤5包括以下内容：
相对雷达测量，利用步骤4中构建的局部地图，找到
Figure FDA00033421198700000411
和原始的特征点
Figure FDA00033421198700000412
之间的对应关系，二者都是相对于关键帧p的位姿运行，关键帧p的位姿会随着测量值的加入而发生变化，利用KNN算法可以找到
Figure FDA00033421198700000413
的K个最近点
Figure FDA0003342119870000051
将上述最近点拟合至坐标系FLp上的平面上，平面方程的系数为：
wTx'+d＝0,x'∈π(xLp) (0.7)
式中，w是平面的法向量，d是距离坐标系FLp原点的距离，两者均定义在坐标系FLp中，对于每个平面特征点x∈FLa，m＝[x,w,d]∈mLa是特征点的相对雷达运动，x定义在坐标系FLa中。
7.根据权利要求1所述的一种基于IMU预积分的激光SLAM定位方法，其特征在于，步骤6包括以下内容：
以相对雷达测量残差和IMU预积分残差为约束条件，建立非线性最小二乘的目标函数并进行联合优化求解，将优化所得到的结果用于更新IMU预测的状态变量，以消除IMU在传递过程中产生的误差；
IMU预积分残差为：
Figure FDA0003342119870000052
其中，
Figure FDA0003342119870000053
为IMU预积分测量Bias修正值，
Figure FDA0003342119870000054
表示k时刻IMU在世界坐标系下的位置，
Figure FDA0003342119870000055
表示k+1时刻IMU在世界坐标系下的位置，gW表示世界坐标系下的重力加速度，Δtk为k时刻的采样频率，
Figure FDA0003342119870000056
表示i时刻到j时刻中间离散的k时刻的速度，
Figure FDA0003342119870000057
表示k时刻的世界坐标系向载体坐标系的转换矩阵，
Figure FDA0003342119870000058
和
Figure FDA0003342119870000059
表示k时刻和k+1的IMU测量得到的加速度的观测值，
Figure FDA00033421198700000510
和
Figure FDA00033421198700000511
表示k时刻和k+1时刻的IMU测量得到的陀螺仪的观测值，
Figure FDA00033421198700000512
表示k时刻的IMU的速度，
Figure FDA00033421198700000513
表示k+1时刻的IMU速度，
Figure FDA00033421198700000514
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure FDA0003342119870000061
表示k+1时刻的载体坐标系到世界坐标系的姿态变换四元数，
Figure FDA0003342119870000062
表示k时刻的载体坐标系到世界坐标系的姿态变换四元数；
相对雷达测量残差为：
Figure FDA0003342119870000063
其中，
Figure FDA0003342119870000064
和
Figure FDA0003342119870000065
分别为激光雷达扫描的相邻姿态，m为残差项，
Figure FDA0003342119870000066
为关键帧姿态，d为点到平面间的距离；
通过滑窗优化方法可以有效限制传感器数据优化的计算量，滑窗能限制关键帧的数量，防止状态的个数不会随时间不断增加，使得优化问题始终在一个有限的复杂度内，不会随时间不断增长；
非线性最小二乘的代价函数为：
Figure FDA0003342119870000067
其中，rL(m,X)是相对雷达测量残差，
Figure FDA0003342119870000068
是IMU测量残差，rp(X)是边缘化的先验项。
CN202111311578.5A 2021-11-08 2021-11-08 一种基于imu预积分的激光slam定位方法 Active CN114136311B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 上海商汤临港智能科技有限公司 地图构建方法、装置、设备及存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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