CN112985416B - 激光与视觉信息融合的鲁棒定位和建图方法及系统 - Google Patents

激光与视觉信息融合的鲁棒定位和建图方法及系统 Download PDF

Info

Publication number
CN112985416B
CN112985416B CN202110415828.3A CN202110415828A CN112985416B CN 112985416 B CN112985416 B CN 112985416B CN 202110415828 A CN202110415828 A CN 202110415828A CN 112985416 B CN112985416 B CN 112985416B
Authority
CN
China
Prior art keywords
visual
pose
laser
current
robot
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
CN202110415828.3A
Other languages
English (en)
Other versions
CN112985416A (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.)
Hunan Xinxin Xiangrong Intelligent Technology Co ltd
Original Assignee
Hunan University
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 Hunan University filed Critical Hunan University
Priority to CN202110415828.3A priority Critical patent/CN112985416B/zh
Publication of CN112985416A publication Critical patent/CN112985416A/zh
Application granted granted Critical
Publication of CN112985416B publication Critical patent/CN112985416B/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/20—Instruments for performing navigational calculations
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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

Abstract

本发明公开了一种激光与视觉信息融合的鲁棒定位和建图方法及系统，本发明包括将融合计算非视觉位姿T t m ；根据视觉图像估计视觉位姿T t v ，若估计失败则使用非视觉位姿T t m 进行非视觉辅助重定位计算视觉位姿T t v ；对视觉位姿T t v 和非视觉位姿T t m 融合得到融合后的机器人位姿T t f 并进行闭环检测，在检测到闭环后采用基于图优化的闭环优化算法对机器人位姿进行全局优化，从而得到二维栅格地图和三维点云地图。本发明能够在机器人运动过快、光照环境极端、场景纹理特征缺失等条件恶劣的情况下，融合多种传感器信息，解决视觉SLAM跟踪丢失的问题，提高机器人的定位精度，同时构建精确的二维栅格地图和三维点云地图。

Description

激光与视觉信息融合的鲁棒定位和建图方法及系统
技术领域
本发明涉及多传感器信息融合的机器人定位技术，具体涉及一种激光与视觉信息融合的鲁棒定位和建图方法及系统。
背景技术
近年来，随着机器人技术的快速发展，同时定位和地图构建（SLAM）作为机器人领域的基础关键技术，仍存在一些尚未解决的问题，针对使用不同传感器的SLAM方案，主要可以分为激光雷达SLAM和视觉SLAM。激光雷达SLAM主要使用的传感器是激光雷达，激光雷达具有精度高，速度快，不受环境光照影响等优点，并且可以准确获取与障碍物之间的距离和角度信息。但是二维激光雷达只能获取一个平面上的信息，信息量不足，无法检测激光扫描平面外的障碍物。目前三维激光雷达的价格普遍非常昂贵，出于成本考虑主要应用于无人驾驶汽车领域，并不适合应用于机器人。视觉SLAM得益于相机技术、计算机性能的快速发展，逐渐涌现很多优秀的定位建图方法，但是普遍的问题就是在大环境下会出现累积误差，而且在相机运动过快、光照条件极端、场景纹理特征严重缺失的情况下，视觉SLAM存在跟踪失败、定位精度差、建图效果不佳等问题。由于单独使用激光雷达或者视觉都存在不可避免的缺点，所以，融合激光雷达和深度相机的信息可以实现优缺点互补，弥补单个传感器不可避免的缺点，提供更加精确且鲁棒的环境地图。
发明内容
本发明要解决的技术问题：针对现有技术的上述问题，提供一种激光与视觉信息融合的鲁棒定位和建图方法及系统，本发明能够在机器人运动过快、光照环境极端、场景纹理特征缺失等条件恶劣的情况下，融合多种传感器信息，解决视觉SLAM跟踪丢失的问题，提高机器人的定位精度，同时构建精确的二维栅格地图和三维点云地图。
为了解决上述技术问题，本发明采用的技术方案为：
一种激光与视觉信息融合的鲁棒定位和建图方法，包括：
1）将惯性测量单元IMU、里程计及激光雷达的位姿融合得到机器人当前时刻t的非视觉位姿T t m ；
2）根据视觉图像中的当前帧与参考关键帧进行特征提取与匹配估计机器人当前时刻t的视觉位姿T t v ，若估计失败则使用非视觉位姿T t m 进行非视觉辅助重定位，最终获取机器人当前时刻t的视觉位姿T t v ；
3）对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，得到融合后的机器人位姿T t f ；
4）针对融合后的机器人位姿T t f 进行闭环检测，在检测到闭环后采用基于图优化的闭环优化算法对机器人位姿进行全局优化，从而得到二维栅格地图和三维点云地图。
可选地，步骤1）包括：
1.1）采用误差状态卡尔曼滤波器对惯性测量单元IMU的位姿进行积分得到标准状态，将里程计数据通过里程计运动模型对误差状态卡尔曼滤波器进行观测更新估计误差状态，利用误差状态修正标准状态，得到真实状态并重置误差状态；
1.2）将误差状态卡尔曼滤波器输出的真实状态作为激光雷达扫描匹配的初始位姿，将激光雷达的数据与当前的子图进行相关扫描匹配，所述子图是指由激光雷达数据构成的一个局部地图，如果扫描匹配成功，则估计出当前时刻t的激光雷达位姿T t l ；如果扫描匹配失败，则根据该帧激光雷达数据构建新的子图，那么上一个子图将会加入到全局地图中，所述全局地图为由所有的子图构成的地图，将已完成创建的全部子图进行全局位姿优化，确定当前时刻t的激光雷达位姿T t l ；
1.3）使用得到的激光雷达位姿T t l 对误差状态卡尔曼滤波器进行观测更新，得到更加精确的机器人当前时刻t的非视觉位姿T t m 。
可选地，步骤2）中使用非视觉位姿T t m 进行非视觉辅助重定位的步骤包括：
2.1）根据T t v =T t-1 v (T t-1 m ) -1 T t m 计算当前时刻t的视觉位姿T t v ，其中，T t m 为当前时刻t的非视觉位姿，T t-1 m 为上一时刻t-1的非视觉位姿，T t-1 v 为上一时刻t-1的视觉位姿；
2.2）根据当前时刻t的视觉位姿T t v 、当前时刻t的邻域ε内的视觉位姿T i v 构建目标函数并进行局部位姿图优化，最终得到最优的机器人的视觉位姿T t v 。
可选地，步骤2.2）中构建的目标函数的函数表达式为：
Figure 726910DEST_PATH_IMAGE001
上式中，T*表示目标优化位姿，ε为时间邻域， T={T t - ε v ,…,T t v }表示当前时刻t的ε邻域内的位姿序列，T t,i v =(T t v )-1 T i v ，T t,i v 表示时刻i和当前时刻t之间的相对位姿，T t v 为当前时刻t的视觉位姿、T i v 为当前时刻t的邻域ε内的视觉位姿, Cov t,i 为T t v 和T i v 之间的协方差矩阵，i表示在当前时刻t的邻域ε内的时刻，0<(t-i)<ε，
Figure 478965DEST_PATH_IMAGE002
表示从向量到矩阵的运算。
可选地，步骤3）包括：
3.1）在当前时刻t=0时机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m 均置为0；在当前时刻t>0时则根据上一时刻t-1的视觉位姿T t-1 v 、非视觉位姿T t-1 m 计算机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m ；在任意当前时刻t，当视觉传感器的速度V t m 为0时，将上一帧的位姿作为当前帧的初始位姿，通过搜索视觉词典的方式在参考帧中找到与当前帧匹配的ORB特征点，优化每个特征点对应3D点重投影误差得到视觉图像中当前帧的视觉位姿T t v ；当视觉传感器的速度V t m 不为0时，根据恒速模型设定当前帧的初始位姿T t v =V t v T t-1 v ，通过投影的方式在参考帧中找到当前帧匹配的ORB特征点，优化每个特征点所对应的3D点重投影误差得到视觉图像中当前帧的视觉位姿T t v ；
3.2）采用扩展卡尔曼滤波器对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，且以视觉位姿T t v 和非视觉位姿T t m 作为观测量，对扩展卡尔曼滤波器进行更新，得到融合后的机器人位姿T t f 。
可选地，步骤4）包括：
4.1）计算视觉图像中与当前帧具有连接关系的所有关键帧的视觉词典得分，所述与当前帧具有连接关系是指与当前帧具有部分公共的特征点；
4.2）根据视觉词典得分，找出和当前帧相似的视觉闭环候选关键帧，并检测视觉闭环候选关键帧与当前帧的连续性，选出视觉闭环关键帧；
4.3）遍历视觉闭环关键帧，查找其与当前帧匹配的地图点；
4.4）计算当前帧的融合位姿T t f 和视觉闭环关键帧的融合位姿T k f 之间的旋转平移相似变换；
4.5）判断旋转平移相似变换在空间几何姿态上是否成立，若成立则判定视觉闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.6）对当前帧的位姿进行调整，并且传播到当前帧相连的关键帧，利用调整过的位姿更新这些相连关键帧对应的地图点；更新当前帧的连接关系，计算和当前帧连接的关键帧的旋转平移相似变换并校正位姿，更新这些关键帧的三维地图点坐标，并优化三维全局位姿图；
4.7）根据视觉闭环检测得到的结果，对当前时刻的激光雷达数据和视觉闭环关键帧对应时间的激光雷达数据进行闭环判断，若两帧激光雷达的位姿具有闭环相似性，则判定激光闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.8）将当前帧激光雷达数据加入视觉闭环关键帧对应时刻的激光子图中进行扫描匹配并计算其闭环约束，进行二维全局位姿优化，得到二维栅格地图和三维点云地图。
可选地，步骤4.7）中两帧激光雷达的位姿具有闭环相似性的函数表达式为：
Figure 301428DEST_PATH_IMAGE003
上式中，T t f 为当前时刻t的融合位姿，T k f 为视觉闭环关键帧对应时间的融合位姿，P t i 为当前时刻的激光雷达点云坐标，P k i 为视觉闭环关键帧对应时间的激光雷达点云坐标，p为激光点云个数，f loop (T t f )表示两帧激光雷达的位姿是否具有闭环相似性。
此外，本发明还提供一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行所述的激光与视觉信息融合的鲁棒定位和建图方法的步骤。
此外，本发明还提供一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，所述存储器中存储有被编程或配置以执行所述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
此外，本发明还提供一种计算机可读存储介质，该计算机可读存储介质中存储有被编程或配置以执行所述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
和现有技术相比，本发明具有下述优点：
1、本发明能够在机器人运动过快、光照环境极端、场景纹理特征缺失等条件恶劣的情况下，融合多种传感器信息，解决视觉SLAM跟踪丢失的问题，提高机器人的定位精度，同时构建精确的二维栅格地图和三维点云地图。
2、本发明通过融合激光雷达和IMU的数据，能减小IMU随时间增大时解算产生的累积误差，使激光雷达和IMU融合得到的机器人位姿更加精确。对视觉位姿和非视觉位姿进行时序位姿融合，有效减小纯视觉特征匹配所产生的累积误差，同时在视觉跟踪定位失败时，提出以激光雷达的位姿来辅助重定位，有效提高重定位效率。
3、本发明为非视觉位姿辅助重定位的建图效果，系统在视觉重定位失败的情况下，仍然可以使用非视觉重定位来估计当前视觉位姿，提高了建图系统的鲁棒性，充分利用视觉SLAM图像闭环检测的能力，可弥补激光雷达数据因数据稀疏而导致闭环能力不足的缺点。
附图说明
图1为本发明实施例方法的基本流程示意图。
图2为本发明实施例方法的基本原理示意图。
图3为本发明实施例中的位姿图优化示意图；
图4为本发明实施例中的3D重投影误差示意图；
图5为本发明实施例中步骤4）的流程图。
图6为作为对比采用纯视觉构建的地图。
图7为本发明实施例方法构建的地图。
图8为本发明实施例方法与其他算法的轨迹对比图。
图9为本发明实施例方法与其他算法的绝对轨迹误差指标对比图。
具体实施方式
如图1和图2所示，本实施例激光与视觉信息融合的鲁棒定位和建图方法包括：
1）将惯性测量单元IMU、里程计及激光雷达的位姿融合得到机器人的非视觉位姿T t m ，可消除惯性测量单元IMU随时间增加而产生的累积误差；
2）根据视觉图像中的当前帧与参考关键帧进行特征提取与匹配估计机器人的视觉位姿，若估计失败则使用非视觉位姿T t m 进行非视觉辅助重定位，最终获取机器人的视觉位姿T t v ；
3）对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，得到融合后的机器人位姿T t f ；
4）针对融合后的机器人位姿T t f 进行闭环检测，在检测到闭环后采用基于图优化的闭环优化算法对机器人位姿进行全局优化，从而得到二维栅格地图和三维点云地图。在检测到闭环后采用基于图优化的闭环优化算法对机器人位姿进行全局优化，实现了对所有地图点的机器人位姿进行协同优化，解决序列建图过程中机器人位姿估计随时间累计的问题，能够进一步提升建图精度。
本实施例中，步骤1）包括：
1.1）采用误差状态卡尔曼滤波器（Error-state Kalman Filter，ESKF）对惯性测量单元IMU的位姿进行积分得到标准状态，将里程计数据通过里程计运动模型对误差状态卡尔曼滤波器进行观测更新估计误差状态，利用误差状态修正标准状态，得到真实状态并重置误差状态；
误差状态卡尔曼滤波器（Error-state Kalman Filter，ESKF）的状态变量可分为标准状态x=[p,v,q,a bt ,w bt ]和误差状态δx[δp,δv,δq,δa bt ,δw bt ]，其中p为位移，v为速度，q为由四元数表示的位姿，a bt 为加速度偏置，w bt 为角速度偏置，δ为误差，误差状态卡尔曼滤波器的真实状态x truth 为标准状态x、误差状态δx之和，即：x truth =x+δx。因此，采用误差状态卡尔曼滤波器对惯性测量单元IMU的位姿进行积分得到标准状态x，将里程计数据通过里程计运动模型对误差状态卡尔曼滤波器进行观测更新估计误差状态δx，利用误差状态δx修正标准状态x，得到真实状态x truth 并重置误差状态。
1.2）将误差状态卡尔曼滤波器输出的真实状态作为激光雷达扫描匹配的初始位姿，将激光雷达的数据与当前的子图进行相关扫描匹配（Correlative Scan Match，CSM），子图是指由激光雷达数据构成的一个局部地图，如果扫描匹配成功，则估计出当前时刻t的激光雷达位姿T t l ；如果扫描匹配失败，则根据该帧激光雷达数据构建新的子图，那么上一个子图将会加入到全局地图中，所述全局地图为由所有的子图构成的地图，将已完成创建的全部子图进行全局位姿优化，确定当前时刻t的激光雷达位姿T t l ；
如果扫描匹配成功，令扫描匹配成功后的位姿为T * ，则有：
Figure 506144DEST_PATH_IMAGE004
上式中，n为激光点数量，S i (T)为把激光点i用位姿T进行转换后的坐标，M(S i (T))为坐标S i (T)的地图占用概率；R为激光雷达坐标系相对世界坐标系的3维旋转矩阵，
Figure 705044DEST_PATH_IMAGE005
为激光雷达坐标系相对世界坐标系的3维平移向量，
Figure 373923DEST_PATH_IMAGE006
为3维旋转矩阵R对应的3维旋转向量，^运算表示从向量到矩阵的运算，
Figure 302040DEST_PATH_IMAGE006
1～
Figure 56370DEST_PATH_IMAGE006
3 为3维旋转向量
Figure 793381DEST_PATH_IMAGE006
中的标量。
扫描匹配成功时估计出时刻的激光雷达位姿T t l 的计算函数表达式为：
Figure 316767DEST_PATH_IMAGE007
上式中，R t l 为在t时刻的旋转矩阵，t t l 为在当前时刻t的平移向量。
如果扫描匹配失败，则根据该帧激光雷达数据构建新的子图，那么上一个子图将会加入到全局地图（由所有的子图构成的地图）中，将已完成创建的全部子图进行全局位姿优化，确定当前时刻的激光雷达位姿T t l 。
1.3）使用得到的激光雷达位姿T t l 对误差状态卡尔曼滤波器进行观测更新，得到更加精确的机器人的非视觉位姿T t m 。将该帧激光雷达数据以机器人的非视觉位姿T t m 插入当前子图，如果该帧激光雷达位姿与其他某个子图中的某帧激光雷达数据的位姿具有相似性，则通过扫描匹配寻找闭环：
Figure 684294DEST_PATH_IMAGE008
上式中，f scan-match (T t m )表示当前帧（对应当前时刻t的帧）的激光雷达位姿T t m 与其他某个子图中的某帧激光雷达数据的位姿T k m （对应时刻k）之间的相似性，I为单位矩阵，T t m 为与当前位姿相似的某一帧非视觉位姿，T k m 为其他某个子图中的某帧激光雷达数据的位姿。
步骤2）根据视觉图像中的当前帧与参考关键帧进行特征提取与匹配估计机器人的视觉位姿时，若当前帧与参考帧的匹配点（ORB特征匹配点）数量少于系统设定阈值则判定估计失败，即：
Figure 925920DEST_PATH_IMAGE009
上式中，f match (F ref ,F cur )表示当前帧与参考帧的匹配点数量，ε为系统设定阈值。
视觉重定位是指从历史关键帧中找出与当前帧之间拥有充足匹配点的候选关键帧，利用随机采样一致性（Random Sample Concensus, RANSAC）迭代，通过3D-2D点匹配（Perspective n Point，PnP）求解当前帧的位姿，即视觉位姿T t v 。步骤2）中使用非视觉位姿T t m 进行非视觉辅助重定位的步骤包括：
2.1）根据T t v =T t-1 v (T t-1 m ) -1 T t m 计算当前时刻t的视觉位姿T t v ，其中，T t m 为当前时刻t的非视觉位姿，T t-1 m 为上一时刻t-1的非视觉位姿，T t-1 v 为上一时刻t-1的视觉位姿；
2.2）根据当前时刻t的视觉位姿T t v 、当前时刻t的邻域ε内的视觉位姿T i v 构建目标函数并进行局部位姿图优化，最终得到最优的机器人的视觉位姿T t v 。
当视觉重定位失败时则进行非视觉位姿辅助重定位。由于非视觉传感器与视觉传感器的工作频率不一致，所以需要对非视觉传感器位姿和视觉传感器位姿进行时间同步。在当前时刻t存在：当前时刻t的视觉位姿T t v ，上一时刻t-1的视觉位姿T t-1 v ；非视觉位姿T k+1 m 、T k m 、T k-1 m ，且有(k+1)/f m >t/f v >k/f m > (t-1)/f v >(k-1)/f m ，其中，k表示非视觉位姿的采样时刻，f m 表示表示非视觉位姿的采样频率，f v 表示视觉位姿的采样频率；
首先对非视觉位姿进行位姿插值计算T t m 和T t-1 m ：
Figure 201043DEST_PATH_IMAGE010
其中，α 1=(t-k)/[(k+1)-k]，α 2=((t-1)- (k-1))/[k-(k-1)]。
上式即为SE(3)插值方法，求解方法如下：
Figure 844514DEST_PATH_IMAGE011
上式中，T表示插值结果位姿，T 1为插值点1的位姿，T 2为插值点2的位姿，α为插值比例，ξ为T对应的SE(3)李代数，ρ为3维平移向量，
Figure 179680DEST_PATH_IMAGE006
为3维旋转矩阵R对应的3维旋转向量，^运算表示从向量到矩阵的运算。则有：
Figure 846285DEST_PATH_IMAGE012
上式中，n为泰勒展开式的多项式项数，
Figure 925100DEST_PATH_IMAGE002
表示从向量到矩阵的运算，ξ 21为插值点1与插值点2的相对位姿对应的SE(3)李代数，ξ 1为插值点1的位姿对应的SE(3)李代数。令：
Figure 423077DEST_PATH_IMAGE013
上式中，T 21为插值点1与插值点2的相对位姿，
Figure 194724DEST_PATH_IMAGE014
表示从矩阵到向量的运算，则T的求解可转换为ξ的求解。
步骤2.1）根据T t v =T t-1 v (T t-1 m ) -1 T t m 计算当前时刻t的视觉位姿T t v 时，求解得到的视觉传感器初始位姿T t v 是在假定视觉传感器和非视觉传感器的速度相等的基础上计算得到的，并没有考虑相邻关键帧之间的匹配约束关系。如图3所示，X t-4～X t为连续的5个关键帧，因此相邻关键帧之间存在匹配约束关系。因此，步骤2.2）根据当前时刻t的视觉位姿T t v 、当前时刻t的邻域ε内的视觉位姿T i v 构建目标函数并进行局部位姿图优化，最终得到最优的机器人的视觉位姿T t v 。本实施例中，步骤2.2）中构建的目标函数的函数表达式为：
Figure 145362DEST_PATH_IMAGE015
上式中，T*表示目标优化位姿，ε为时间邻域， T={T t - ε v ,…,T t v }表示当前时刻t的ε邻域内的位姿序列，T t,i v =(T t v )-1 T i v ，T t,i v 表示时刻i和当前时刻t之间的相对位姿，T t v 为当前时刻t的视觉位姿、T i v 为当前时刻t的邻域ε内的视觉位姿, Cov t,i 为T t v 和T i v 之间的协方差矩阵，i表示在当前时刻t的邻域ε内的时刻，0<(t-i)<ε。
本实施例中，步骤3）包括：
3.1）在当前时刻t=0（系统初始化时）时机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m 均置为0；在当前时刻t>0时则根据上一时刻t-1的视觉位姿T t-1 v 、非视觉位姿T t-1 m 计算机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m ；
本实施例中，步骤3.1）中根据上一时刻t-1的视觉位姿T t-1 v 、非视觉位姿T t-1 m 计算机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m 的函数表达式为：
Figure 27868DEST_PATH_IMAGE016
上式中，T t-1 v 为上一时刻t-1的视觉位姿，T t-2 v 为上上时刻t-2的视觉位姿，T t-1 m 为上一时刻t-1的非视觉位姿，T t-2 m 为上上时刻t-2的非视觉位姿。
在任意当前时刻t，当视觉传感器的速度V t m 为0时，将上一帧的位姿作为当前帧的初始位姿，通过搜索视觉词典（Bag of Words，BoW）的方式在参考帧中找到与当前帧匹配的ORB（Oriented FAST and Rotated BRIEF，有向快速和旋转描述）特征点，ORB特征点是有向快速角点和旋转描述子的合称，优化每个特征点对应3D点重投影误差（BundleAdjustment）得到视觉图像中当前帧的视觉位姿T t v ；如图4所示，P1～P3表示三个特征点，X t-3～X t为连续的4个关键帧，每一个特征点对应三个关键帧，优化每个特征点Pi三个关键帧对应3D点重投影误差（Bundle Adjustment），即可得到视觉图像中当前帧的视觉位姿T t v 。其中，搜索视觉词典（Bag of Words，BoW）的方式可参见文献：Gálvez-López D, Tardos J D.Bags of binary words for fast place recognition in image sequences[J]. IEEETransactions on Robotics. IEEE, 2012, 28(5): 1188-1197。ORB（Oriented FAST andRotated BRIEF）特征点的定义可参见文献：Rublee E, Rabaud V, Konolige K, et al.ORB: An efficient alternative to SIFT or SURF[C]. International conference oncomputer vision. IEEE, 2011: 2564-2571。优化每个特征点对应3D点重投影误差（Bundle Adjustment）得到视觉图像中当前帧的视觉位姿T t v 可参见文献：Triggs B,Mclauchlan P F, Hartley R, et al. Bundle Adjustment - A Modern Synthesis[C].International conference on computer vision. IEEE, 1999: 298-372，优化的目标函数为：
Figure 318035DEST_PATH_IMAGE017
上式中，T t v* 为当前时刻t的视觉位姿T t v 的目标函数，τ为优化中空间点的个数，u i 为某一空间点坐标P i 投影的像素坐标，s i 为缩放系数，K为相机内参矩阵。
在任意当前时刻t，当视觉传感器的速度V t m 不为0时，根据恒速模型设定当前帧的初始位姿T t v =V t v T t-1 v ，通过投影的方式在参考帧中找到当前帧匹配的ORB特征点，优化每个特征点所对应的3D点重投影误差得到视觉图像中当前帧的视觉位姿T t v ，与视觉传感器的速度V t m 为0的方法相似。
3.2）采用扩展卡尔曼滤波器对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，且以视觉位姿T t v 和非视觉位姿T t m 作为观测量，对扩展卡尔曼滤波器进行更新，得到融合后的机器人位姿T t f 。
采用扩展卡尔曼滤波器融合视觉位姿T t v 和非视觉位姿T t m 时的相关方程如下：
运动方程为：
Figure 260583DEST_PATH_IMAGE018
上式中，x k 为时刻k的状态向量，x k-1为时刻k-1的状态向量，v k 为时刻k的输入向量，w k 为运动方程中的零均值高斯噪声，f(x k-1, v k , w k )表示在上一时刻位姿为x k-1时加入输入v k 和噪声w k 后的状态量，且有：
Figure 698518DEST_PATH_IMAGE019
其中，(x,y)表示机器人在二维平面中的位置，θ表示机器人在二维地图中的转角，即偏航角。
观测方程为：
Figure 119135DEST_PATH_IMAGE020
上式中，y k 为时刻k的观测向量，x k 为时刻k的状态向量，u k 为观测方程中的零均值高斯噪声，g(x k， u k )表示在位姿为x k ，噪声为u k 时的观测。
预测方程为：
Figure 591704DEST_PATH_IMAGE021
上式中，P k 为时刻k的扩展卡尔曼滤波器的协方差，P k-1为时刻k-1的扩展卡尔曼滤波器的协方差，Q k 为时刻k的w的协方差，w为运动方程中的零均值高斯噪声，x k 为时刻k的状态向量，f(x k-1, v k , 0)表示在无运动噪声下的状态量，F k-1为时刻k-1的运动方程在位姿为x k-1时的导数，其函数表达式为：
Figure 642837DEST_PATH_IMAGE022
其中，x k-1为时刻k-1的状态向量，v k 为时刻k的输入向量，w k 为运动方程中的零均值高斯噪声；
卡尔曼增益为：
Figure 568068DEST_PATH_IMAGE023
上式中，K k 为时刻k的卡尔曼增益，P k 为时刻k的扩展卡尔曼滤波器的协方差，R k 为时刻k的u的协方差，u为观测方程中的零均值高斯噪声，G k 为时刻k的运动方程在位姿为x k 时的一阶导数，其函数表达式为：
Figure 792376DEST_PATH_IMAGE024
其中，x k 为时刻k的状态向量，u k 为时刻k的观测方程中的零均值高斯噪声。
更新方程为：
Figure 853872DEST_PATH_IMAGE025
上式中，P k 为时刻k的扩展卡尔曼滤波器的协方差，K k 为时刻k的卡尔曼增益，G k 为时刻k的观测方程的一阶导数，x k 为时刻k的状态向量，y k 为时刻k的观测向量，g(x k , 0)表示在无观测噪声下的观测量。
如图5所示，步骤4）包括：
4.1）计算视觉图像中与当前帧具有连接关系的所有关键帧的视觉词典得分，所述与当前帧具有连接关系是指与当前帧具有部分公共的特征点；
4.2）根据视觉词典得分，找出和当前帧相似的视觉闭环候选关键帧，并检测视觉闭环候选关键帧与当前帧的连续性，选出视觉闭环关键帧；
4.3）遍历视觉闭环关键帧，查找其与当前帧匹配的地图点；
4.4）计算当前帧的融合位姿T t f 和视觉闭环关键帧的融合位姿T k f 之间的旋转平移相似变换；
4.5）判断旋转平移相似变换在空间几何姿态上是否成立，若成立则判定视觉闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.6）对当前帧的位姿进行调整，并且传播到当前帧相连的关键帧，利用调整过的位姿更新这些相连关键帧对应的地图点；更新当前帧的连接关系，计算和当前帧连接的关键帧的旋转平移相似变换并校正位姿，更新这些关键帧的三维地图点坐标，并优化三维全局位姿图；
4.7）根据视觉闭环检测得到的结果，对当前时刻的激光雷达数据和视觉闭环关键帧对应时间的激光雷达数据进行闭环判断，若两帧激光雷达的位姿具有闭环相似性，则判定激光闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.8）将当前帧激光雷达数据加入视觉闭环关键帧对应时刻的激光子图中进行扫描匹配并计算其闭环约束，进行二维全局位姿优化，得到二维栅格地图和三维点云地图。
本实施例中，步骤4.7）中两帧激光雷达的位姿具有闭环相似性的函数表达式为：
Figure 138223DEST_PATH_IMAGE003
上式中，T t f 为当前时刻的融合位姿，T k f 为视觉闭环关键帧对应时间的融合位姿，P t i 为当前时刻的激光雷达点云坐标，P k i 为视觉闭环关键帧对应时间的激光雷达点云坐标，p为激光点云个数，f loop (T t f )表示两帧激光雷达的位姿是否具有闭环相似性。
若当前时刻与视觉闭关键帧对应时刻的位姿具有闭环相似性，则两个时刻位姿的转换位姿近似为单位矩阵，根据上述函数即可求解两个时刻的闭环相似性。
图6所示为视觉跟踪失败的建图效果，在视觉跟踪丢失并且视觉重定位失败的情况下，建图系统无法继续完成建图工作。图7所示为采用本实施例激光与视觉信息融合的鲁棒定位和建图方法进行非视觉位姿辅助重定位的建图效果，系统在视觉重定位失败的情况下，仍然可以使用非视觉重定位来估计当前视觉位姿，保证建图过程的连续性。图8为本发明与其他算法的轨迹对比图，其中，ORB_SLAM2曲线是使用ORB_SLAM2算法得到的机器人轨迹曲线，Ours曲线则是使用本专利方法得到的机器人轨迹曲线，Groundtruth曲线是机器人真值轨迹。可以看出本专利的方法不仅在定位精度上要优于ORB_SLAM2算法，而且也可以在ORB_SLAM2算法跟踪丢失时，继续完成建图工作。图9为本发明方法与其他算法的绝对轨迹误差指标对比图，可以看出本发明提出的方法在OpenLORIS-Scene数据集上的定位效果明显优于其他算法。因此，本专利提出的方法不仅充分利用视觉SLAM图像闭环检测的能力，来弥补激光雷达数据因数据稀疏而导致闭环能力不足的缺点，而且也提高了建图系统的鲁棒性和精确性。
综上所述，本实施例激光与视觉信息融合的鲁棒定位和建图方法通过融合激光雷达和IMU的数据，能减小IMU随时间增大时解算产生的累积误差，使激光雷达和IMU融合得到的机器人位姿更加精确。本实施例激光与视觉信息融合的鲁棒定位和建图方法对视觉位姿和非视觉位姿进行时序位姿融合，有效减小纯视觉特征匹配所产生的累积误差，同时在视觉跟踪定位失败时，提出以激光雷达的位姿来辅助重定位，有效提高重定位效率。
此外，本实施例还提供一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行前述的激光与视觉信息融合的鲁棒定位和建图方法的步骤。
此外，本实施例还提供一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，所述存储器中存储有被编程或配置以执行前述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
此外，本实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有被编程或配置以执行前述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可读存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，包括：
1）将惯性测量单元IMU、里程计及激光雷达的位姿融合得到机器人当前时刻t的非视觉位姿T t m ；
2）根据视觉图像中的当前帧与参考关键帧进行特征提取与匹配估计机器人当前时刻t的视觉位姿T t v ，若估计失败则使用非视觉位姿T t m 进行非视觉辅助重定位，最终获取机器人当前时刻t的视觉位姿T t v ；
3）对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，得到融合后的机器人位姿T t f ；
4）针对融合后的机器人位姿T t f 进行闭环检测，在检测到闭环后采用基于图优化的闭环优化算法对机器人位姿进行全局优化，从而得到二维栅格地图和三维点云地图；
步骤2）中使用非视觉位姿T t m 进行非视觉辅助重定位的步骤包括：
2.1）根据T t v =T t-1 v (T t-1 m ) -1 T t m 计算当前时刻t的视觉位姿T t v ，其中，T t m 为当前时刻t的非视觉位姿，T t-1 m 为上一时刻t-1的非视觉位姿，T t-1 v 为上一时刻t-1的视觉位姿；
2.2）根据当前时刻t的视觉位姿T t v 、当前时刻t的邻域ε内的视觉位姿T i v 构建目标函数并进行局部位姿图优化，最终得到最优的机器人当前时刻t的视觉位姿T t v 。
2.根据权利要求1所述的激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，步骤1）包括：
1.1）采用误差状态卡尔曼滤波器对惯性测量单元IMU的位姿进行积分得到标准状态，将里程计数据通过里程计运动模型对误差状态卡尔曼滤波器进行观测更新估计误差状态，利用误差状态修正标准状态，得到真实状态并重置误差状态；
1.2）将误差状态卡尔曼滤波器输出的真实状态作为激光雷达扫描匹配的初始位姿，将激光雷达的数据与当前的子图进行相关扫描匹配，所述子图是指由激光雷达数据构成的一个局部地图，如果扫描匹配成功，则估计出当前时刻t的激光雷达位姿T t l ；如果扫描匹配失败，则根据该帧激光雷达数据构建新的子图，那么上一个子图将会加入到全局地图中，所述全局地图为由所有的子图构成的地图，将已完成创建的全部子图进行全局位姿优化，确定当前时刻t的激光雷达位姿T t l ；
1.3）使用得到的激光雷达位姿T t l 对误差状态卡尔曼滤波器进行观测更新，得到更加精确的机器人当前时刻t的非视觉位姿T t m 。
3.根据权利要求1所述的激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，步骤2.2）中构建的目标函数的函数表达式为：
Figure 30178DEST_PATH_IMAGE001
上式中，T*表示目标优化位姿，ε为时间邻域，T={T t - ε v ,…,T t v }表示当前时刻t的ε邻域内的位姿序列，T t,i v =(T t v )-1 T i v ，T t,i v 表示时刻i和当前时刻t之间的相对位姿，T t v 为当前时刻t的视觉位姿、T i v 为当前时刻t的邻域ε内的视觉位姿, Cov t,i 为T t v 和T i v 之间的协方差矩阵，i表示在当前时刻t的邻域ε内的时刻，0<(t-i)<ε，
Figure 885002DEST_PATH_IMAGE002
表示从向量到矩阵的运算。
4.根据权利要求1所述的激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，步骤3）包括：
3.1）在当前时刻t=0时机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m 均置为0；在当前时刻t>0时则根据上一时刻t-1的视觉位姿T t-1 v 、非视觉位姿T t-1 m 计算机器人的非视觉传感器的速度V t v 和视觉传感器的速度V t m ；在任意当前时刻t，当视觉传感器的速度V t m 为0时，将上一帧的位姿作为当前帧的初始位姿，通过搜索视觉词典的方式在参考帧中找到与当前帧匹配的ORB特征点，优化每个特征点对应3D点重投影误差得到视觉图像中当前帧的视觉位姿T t v ；当视觉传感器的速度V t m 不为0时，根据恒速模型设定当前帧的初始位姿T t v =V t v T t-1 v ，通过投影的方式在参考帧中找到当前帧匹配的ORB特征点，优化每个特征点所对应的3D点重投影误差得到视觉图像中当前帧的视觉位姿T t v ；
3.2）采用扩展卡尔曼滤波器对机器人的视觉位姿T t v 和非视觉位姿T t m 进行融合，且以视觉位姿T t v 和非视觉位姿T t m 作为观测量，对扩展卡尔曼滤波器进行更新，得到融合后的机器人位姿T t f 。
5.根据权利要求1所述的激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，步骤4）包括：
4.1）计算视觉图像中与当前帧具有连接关系的所有关键帧的视觉词典得分，所述与当前帧具有连接关系是指与当前帧具有部分公共的特征点；
4.2）根据视觉词典得分，找出和当前帧相似的视觉闭环候选关键帧，并检测视觉闭环候选关键帧与当前帧的连续性，选出视觉闭环关键帧；
4.3）遍历视觉闭环关键帧，查找其与当前帧匹配的地图点；
4.4）计算当前帧的融合位姿T t f 和视觉闭环关键帧的融合位姿T k f 之间的旋转平移相似变换；
4.5）判断旋转平移相似变换在空间几何姿态上是否成立，若成立则判定视觉闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.6）对当前帧的位姿进行调整，并且传播到当前帧相连的关键帧，利用调整过的位姿更新这些相连关键帧对应的地图点；更新当前帧的连接关系，计算和当前帧连接的关键帧的旋转平移相似变换并校正位姿，更新这些关键帧的三维地图点坐标，并优化三维全局位姿图；
4.7）根据视觉闭环检测得到的结果，对当前时刻的激光雷达数据和视觉闭环关键帧对应时间的激光雷达数据进行闭环判断，若两帧激光雷达的位姿具有闭环相似性，则判定激光闭环成立，跳转执行下一步；否则，闭环检测失败，结束并退出；
4.8）将当前帧激光雷达数据加入视觉闭环关键帧对应时刻的激光子图中进行扫描匹配并计算其闭环约束，进行二维全局位姿优化，得到二维栅格地图和三维点云地图。
6.根据权利要求5所述的激光与视觉信息融合的鲁棒定位和建图方法，其特征在于，步骤4.7）中两帧激光雷达的位姿具有闭环相似性的函数表达式为：
Figure 664739DEST_PATH_IMAGE003
上式中，T t f 为当前时刻t的融合位姿，T k f 为视觉闭环关键帧对应时刻k的融合位姿，P t i 为当前时刻的激光雷达点云坐标，P k i 为视觉闭环关键帧对应时间的激光雷达点云坐标，p为激光点云个数，f loop (T t f )表示两帧激光雷达的位姿是否具有闭环相似性。
7.一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，其特征在于，所述微处理器被编程或配置以执行权利要求1～6中任意一项所述的激光与视觉信息融合的鲁棒定位和建图方法的步骤。
8.一种激光与视觉信息融合的鲁棒定位和建图系统，包括相互连接的微处理器和存储器，其特征在于，所述存储器中存储有被编程或配置以执行权利要求1～6中任意一项所述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
9.一种计算机可读存储介质，其特征在于，该计算机可读存储介质中存储有被编程或配置以执行权利要求1～6中任意一项所述的激光与视觉信息融合的鲁棒定位和建图方法的计算机程序。
CN202110415828.3A 2021-04-19 2021-04-19 激光与视觉信息融合的鲁棒定位和建图方法及系统 Active CN112985416B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110415828.3A CN112985416B (zh) 2021-04-19 2021-04-19 激光与视觉信息融合的鲁棒定位和建图方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110415828.3A CN112985416B (zh) 2021-04-19 2021-04-19 激光与视觉信息融合的鲁棒定位和建图方法及系统

Publications (2)

Publication Number Publication Date
CN112985416A CN112985416A (zh) 2021-06-18
CN112985416B true CN112985416B (zh) 2021-07-30

Family

ID=76340950

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110415828.3A Active CN112985416B (zh) 2021-04-19 2021-04-19 激光与视觉信息融合的鲁棒定位和建图方法及系统

Country Status (1)

Country Link
CN (1) CN112985416B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113252033B (zh) * 2021-06-29 2021-10-15 长沙海格北斗信息技术有限公司 基于多传感器融合的定位方法、定位系统及机器人
CN113777615B (zh) * 2021-07-19 2024-03-29 派特纳(上海)机器人科技有限公司 室内机器人的定位方法、系统及清洁机器人
CN114088104B (zh) * 2021-07-23 2023-09-29 武汉理工大学 一种自动驾驶场景下的地图生成方法
CN113532439B (zh) * 2021-07-26 2023-08-25 广东电网有限责任公司 输电线路巡检机器人同步定位与地图构建方法及装置
CN113587933B (zh) * 2021-07-29 2024-02-02 山东山速机器人科技有限公司 一种基于分支定界算法的室内移动机器人定位方法
CN113793379A (zh) * 2021-08-12 2021-12-14 视辰信息科技(上海)有限公司 相机姿态求解方法及系统、设备和计算机可读存储介质
CN113763548B (zh) * 2021-08-17 2024-02-27 同济大学 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN113778096B (zh) * 2021-09-15 2022-11-08 杭州景吾智能科技有限公司 室内机器人的定位与模型构建方法及系统
CN114001733B (zh) * 2021-10-28 2024-03-15 浙江大学 一种基于地图的一致性高效视觉惯性定位算法
CN114047766B (zh) * 2021-11-22 2023-11-21 上海交通大学 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN114199240B (zh) * 2022-02-18 2022-06-21 武汉理工大学 无gps信号下二维码、激光雷达与imu融合定位系统及方法
CN114820799B (zh) * 2022-06-24 2022-09-16 南开大学 一种用于行人位置估计的数据自动标注方法、装置及系统
CN115575973B (zh) * 2022-11-21 2023-04-28 南京理工大学 基于路标主动重观测的星表巡视器激光雷达定位方法
CN115797490A (zh) * 2022-12-19 2023-03-14 广州宸境科技有限公司 基于激光视觉融合的建图方法及系统
CN117451035B (zh) * 2023-12-25 2024-02-27 江苏中科重德智能科技有限公司 一种激光slam自适应自动更新地图的方法及系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
WO2017155970A1 (en) * 2016-03-11 2017-09-14 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN108303096A (zh) * 2018-02-12 2018-07-20 杭州蓝芯科技有限公司 一种视觉辅助激光定位系统及方法
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN109887057A (zh) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 生成高精度地图的方法和装置
CN110174107A (zh) * 2019-04-30 2019-08-27 厦门大学 一种导览机器人定位建图激光视觉融合方法和机器人
CN110428467A (zh) * 2019-07-30 2019-11-08 四川大学 一种相机、imu和激光雷达联合的机器人定位方法
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SG10201700299QA (en) * 2017-01-13 2018-08-30 Otsaw Digital Pte Ltd Three-dimensional mapping of an environment

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105371840A (zh) * 2015-10-30 2016-03-02 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
WO2017155970A1 (en) * 2016-03-11 2017-09-14 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN108303096A (zh) * 2018-02-12 2018-07-20 杭州蓝芯科技有限公司 一种视觉辅助激光定位系统及方法
CN109887057A (zh) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 生成高精度地图的方法和装置
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN110174107A (zh) * 2019-04-30 2019-08-27 厦门大学 一种导览机器人定位建图激光视觉融合方法和机器人
CN110428467A (zh) * 2019-07-30 2019-11-08 四川大学 一种相机、imu和激光雷达联合的机器人定位方法
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Robust High Accuracy Visual-Inertial-Laser SLAM System;Wang Zengyuan,et.al;《2019 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》;20191231;第6636-6641页 *
基于ESKF与图优化的轻量级多传感器融合SLAM系统;李文豪;《科学技术创新》;20210315(第8期);第15-18页 *
视觉与激光融合SLAM研究综述;王锦凯等;《辽宁工业大学学报(自然科学版)》;20201231;第40卷(第6期);第356-361页 *

Also Published As

Publication number Publication date
CN112985416A (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及系统
CN110782494A (zh) 一种基于点线融合的视觉slam方法
Royer et al. Localization in urban environments: monocular vision compared to a differential GPS sensor
Liang et al. Visual laser-SLAM in large-scale indoor environments
WO2019057179A1 (zh) 一种基于点线特征的视觉slam方法和装置
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
Parra et al. Robust visual odometry for vehicle localization in urban environments
Chien et al. Visual odometry driven online calibration for monocular lidar-camera systems
CN113706626B (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
DK2984625T3 (en) PROCEDURE FOR ESTIMATING THE ANGLE DIFFERENCE FOR A MOBILE ELEMENT IN TERMS OF A REFERENCE DIRECTION
Sujiwo et al. Monocular vision-based localization using ORB-SLAM with LIDAR-aided mapping in real-world robot challenge
Engelmann et al. SAMP: shape and motion priors for 4d vehicle reconstruction
CN113175929B (zh) 一种基于upf的空间非合作目标相对位姿估计方法
GB2599947A (en) Visual-inertial localisation in an existing map
JP6410231B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
Chiu et al. Precise vision-aided aerial navigation
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
Comport et al. Robust real-time visual tracking: Comparison, theoretical analysis and performance evaluation
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
US20230117498A1 (en) Visual-inertial localisation in an existing map
Zhang et al. A Static Feature Point Extraction Algorithm for Visual-Inertial SLAM
Li-Chee-Ming et al. Augmenting visp’s 3d model-based tracker with rgb-d slam for 3d pose estimation in indoor environments
Song et al. Scale Estimation with Dual Quadrics for Monocular Object SLAM
Yilmaz et al. AV-SLAM: Autonomous vehicle SLAM with gravity direction initialization
Aladem et al. Evaluation of a Stereo Visual Odometry Algorithm for Passenger Vehicle Navigation

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230719

Address after: 410001 No. 002, Floor 5, Building B, No. 10, Zone 2, CSCEC Smart Industrial Park, No. 50, Jinjiang Road, Yuelu Street, Yuelu District, Changsha, Hunan Province

Patentee after: Hunan Xinxin Xiangrong Intelligent Technology Co.,Ltd.

Address before: Yuelu District City, Hunan province 410082 Changsha Lushan Road No. 1

Patentee before: HUNAN University