CN113066105B - 激光雷达和惯性测量单元融合的定位与建图方法及系统 - Google Patents

激光雷达和惯性测量单元融合的定位与建图方法及系统 Download PDF

Info

Publication number
CN113066105B
CN113066105B CN202110362412.XA CN202110362412A CN113066105B CN 113066105 B CN113066105 B CN 113066105B CN 202110362412 A CN202110362412 A CN 202110362412A CN 113066105 B CN113066105 B CN 113066105B
Authority
CN
China
Prior art keywords
laser
frame
laser radar
odometer
pose
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.)
Active
Application number
CN202110362412.XA
Other languages
English (en)
Other versions
CN113066105A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110362412.XA priority Critical patent/CN113066105B/zh
Publication of CN113066105A publication Critical patent/CN113066105A/zh
Application granted granted Critical
Publication of CN113066105B publication Critical patent/CN113066105B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了激光雷达和惯性测量单元融合的定位与建图系统及方法，解决了纯激光SLAM算法中存在的高度误差累积问题。首先对激光雷达和IMU进行联合标定。激光雷达获取原始点云数据，送入激光里程计。IMU获取激光雷达位姿送入激光里程计。激光里程计包括高频激光里程计和低频激光里程计；高频激光里程计对原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，激光雷达的位姿差值。低频激光里程计，选取关键帧，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出。构建因子图模型进行优化求解得到优化后的变量节点，得到激光雷达在世界坐标系下的位姿以及3D点云地图。

Description

激光雷达和惯性测量单元融合的定位与建图方法及系统
技术领域
本发明涉及机器人感知与导航技术领域，具体涉及一种3D激光雷达和惯性测量单元融合的定位与建图方法及系统。
背景技术
即时定位与建图(Simultaneous Localization and Mapping，SLAM)是当前机器人导航定位领域具有研究前景的技术领域。即时建图与定位技术的意义在于，其并不依赖于外部测量给出的信息确定自身的位置，而是依赖于自身传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置和当前位置的传感器数据建立环境点云地图。SLAM领域目前主要分为激光SLAM技术和视觉SLAM技术、以及多传感器融合SLAM技术。激光SLAM技术由于性能稳定，鲁棒性强，已经被广泛应用于无人驾驶、智能机器人、三维环境重建、智能农业等各个领域中。但是，SLAM技术仍然存在一系列问题，如何使SLAM技术适应各种复杂环境如低特征环境、高速移动环境，大尺度(千米级)环境，仍然是专家学者们研究的热点。
针对激光SLAM问题中的大尺度环境问题，已有的解决方案有如下几种：
方案1：文献(Zhang J,Kaess M,Singh S.On degeneracy of optimization-based state estimation problems.[C]//2016IEEE International Conference onRobotics and Automation(ICRA).IEEE,2016.)文献提出了LOAM算法。LOAM主要贡献在于，提出了基于边缘点和平面点特征的激光点云配准方法以及利用匀速运动假设来消除激光点云的运动畸变，取得了低漂移和低计算成本的平衡。但美中不足的是，LOAM并没有考虑时下流行的后端优化方法去做后端回环处理。值得一提的是，LOAM中有使用惯性导航器件作为点云配准的初始值，相对于原始版本并没有取得很好的结果。
方案2：文献(Shan T,Englot B.Lego-loam:Lightweight and ground-optimizedlidar odometry and mapping on variable terrain[C]//2018IEEE/RSJ InternationalConference on Intelligent Robots and Systems(IROS).IEEE,2018:4758-4765.)文献中提出的LEGO_LOAM算法针对地面无人平台对LOAM算法进行了改进，提出了利用点云地面分割技术提取出地面，从而能够估计地面无人平台的横滚角、俯仰角以及高度，再利用剩余的特征点进行点云配准，估计出地面无人平台的偏航角和二维位置，同时加入了后端回环优化的部分使得回环之后点云地图会更加精确。但是没有完全解决大尺度环境下激光SLAM算法高度漂移的问题。
方案3：文献(Shan T,Englot B,Meyers D,et al.LIO-SAM:Tightly-coupledLidar Inertial Odometry via Smoothing and Mapping[J].2020.)该文献提出了LIO-SAM算法，在LEGO_LOAM的基础上加入了惯性测量单元里程计，并用图优化的理念对激光里程计和惯性里程计进行紧耦合，使得里程计积分出的位姿更加精确，同时，在后端优化中，使用GPS对部分位姿节点进行约束，从而能获得更加贴近于现实的建图效果。但是算法需要高精度的惯性导航器件和激光雷达的精确标定，实现条件较为苛刻。
结合以上三个已有方案，如何实现针对地面无人平台的即时定位和建图，并避免纯激光SLAM算法中存在的高度误差累积问题，是目前亟待解决的问题。
发明内容
有鉴于此，本发明提供了3D激光雷达和惯性测量单元融合的即时定位与建图系统，是一种基于因子图优化的激光雷达和惯性导航单元IMU融合的即时定位与建图框架，解决了纯激光SLAM算法中存在的高度误差累积问题。。
为达到上述目的，本发明的技术方案为：
激光雷达和惯性测量单元融合的即时定位与建图方法，包括如下步骤：
步骤一、对激光雷达和惯性测量单元IMU进行联合标定。
步骤二、激光雷达获取原始点云数据，以激光帧的形式送入激光里程计；。
惯性测量单元IMU获取激光雷达位姿送入激光里程计。
步骤三、激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz。
高频激光里程计，对原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值。
低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出。
步骤四、构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
进一步地，对激光雷达和惯性测量单元IMU进行联合标定，具体为：
利用手眼标定方程对激光雷达和惯性测量单元IMU进行联合标定。
进一步地，高频激光里程计中，对原始点云数据进行运动畸变去除，具体为：
激光雷达生成的原始点云数据中，第k帧点云中每一个激光点i对应的激光雷达位姿
Figure BDA0003006078250000041
分布在第k-1帧和第k帧点云对应的世界坐标系下激光雷达位姿
Figure BDA0003006078250000042
之间，并认为第k帧点云最后一个接收到的激光点iend对应的激光雷达位姿
Figure BDA0003006078250000043
即为
Figure BDA0003006078250000044
第k-1帧和第k帧点云之间的激光雷达变换矩阵平移分量
Figure BDA0003006078250000045
为匀速运动，则第i个激光点对应的激光雷达位姿相对于
Figure BDA0003006078250000046
的平移分量
Figure BDA0003006078250000047
由第k-2帧和第k-1帧点云之间的激光雷达变换矩阵平移分量
Figure BDA0003006078250000048
进行时间线性插值给出，而每一个激光点相对于
Figure BDA0003006078250000049
的旋转分量
Figure BDA00030060782500000410
则依靠惯性测量单元IMU的测量差值给出；由此求得激光点i相对于点iend对应的激光雷达运动关系矩阵
Figure BDA00030060782500000411
将第k帧点云每一个激光点i根据
Figure BDA00030060782500000412
变换到iend对应的激光雷达位姿
Figure BDA00030060782500000413
下，得到除去运动畸变的第k帧点云。
进一步地，特征点提取高频激光里程计中，特征点提取具体采用如下方法：
特征点包括边缘点和平面点。
首先将去除运动畸变的点云按照激光束的水平角均分为四部分，对于每一部分点云，求解其中每条线束中每个点的曲率，则每条线束中曲率最大的点作为边缘点，提取到边缘点集
Figure BDA00030060782500000414
每条线束中曲率最小的点作为平面点，提取到平面点集
Figure BDA00030060782500000415
每一部分点云最多提供2个边缘点和4个平面点，由此得到特征点云。
进一步地，高频激光里程计中，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值，具体为：
进行第k帧特征点云
Figure BDA00030060782500000416
与k-1帧特征点云
Figure BDA00030060782500000417
的配准；其中第k帧特征点云包括第k帧边缘点集
Figure BDA00030060782500000418
第k帧平面点集
Figure BDA00030060782500000419
k-1帧特征点云包括第k-1帧边缘点集
Figure BDA0003006078250000051
第k-1帧平面点集
Figure BDA0003006078250000052
将所有第k帧的待匹配特征点i根据上一次运动估计得出的第k-2帧和第k-1帧之间激光雷达的变换矩阵
Figure BDA0003006078250000053
变换到第k-1帧坐标系，构建当前帧第k帧与第k-1帧中所有的边缘点和平面点的误差项di并累加，由此构造最小二乘函数，即
Figure BDA0003006078250000054
其中，向量f的每一行代表了一个特征点，变量
Figure BDA0003006078250000055
为第k-1帧到第k帧的变换矩阵，
Figure BDA0003006078250000056
为
Figure BDA0003006078250000057
的初始值，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑di，求解出的
Figure BDA0003006078250000058
即为第k个激光帧和第k-1个激光帧之间激光雷达的位姿差值。
进一步地，低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出，具体为：
根据低频激光里程计的处理频率选取关键帧，关键帧序号K，关键帧K对应的特征点云的平面点集为
Figure BDA0003006078250000059
边缘点集
Figure BDA00030060782500000510
关键帧时刻激光雷达世界坐标系下的位姿为
Figure BDA00030060782500000511
局部点云地图
Figure BDA00030060782500000512
由时间序列上的过去10个关键帧点云投影到世界坐标系下拼接而成。
前一关键帧即第K-1个关键帧激光雷达世界坐标系下的位姿为
Figure BDA00030060782500000513
第K个关键帧激光雷达世界坐标系下的位姿
Figure BDA00030060782500000514
为待求解值，其初始估计值为
Figure BDA00030060782500000515
和第K个至第K-1个关键帧之间所有激光帧之间激光雷达的位姿差值求积。
首先将当前关键帧的一个特征点由初始估计
Figure BDA00030060782500000516
映射到世界坐标系下，对于映射完成的一个特征点i，
Figure BDA00030060782500000517
为其对应特征地图的临近点，计算
Figure BDA00030060782500000518
的协方差矩阵M，求出其特征值V和特征向量E，若
Figure BDA00030060782500000519
对应的三个特征值中，一个特征值大于其余两个超过设定阈值，
Figure BDA00030060782500000520
的分布为边缘线，则最大特征值对应的特征向量代表了该边缘线的方向向量；若
Figure BDA00030060782500000521
有一特征值小于其余两个超过设阈值，
Figure BDA00030060782500000522
的分布为平面，最小特征值对应的特征向量代表了该平面的法向量；
Figure BDA0003006078250000061
的重心唯一确定了线或者平面的位置。
构建第K个至第K-1个关键帧中所有特征点的误差项Di由此构造最小二乘函数，即
Figure BDA0003006078250000062
其中，向量F的每一行代表了一个特征点，变量
Figure BDA0003006078250000063
为第K-1帧到第K帧的变换矩阵，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑Di，求解出的
Figure BDA0003006078250000064
即为第K个关键帧和第K-1个关键帧之间激光雷达的位姿差值，结合初始估计值，得到第K个关键帧中激光雷达在世界坐标系下的位姿，即为激光雷达的激光里程计位姿并输出。
进一步地，因子图模型通过增量式平滑与映射Incremental Smoothing andMapping Using the Bayes Tree即ISAM2算法求解。
本发明的另外一个实施例还提供了激光雷达和惯性测量单元融合的即时定位与建图系统，其特征在于，包括标定模块、激光里程计以及因子图位姿融合模块。
标定模块，用于对3D激光雷达和惯性测量单元IMU进行联合标定。
3D激光雷达获取原始点云数据，以激光帧的形式送入激光里程计。
惯性测量单元IMU获取激光雷达位姿送入激光里程计。
激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz。
高频激光里程计，对原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值。
低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出。
因子图位姿融合模块，用于构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
有益效果：
本发明提供了3D激光雷达和惯性测量单元融合的即时定位与建图系统，前端使用高频激光里程计和低频激光里程计进行配合得到较为精确的机器人位姿结果，根据手眼标定方程来实现惯性导航器件运动到激光雷达运动的推算，构建因子图模型进行融合优化，抑制了激光雷达里程计存在的姿态角累积误差，进而能够在大尺度环境中获得更精确的机器人位姿和高精度的点云地图。本发明解决了纯激光雷达里程计的姿态角估计偏差问题和高度误差累积问题，其效果要比直接融合更为鲁棒。
附图说明
图1为基于因子图优化的激光雷达和惯性导航单元融合的即时定位与建图框架图；
图2为激光里程计算法框架图；
图3为算法对比图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明提供了一种基于因子图优化的3D激光雷达和惯性测量单元融合的即时定位与建图框架，该系统整体框架如图1所示。
本发明实施例提供的激光雷达和惯性测量单元融合的即时定位与建图方法，包括如下步骤：
步骤一、对激光雷达和惯性测量单元IMU进行联合标定。
为了确定激光雷达和IMU的相对旋转矩阵
Figure BDA0003006078250000081
利用手眼标定方程对激光雷达和IMU进行联合标定，该步骤在离线条件室内特征点丰富环境下进行。
(1)手眼标定方程
手眼标定方程TATX＝TXTB，其中TA为A坐标系下从初始时刻到当前时刻变换矩阵，TB为B坐标系下初始时刻到当前时刻变换矩阵，TX为A到B坐标系的变换矩阵，针对激光雷达和IMU刚性连接，可表示为
Figure BDA0003006078250000082
其中：L0为激光雷达初始帧坐标系，L为激光雷达当前帧坐标系，I为IMU坐标系，
Figure BDA0003006078250000083
为激光雷达当前帧坐标系到初始帧坐标系的变换矩阵，
Figure BDA0003006078250000084
为激光雷达坐标系下IMU坐标系的变换矩阵，也就是待求出的外参变换矩阵，
Figure BDA0003006078250000085
为IMU从当前时刻到初始时刻的位姿变换关系。
其推导过程如下：
Figure BDA0003006078250000086
其中：W为世界坐标系，在此处为IMU参考的东北天坐标系。
Figure BDA0003006078250000087
Figure BDA0003006078250000088
Figure BDA0003006078250000091
Figure BDA0003006078250000092
由于传感器刚性连接，所以
Figure BDA0003006078250000093
标定方程得证。
(2)手眼标定方程求解
为了标定准确，减少观测中噪声对标定结果的影响，需要多组观测中进行求解，求解TATX＝TXTB方程即可转化为最小二乘问题。
Figure BDA0003006078250000094
其中，k为观测次数，d(·)为两矩阵的距离，
Figure BDA0003006078250000095
和
Figure BDA0003006078250000096
意为第i次观测的TA和TB。
该最小二乘问题求解方法如下：
TATX＝TXTB写成矩阵形式为
Figure BDA0003006078250000097
其中，RA为TA的旋转矩阵，tA为TA的平移向量，RX为TX的旋转矩阵，tX为TX的平移向量，RB为TB的旋转矩阵，tB为TB的平移向量。
将上式展开可得
RARX＝RXRB
RAbX+bA＝RXbB+bX
令logRA＝α,logRB＝β，α，β为旋转矩阵RA，RB对应的李代数，则RARX＝RXRB可以化为:
α＝RXβRX T＝[RXβ]
其中，log(·)为旋转矩阵从李群到李代数的转换，其满足对数映射：
Figure BDA0003006078250000098
1+2cosφ＝tr(R)
R为李群表示的旋转矩阵，φ为R对应的李代数。
RARX＝RXRB可以写成α＝RXβ，当存在多组观测值时，求解该方程可以转化为以下最小二乘问题：
Figure BDA0003006078250000101
其中，k为观测次数，βi为第i次观测的β，αi为第i次观测的α。
最优解为
Figure BDA0003006078250000102
以上为该手眼标定方程的求解方法。输入矩阵RA，RB，即A，B坐标系下的累积旋转矩阵，在该问题中RA为激光雷达坐标系下的累积旋转矩阵，RB为imu坐标系下的累积旋转矩阵，待求量RX为激光雷达和IMU的相对旋转矩阵
Figure BDA0003006078250000103
其中，RA，RB通过激光里程计和IMU观测值给出。此时，IMU给出的累积旋转矩阵可统一转换为激光雷达坐标系下的累积旋转矩阵
Figure BDA0003006078250000104
后面不再赘述。
步骤二、激光雷达获取原始点云数据，以激光帧的形式送入激光里程计；惯性测量单元IMU获取激光雷达位姿送入激光里程计。
步骤三、激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz。
高频激光里程计，对原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值。
低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出。
激光里程计框架如图2所示。其中具体可采用如下步骤：
(1)点云运动畸变去除
由于机械激光雷达扫描特性，在载体运动时，处理器读出的激光点云并非在同一个激光雷达坐标系下，而是分布在一个激光扫描周期内(通常为0.1s)激光雷达的运动轨迹上。为了准确地估计激光雷达的运动，对点云进行去运动畸变处理。
激光雷达生成的原始点云数据中，第k帧点云中每一个激光点i对应的激光雷达位姿
Figure BDA0003006078250000111
分布在第k-1帧和第k帧点云对应的世界坐标系下激光雷达位姿
Figure BDA0003006078250000112
之间，并认为第k帧点云最后一个接收到的激光点iend对应的激光雷达位姿
Figure BDA0003006078250000113
即为
Figure BDA0003006078250000114
第k-1帧和第k帧点云之间的激光雷达变换矩阵平移分量
Figure BDA0003006078250000115
为匀速运动，则第i个激光点对应的激光雷达位姿相对于
Figure BDA0003006078250000116
的平移分量
Figure BDA0003006078250000117
由第k-2帧和第k-1帧点云之间的激光雷达变换矩阵平移分量
Figure BDA0003006078250000118
进行时间线性插值给出，而每一个激光点相对于
Figure BDA0003006078250000119
的旋转分量
Figure BDA00030060782500001110
则依靠惯性测量单元IMU的测量差值给出；由此求得激光点i相对于点iend对应的激光雷达运动关系矩阵
Figure BDA00030060782500001111
将第k帧点云每一个激光点i根据
Figure BDA00030060782500001112
变换到iend对应的激光雷达位姿
Figure BDA00030060782500001117
下，得到除去运动畸变的第k帧点云。
(2)特征点提取
点云运动畸变校正完成后，参考LOAM算法进行了特征点提取，以VLP_16激光雷达为例，激光雷达在垂直方向上有16条线束，垂直视场角为-15度到15度，每一线束依据曲率公式提取出点云中的边缘点和角点，曲率公式如下：
Figure BDA00030060782500001113
Figure BDA00030060782500001114
为激光雷达坐标系{L}下第k条线束第i个点，|s|为该条线束上的所有点的数量。其中，每条线束中曲率最大的点提取到边缘点集
Figure BDA00030060782500001115
曲率最小的提取为平面点集
Figure BDA00030060782500001116
为了提取出整个环境中的特征点，将点云按激光束的水平角均分成4部分，每一部分点云最多提供2个边缘点和4个平面点。第k条线束第i个激光点c；
(3)高频运动估计
为了粗略的估计地面无人平台在两次激光帧之间的相对运动，进行第k帧特征点云
Figure BDA0003006078250000121
与k-1帧特征点云
Figure BDA0003006078250000122
的配准，配准算法的思想是，根据当前帧与前一帧的误差构造最小二乘函数求解，求解出第k-1帧到第k帧激光雷达的变换矩阵
Figure BDA0003006078250000123
高频激光里程计中，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值，具体为：
配准算法流程如下：
1、边缘点误差
对于当前帧(第k帧)中的某点
Figure BDA0003006078250000124
(
Figure BDA0003006078250000125
为第k帧边缘点特征点云)，找到最近点
Figure BDA0003006078250000126
(
Figure BDA0003006078250000127
为第k-1帧边缘点特征点云)，找到j点相邻线束最近点
Figure BDA0003006078250000128
则j点、l点组成了i点对应的边缘线，点到直线的距离dε可以表示如下：
Figure BDA0003006078250000129
该距离dε即为边缘点误差。
2、平面点误差
对于当前帧(第k帧)中的某点
Figure BDA00030060782500001210
(
Figure BDA00030060782500001211
为第k帧平面点特征点云)，同样找到最近点
Figure BDA00030060782500001212
(
Figure BDA00030060782500001213
为第k-1帧平面点特征点云)，找到与点j相同线束的最近点l找到
Figure BDA00030060782500001214
再寻找点j相邻线束最近点
Figure BDA00030060782500001215
则j,l,m为i对应的平面，点到平面的距离
Figure BDA00030060782500001216
可以表示如下：
Figure BDA0003006078250000131
该距离即为平面点误差。
3、运动估计
根据匀速运动假设，将所有第k帧的待匹配特征点i根据上一次运动估计得出的第k-2帧和第k-1帧之间激光雷达的变换矩阵
Figure BDA0003006078250000132
变换到第k-1帧坐标系，构建当前帧第k帧与第k-1帧中所有的边缘点和平面点的误差项di并累加，由此构造最小二乘函数，即
Figure BDA0003006078250000133
其中，向量f的每一行代表了一个特征点，变量
Figure BDA0003006078250000134
为第k-1帧到第k帧的变换矩阵，
Figure BDA0003006078250000135
为
Figure BDA0003006078250000136
的初始值，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑di，求解出的
Figure BDA0003006078250000137
即为第k个激光帧和第k-1个激光帧之间激光雷达的位姿差值。
(4)低频运动估计
低频运动估计与高频运动估计的不同为，低频运动估计是帧与局部地图的匹配，而且构造的误差项更复杂，效果也更好。其当前帧称为关键帧，关键帧序号K(与高频运动估计中的k相区别)、特征点云
Figure BDA0003006078250000138
(第K个关键帧所代表的特征点云)和关键帧时刻激光雷达世界坐标系下的位姿
Figure BDA0003006078250000139
保存在内存中，以便进行点云地图构建、局部点云地图生成和因子图节点初始化。局部点云地图
Figure BDA00030060782500001310
由时间序列上的过去10个关键帧点云投影到世界坐标系下拼接而成。
前一关键帧(第K-1帧)激光雷达世界坐标系下的位姿为
Figure BDA00030060782500001311
当前关键帧(第K帧)激光雷达世界坐标系下的位姿
Figure BDA00030060782500001312
为待求解值，其初始估计为
Figure BDA00030060782500001313
和高频运动估计出的两关键帧之间的差值
Figure BDA00030060782500001314
求积。
求解过程如下：首先将当前关键帧的点由初始估计
Figure BDA00030060782500001315
映射到世界坐标系下，对于转换完成一特征点i，
Figure BDA00030060782500001316
为其对应特征地图的临近点，计算
Figure BDA00030060782500001317
的协方差矩阵M，求出其特征值V和特征向量E，若
Figure BDA0003006078250000141
分布近似为边缘线，一共三个特征值则其一个特征值远大于其余两个，并且最大特征值对应的特征向量代表了该边缘线的方向向量。若
Figure BDA0003006078250000142
近似为平面，则有一特征值远远小于其余两个，且该特征值对应的特征向量代表了该平面的法向量。
Figure BDA0003006078250000143
的重心唯一确定了线或者平面的位置。然后可以通过高频运动估计中的公式构建误差项、构建非线性最小二乘问题进行计算求解。线和平面的位置，计算点到直线的距离和点到面的距离构建误差项，
构建第K个至第K-1个关键帧中所有特征点的误差项Di由此构造最小二乘函数，即
Figure BDA0003006078250000144
其中，向量F的每一行代表了一个特征点，变量
Figure BDA0003006078250000145
为第K-1帧到第K帧的变换矩阵，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑Di，求解出的
Figure BDA0003006078250000146
即为第K个关键帧和第K-1个关键帧之间激光雷达的位姿差值，结合初始估计值，得到第K个关键帧中激光雷达在世界坐标系下的位姿，即为激光雷达的激光里程计位姿并输出。
步骤四、构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
为了提高算法的精确性，抑制姿态角累积误差和高度累积误差，构建因子图模型对激光里程计的结果和IMU的观测值进行图优化。构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
图模型通过增量式平滑与映射(Incremental Smoothing and Mapping Usingthe Bayes Tree，ISAM2)算法求解。其与纯激光里程计效果对比如图3所示。对变量节点进行整体调整，输出优化后的变量节点；由此得到关键帧时刻的所有位姿，所有关键帧时刻位姿拼接成3D点云地图。
本发明另外一个实施例还提供了激光雷达和惯性测量单元融合的即时定位与建图系统，包括标定模块、激光里程计以及因子图位姿融合模块。
标定模块，用于对3D激光雷达和惯性测量单元IMU进行联合标定。
3D激光雷达获取原始点云数据，以激光帧的形式送入激光里程计。
惯性测量单元IMU获取激光雷达位姿送入激光里程计。
激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz。
高频激光里程计，对原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值。
低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出。
因子图位姿融合模块，用于构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.激光雷达和惯性测量单元融合的定位与建图方法，其特征在于，包括如下步骤：
步骤一、对激光雷达和惯性测量单元IMU进行联合标定；
步骤二、激光雷达获取原始点云数据，以激光帧的形式送入激光里程计；
惯性测量单元IMU获取激光雷达位姿送入所述激光里程计；
步骤三、所述激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz；
所述高频激光里程计，对所述原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值；
所述低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与所述局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出；
步骤四、构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对所述因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
2.如权利要求1所述的方法，其特征在于，所述对激光雷达和惯性测量单元IMU进行联合标定，具体为：
利用手眼标定方程对激光雷达和惯性测量单元IMU进行联合标定。
3.如权利要求1所述的方法，其特征在于，所述高频激光里程计中，对所述原始点云数据进行运动畸变去除，具体为：
激光雷达生成的原始点云数据中，第k帧点云中每一个激光点i对应的激光雷达位姿
Figure FDA0003725065860000021
分布在第k-1帧和第k帧点云对应的世界坐标系下激光雷达位姿
Figure FDA0003725065860000022
之间，并认为第k帧点云最后一个接收到的激光点iend对应的激光雷达位姿
Figure FDA0003725065860000023
即为
Figure FDA0003725065860000024
第k-1帧和第k帧点云之间的激光雷达变换矩阵平移分量
Figure FDA0003725065860000025
为匀速运动，则第i个激光点对应的激光雷达位姿相对于
Figure FDA0003725065860000026
的平移分量
Figure FDA0003725065860000027
由第k-2帧和第k-1帧点云之间的激光雷达变换矩阵平移分量
Figure FDA0003725065860000028
进行时间线性插值给出，而每一个激光点相对于
Figure FDA0003725065860000029
的旋转分量
Figure FDA00037250658600000210
则依靠所述惯性测量单元IMU的测量差值给出；由此求得激光点i相对于点iend对应的激光雷达运动关系矩阵
Figure FDA00037250658600000211
将第k帧点云每一个激光点i根据
Figure FDA00037250658600000212
变换到iend对应的激光雷达位姿
Figure FDA00037250658600000213
下，得到除去运动畸变的第k帧点云。
4.如权利要求3所述的方法，其特征在于，所述特征点提取高频激光里程计中，特征点提取具体采用如下方法：
特征点包括边缘点和平面点；
首先将去除运动畸变的点云按照激光束的水平角均分为四部分，对于每一部分点云，求解其中每条线束中每个点的曲率，则每条线束中曲率最大的点作为边缘点，提取到边缘点集
Figure FDA00037250658600000214
每条线束中曲率最小的点作为平面点，提取到平面点集
Figure FDA00037250658600000215
每一部分点云最多提供2个边缘点和4个平面点，由此得到特征点云。
5.如权利要求4所述的方法，其特征在于，所述高频激光里程计中，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值，具体为：
进行第k帧特征点云
Figure FDA0003725065860000031
与k-1帧特征点云
Figure FDA0003725065860000032
的配准；其中第k帧特征点云包括第k帧边缘点集
Figure FDA0003725065860000033
第k帧平面点集
Figure FDA0003725065860000034
k-1帧特征点云包括第k-1帧边缘点集
Figure FDA0003725065860000035
第k-1帧平面点集
Figure FDA0003725065860000036
将所有第k帧的待匹配特征点i根据上一次运动估计得出的第k-2帧和第k-1帧之间激光雷达的变换矩阵
Figure FDA0003725065860000037
变换到第k-1帧坐标系，构建当前帧第k帧与第k-1帧中所有的边缘点和平面点的误差项di并累加，由此构造非线性最小二乘函数，即
Figure FDA0003725065860000038
其中，向量f的每一行代表了一个特征点，变量
Figure FDA0003725065860000039
为第k-1帧到第k帧的变换矩阵，
Figure FDA00037250658600000310
为
Figure FDA00037250658600000311
的初始值，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑di，求解出的
Figure FDA00037250658600000312
即为第k个激光帧和第k-1个激光帧之间激光雷达的位姿差值。
6.如权利要求5所述的方法，其特征在于，所述低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与所述局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出，具体为：
根据低频激光里程计的处理频率选取关键帧，关键帧序号K，关键帧K对应的特征点云的平面点集为
Figure FDA00037250658600000313
边缘点集
Figure FDA00037250658600000314
关键帧时刻激光雷达世界坐标系下的位姿为
Figure FDA00037250658600000315
局部点云地图
Figure FDA00037250658600000316
由时间序列上的过去10个关键帧点云投影到世界坐标系下拼接而成；
前一关键帧即第K-1个关键帧激光雷达世界坐标系下的位姿为
Figure FDA00037250658600000317
第K个关键帧激光雷达世界坐标系下的位姿
Figure FDA00037250658600000318
为待求解值，其初始估计值为
Figure FDA00037250658600000319
和第K个至第K-1个关键帧之间所有激光帧之间激光雷达的位姿差值求积；
首先将当前关键帧的一个特征点由初始估计
Figure FDA00037250658600000320
映射到世界坐标系下，对于映射完成的一个特征点i，
Figure FDA00037250658600000321
为其对应特征地图的临近点，计算
Figure FDA00037250658600000322
的协方差矩阵M，求出其特征值V和特征向量E，若
Figure FDA0003725065860000041
对应的三个特征值中，一个特征值大于其余两个且超过设定阈值，
Figure FDA0003725065860000042
的分布为边缘线，则最大特征值对应的特征向量代表了该边缘线的方向向量；若
Figure FDA0003725065860000043
有一特征值小于其余两个且超过设定阈值，
Figure FDA0003725065860000044
的分布为平面，最小特征值对应的特征向量代表了该平面的法向量；
Figure FDA0003725065860000045
的重心唯一确定了线或者平面的位置；
构建第K个至第K-1个关键帧中所有特征点的误差项Di由此构造非线性最小二乘函数，即
Figure FDA0003725065860000046
其中，向量F的每一行代表了一个特征点，变量
Figure FDA0003725065860000047
为第K-1帧到第K帧的变换矩阵，用莱文贝格-马夸特方法求解该非线性最小二乘函数，最小化∑Di，求解出的
Figure FDA0003725065860000048
即为第K个关键帧和第K-1个关键帧之间激光雷达的位姿差值，结合初始估计值，得到第K个关键帧中激光雷达在世界坐标系下的位姿，即为激光雷达的激光里程计位姿并输出。
7.如权利要求6所述的方法，其特征在于，所述因子图模型通过增量式平滑与映射Incremental Smoothing and Mapping Using the Bayes Tree即ISAM2算法求解。
8.激光雷达和惯性测量单元融合的定位与建图系统，其特征在于，包括标定模块、激光里程计以及因子图位姿融合模块；
所述标定模块，用于对3D激光雷达和惯性测量单元IMU进行联合标定；
3D激光雷达获取原始点云数据，以激光帧的形式送入所述激光里程计；
惯性测量单元IMU获取激光雷达位姿送入所述激光里程计；
所述激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz；
所述高频激光里程计，对所述原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值；
所述低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与所述局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出；
所述因子图位姿融合模块，用于构建因子图模型G＝(X,I,EI,EL)；其中因子图模型的变量节点为X＝{X1,X2,X3,...,XK}，X1,X2,X3,...,XK为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I1,I2,I3,...,IK}，I1,I2,I3,...,IK为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边EI连接变量节点和因子节点；二元边EL为激光里程计得到的3D激光雷达的位姿差值；对所述因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。
CN202110362412.XA 2021-04-02 2021-04-02 激光雷达和惯性测量单元融合的定位与建图方法及系统 Active CN113066105B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110362412.XA CN113066105B (zh) 2021-04-02 2021-04-02 激光雷达和惯性测量单元融合的定位与建图方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110362412.XA CN113066105B (zh) 2021-04-02 2021-04-02 激光雷达和惯性测量单元融合的定位与建图方法及系统

Publications (2)

Publication Number Publication Date
CN113066105A CN113066105A (zh) 2021-07-02
CN113066105B true CN113066105B (zh) 2022-10-21

Family

ID=76565503

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110362412.XA Active CN113066105B (zh) 2021-04-02 2021-04-02 激光雷达和惯性测量单元融合的定位与建图方法及系统

Country Status (1)

Country Link
CN (1) CN113066105B (zh)

Families Citing this family (55)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113379915B (zh) * 2021-07-05 2022-12-23 广东工业大学 一种基于点云融合的行车场景构建方法
CN113686600B (zh) * 2021-07-13 2024-06-21 江苏省农业机械试验鉴定站 一种旋耕机、开沟机性能鉴定装置
CN113554705B (zh) * 2021-07-14 2024-03-19 南京航空航天大学 一种变化场景下的激光雷达鲁棒定位方法
CN113570715B (zh) * 2021-07-23 2023-10-13 东北大学 基于传感器融合的旋转激光实时定位建模系统及方法
CN114088104B (zh) * 2021-07-23 2023-09-29 武汉理工大学 一种自动驾驶场景下的地图生成方法
CN113740871B (zh) * 2021-07-30 2024-04-02 西安交通大学 一种在高动态环境下的激光slam方法、系统设备及存储介质
CN113587934B (zh) * 2021-07-30 2024-03-19 深圳市普渡科技有限公司 一种机器人、室内定位方法、装置和可读存储介质
CN113671530B (zh) * 2021-08-06 2024-01-12 北京京东乾石科技有限公司 位姿确定方法、装置及存储介质和电子设备
CN113640778A (zh) * 2021-08-12 2021-11-12 东风悦享科技有限公司 一种基于无重叠视场的多激光雷达的联合标定方法
CN113763549B (zh) * 2021-08-19 2023-07-07 同济大学 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN113658337B (zh) * 2021-08-24 2022-05-03 哈尔滨工业大学 一种基于车辙线的多模态里程计方法
CN113534097B (zh) * 2021-08-27 2023-11-24 北京工业大学 一种适用于旋轴激光雷达的优化方法
CN113935904A (zh) * 2021-08-27 2022-01-14 清华大学 激光里程计误差修正方法、系统、存储介质及计算设备
CN113744398B (zh) * 2021-09-03 2023-04-28 电子科技大学 一种基于激光与微波协同的地图重构融合方法
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN113781582B (zh) * 2021-09-18 2023-09-19 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN113985429B (zh) * 2021-09-23 2024-07-26 天津大学 基于三维激光雷达的无人机环境扫描与重构方法
CN113884090A (zh) * 2021-09-28 2022-01-04 中国科学技术大学先进技术研究院 智能平台车环境感知系统及其数据融合方法
CN114018236B (zh) * 2021-09-30 2023-11-03 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN113878578B (zh) * 2021-09-30 2024-01-16 上海景吾智能科技有限公司 适用于复合型机器人的动态自适应定位方法和系统
CN114018254B (zh) * 2021-10-27 2024-03-12 南京师范大学 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114018248B (zh) * 2021-10-29 2023-08-04 同济大学 一种融合码盘和激光雷达的里程计方法与建图方法
CN114136311B (zh) * 2021-11-08 2023-08-04 上海应用技术大学 一种基于imu预积分的激光slam定位方法
CN114234976A (zh) * 2021-11-29 2022-03-25 山东恒创智控科技有限公司 一种改进型slam与多传感器融合的机器人定位方法及系统
CN114459500B (zh) * 2021-12-01 2024-05-24 岱悟智能科技(上海)有限公司 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质
CN114170280B (zh) * 2021-12-09 2023-11-28 北京能创科技有限公司 基于双窗口的激光里程计方法、系统、装置
CN114264301B (zh) * 2021-12-13 2024-06-21 青岛慧拓智能机器有限公司 车载多传感器融合定位方法、装置、芯片及终端
CN114440892B (zh) * 2022-01-27 2023-11-03 中国人民解放军军事科学院国防科技创新研究院 一种基于拓扑地图和里程计的自定位方法
CN114440928A (zh) * 2022-01-27 2022-05-06 杭州申昊科技股份有限公司 激光雷达与里程计的联合标定方法、机器人、设备和介质
CN114459474B (zh) * 2022-02-16 2023-11-24 北方工业大学 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN114526745B (zh) * 2022-02-18 2024-04-12 太原市威格传世汽车科技有限责任公司 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN114563000B (zh) * 2022-02-23 2024-05-07 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN114563795B (zh) * 2022-02-25 2023-01-17 湖南大学无锡智能控制研究院 基于激光里程计和标签融合算法的定位追踪方法及系统
CN114581481B (zh) * 2022-03-07 2023-08-25 广州小鹏自动驾驶科技有限公司 一种目标物速度的估计方法及装置、车辆和存储介质
CN114518111A (zh) * 2022-03-11 2022-05-20 六安智梭无人车科技有限公司 一种激光雷达与惯性测量单元标定方法及系统
CN114608561B (zh) * 2022-03-22 2024-04-26 中国矿业大学 一种基于多传感器融合的定位与建图方法及系统
CN114674311B (zh) * 2022-03-22 2024-05-24 中国矿业大学 一种室内定位与建图方法及系统
CN114659518B (zh) * 2022-03-29 2024-05-14 亿嘉和科技股份有限公司 一种固定场景下的高精鲁棒定位方法
CN114838726B (zh) * 2022-04-20 2024-07-30 哈尔滨理工大学 一种基于多传感器数据融合的gps数据修正方法
CN115143958A (zh) * 2022-04-28 2022-10-04 西南科技大学 一种基于gpu加速的多传感器融合的slam方法
CN114777775B (zh) * 2022-05-06 2024-06-14 浙江师范大学 一种多传感器融合的定位方法及系统
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN115512054B (zh) * 2022-09-06 2024-07-12 武汉大学 三维时空连续点云地图的构建方法
CN115435816B (zh) * 2022-11-07 2023-04-11 山东大学 在线双舵轮agv内外参标定方法、系统、介质及设备
CN116299500B (zh) * 2022-12-14 2024-03-15 江苏集萃清联智控科技有限公司 一种融合目标检测和跟踪的激光slam定位方法及设备
CN115638787B (zh) * 2022-12-23 2023-03-21 安徽蔚来智驾科技有限公司 一种数字地图生成方法、计算机可读存储介质及电子设备
CN115655262B (zh) * 2022-12-26 2023-03-21 广东省科学院智能制造研究所 基于深度学习感知的多层级语义地图构建方法和装置
CN116295507B (zh) * 2023-05-26 2023-08-15 南京师范大学 一种基于深度学习的激光惯性里程计优化方法、系统
CN116678427B (zh) * 2023-06-25 2024-07-19 东南大学 基于城市峡谷稀疏特征地图约束的动态定位方法和装置
CN116929338B (zh) * 2023-09-15 2024-01-09 深圳市智绘科技有限公司 地图构建方法、设备及存储介质
CN116977226B (zh) * 2023-09-22 2024-01-19 天津云圣智能科技有限责任公司 点云数据分层的处理方法、装置、电子设备及存储介质
CN117092624A (zh) * 2023-09-27 2023-11-21 山东大学 一种外参标定方法、系统、介质及设备
CN117269977A (zh) * 2023-11-23 2023-12-22 湖南大学 基于垂直优化的激光slam实现方法和系统
CN117470280B (zh) * 2023-12-21 2024-04-05 绘见科技(深圳)有限公司 激光slam实时精度评估方法、装置、介质和设备
CN117495968B (zh) * 2024-01-02 2024-05-17 苏州中德睿博智能科技有限公司 基于3d激光雷达的移动机器人位姿跟踪方法及装置

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN112082545A (zh) * 2020-07-29 2020-12-15 武汉威图传视科技有限公司 一种基于imu和激光雷达的地图生成方法、装置及系统

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3078935A1 (en) * 2015-04-10 2016-10-12 The European Atomic Energy Community (EURATOM), represented by the European Commission Method and device for real-time mapping and localization
WO2018048353A1 (en) * 2016-09-09 2018-03-15 Nanyang Technological University Simultaneous localization and mapping methods and apparatus
CN107179086B (zh) * 2017-05-24 2020-04-24 北京数字绿土科技有限公司 一种基于激光雷达的制图方法、装置及系统
CN111045017B (zh) * 2019-12-20 2023-03-31 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111207774B (zh) * 2020-01-17 2021-12-03 山东大学 一种用于激光-imu外参标定的方法及系统
CN112347840B (zh) * 2020-08-25 2022-12-02 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN111983639B (zh) * 2020-08-25 2023-06-02 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN112082545A (zh) * 2020-07-29 2020-12-15 武汉威图传视科技有限公司 一种基于imu和激光雷达的地图生成方法、装置及系统

Also Published As

Publication number Publication date
CN113066105A (zh) 2021-07-02

Similar Documents

Publication Publication Date Title
CN113066105B (zh) 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
WO2021232470A1 (zh) 基于多传感器融合的slam制图方法、系统
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
Anderson et al. Towards relative continuous-time SLAM
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112254729B (zh) 一种基于多传感器融合的移动机器人定位方法
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
CN111156997B (zh) 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN111415417B (zh) 一种集成稀疏点云的移动机器人拓扑经验地图构建方法
CN111780781B (zh) 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN111366153B (zh) 一种激光雷达与imu紧耦合的定位方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
Huang et al. Extrinsic multi-sensor calibration for mobile robots using the Gauss-Helmert model
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN111474932A (zh) 一种集成情景经验的移动机器人建图与导航方法
CN114234967A (zh) 一种基于多传感器融合的六足机器人定位方法
CN114777775A (zh) 一种多传感器融合的定位方法及系统
CN112284381B (zh) 视觉惯性实时初始化对准方法及系统
CN112837314A (zh) 基于2D-LiDAR和Kinect的果树冠层参数检测系统和方法

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