CN114608554A - 一种手持slam设备以及机器人即时定位与建图方法 - Google Patents

一种手持slam设备以及机器人即时定位与建图方法 Download PDF

Info

Publication number
CN114608554A
CN114608554A CN202210160734.0A CN202210160734A CN114608554A CN 114608554 A CN114608554 A CN 114608554A CN 202210160734 A CN202210160734 A CN 202210160734A CN 114608554 A CN114608554 A CN 114608554A
Authority
CN
China
Prior art keywords
laser
inertia
camera
point
visual
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
CN202210160734.0A
Other languages
English (en)
Other versions
CN114608554B (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 CN202210160734.0A priority Critical patent/CN114608554B/zh
Publication of CN114608554A publication Critical patent/CN114608554A/zh
Application granted granted Critical
Publication of CN114608554B publication Critical patent/CN114608554B/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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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/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/1656—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 passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Computer Graphics (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种手持SLAM设备以及机器人即时定位与建图方法。本发明将相机、惯性测量器件以及激光雷达的优点相结合，激光‑惯性子***为视觉‑惯性子***提供深度信息，由此消除单目相机存在的尺度不确定性；而视觉‑惯性子***为激光‑惯性子***提供先验估计和回环检测信息，进而得到更加精确的位姿估计。两个***优势互补，大大提高了即时定位与建图***的精确性和鲁棒性。同时，设计的手持SLAM设备便于携带，并为定位的准确性奠定基础。

Description

一种手持SLAM设备以及机器人即时定位与建图方法
技术领域
本发明涉及机器人感知与导航技术领域，具体涉及一种手持SLAM设备以及机器人即时定位与建图方法。
背景技术
即时定位与建图(Simultaneous Localization and Mapping，SLAM)技术是当前在机器人自主导航与定位领域内较为热门的研究方向。即时定位与建图技术的主要特点在于，机器人并不依赖由外部测量得到的信息来确定自身所处的位置，而主要通过自身搭载的传感器来给出位置的增量，从而确定自身在环境中的位置，同时根据定位得到的结果和当前时刻传感器返回的数据建立起环境点云地图。该领域主要分为两个方向，即激光SLAM技术与视觉SLAM技术，激光SLAM技术优点是可以适应较为复杂的环境，但是对于环境特征的提取以及回环检测不敏感；视觉SLAM技术优点是容易捕捉环境特征点和检测重复环境，但是受光照、视场角等因素的影响导致其对环境的要求较高。专家学者们针对以上所列出的优点和缺点，提出不同的解决方案，针对激光SLAM技术和视觉SLAM技术，已有的解决方案有如下几种：
方案1：文献(Zhang J,Kaess M,Singh S.On degeneracy of optimization-based state estimation problems.[C]//2016 IEEE International Conferernce onRobotics and Automation(ICRA).IEEE,2016.)文献提出了LOAM算法。LOAM算法的贡献在于其提出了一种基于平面点和边缘点特征提取和点云匹配方法，并且通过假设匀速运动来消除由运动造成的点云畸变，以此来保证算法的实时性。但是LOAM算法并没有在后端对回环检测进行处理，并且在使用惯性测量器件时采用的是松耦合的处理方式。
方案2：文献(Tong,Qin,Peiliang,et al.VINS-Mono:A Robust and VersatileMonocular Visual-Inertial State Estimator[J]//IEEE Transactions on Robotics,2018,34(4):1004-20.)文献提出了VINS算法。VINS算法的贡献在于其提供了一种前端接受图像提取环境中的特征点，并对惯性测量器件得到的数据进行预处理，以及后端进行联合优化、全局图优化并进行回环检测的算法框架。但是在实际测试中，VINS算法对环境的要求较高，往往会出现初始化失败的现象，实现条件较为苛刻。
方案3：文献(Lin J,Zheng C,Xu W,et al.R2LIVE:A Robust,Real-time,LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping[J]//IEEE Roboticsand Automation Letters,2021,6(4):7469-76.)文献提出了R2LIVE算法。R2LIVE主要贡献在于其提出了一种激光雷达、相机、惯性测量器件三种传感器紧耦合的即时定位与建图框架，该方案以高频率惯性测量器件的测量结果作为先验估计，通过多线程分别对激光雷达和相机的测量结果用误差状态迭代卡尔曼滤波进行优化，具有较强的鲁棒性。但是该算法不适合处理大量数据，在点云数据量较大时，该算法的实时性将不能保证。
发明内容
有鉴于此，本发明提供了一种手持SLAM设备以及机器人即时定位与建图方法，能够实现激光-惯性子***和视觉-惯性子***的优势互补，大大提高了即时定位与建图***的精确性和鲁棒性。
本发明的机器人即时定位与建图方法，采用激光雷达、相机和惯性测量器件进行即时定位与建图，包括：
采用相机和惯性测量器件，基于视觉SLAM技术，对构建的状态变量进行最优估计，获得视觉-惯性里程计的姿态信息和视觉回环；其中，所述构建的状态变量包含惯性测量器件提供的姿态信息与测量偏差，以及由激光雷达提供的特征点的深度信息；
采用激光雷达和惯性测量器件，以视觉-惯性里程计的姿态信息和视觉回环作为先验信息，基于激光SLAM技术，获得激光-惯性里程计的姿态信息；基于激光-惯性里程计的姿态信息，建立环境的点云地图。
较优的，所述特征点采用KLT稀疏光流算法获得。
较优的，所述视觉SLAM技术中，采用DBoW2词袋模型进行回环检测。
较优的，所述步骤1中，所述构建的状态变量Y为：
Figure BDA0003514539940000031
Figure BDA0003514539940000032
Figure BDA0003514539940000033
其中，角标W表示世界坐标系，角标I表示惯性测量器件坐标系，角标C表示相机坐标系；
Figure BDA0003514539940000034
为惯性测量器件测量的姿态信息，
Figure BDA0003514539940000035
为惯性测量器件的偏差；n为关键帧的数量；
Figure BDA0003514539940000036
为相机到惯性测量器件的姿态变换；dl为第l个特征点的深度信息，由激光激光雷达给出，l∈[1,m],m为特征点的数量。
较优的，所述最优估计采用最大后验概率模型。
较优的，所述基于视觉SLAM技术中，基于最大后验概率模型的优化求解为：
Figure BDA0003514539940000041
其中，
Figure BDA0003514539940000042
为所有惯性测量器件测量值的集合；
Figure BDA0003514539940000043
为所有在当前滑动窗口下至少被观察到两次的特征点的集合；
Figure BDA0003514539940000044
为所有产生回环检测的关键帧集合；(l，j)表示在第j帧图像观测第l个特征点；(l,V)表示在回环时刻的相机坐标系V下观测第l个特征点；
||rp-HpY||2为图像边缘化对应的残差项，
Figure BDA0003514539940000045
和
Figure BDA0003514539940000046
分别为惯性测量器件残差项和视觉特征残差项；
Figure BDA0003514539940000047
Figure BDA0003514539940000048
Figure BDA0003514539940000049
Figure BDA0003514539940000051
其中，
Figure BDA0003514539940000052
为坐标系W到坐标系Ii的旋转矩阵，角标i为i时刻；Δti为两次惯性测量器件测量值之间的时间差，gW为重力加速度，[·]xyz提取四元数q的向量部分以表示误差状态，
Figure BDA0003514539940000053
为四元数乘法；
Figure BDA0003514539940000054
是在两个连续图像帧之间的时间间隔内仅使用噪声加速计和陀螺仪测量的预集成IMU测量项；
Figure BDA0003514539940000055
是在i帧图像中第一次观察到的第l个特征点所对应的观测量；
Figure BDA0003514539940000056
是同样的特征在第j帧图像观察得到的观测量；
Figure BDA0003514539940000057
是一种反投影函数，它使用相机的固有参数将像素位置转换为单位向量；λl是首次观测到的第l个特征点的反深度；
Figure BDA0003514539940000058
是残差向量对应的切平面；bi,b2是切平面
Figure BDA0003514539940000059
两个任意选择的正交基；
Figure BDA00035145399400000510
是切平面中具有固定长度的标准协方差；
函数ρ(·)表示为：
Figure BDA00035145399400000511
较优的，激光-惯性里程计的姿态信息的获取方法为：
基于惯性测量器件测量的姿态信息，对激光云图进行去畸变处理；
将去畸变处理后的激光云图中的点云数据进行特征分类，分为平面点和边缘点；分别计算平面点和边缘点的特征残差；
以视觉-惯性里程计的姿态信息和视觉回环作为先验信息，对平面点和边缘点的特征残差和进行优化，获取最优激光-惯性里程计的姿态信息。
较优的，所述对平面点和边缘点的特征残差和进行优化中，当视觉-惯性里程计的视觉回环和激光-惯性里程计的激光回环一致时，认为是一个可靠的回环，否则，不是一个可靠的回环。
较优的，所述激光-惯性里程计的激光回环基于ICP算法计算得到。
较优的，所述点云数据进行特征分类时，首先对每一线激光雷达上的点云数据，按照如下的曲率公式计算平滑度：
Figure BDA0003514539940000061
其中，
Figure BDA0003514539940000062
为激光坐标系L下第k次扫描的第i个约束点，s表示该次扫描内i点邻域内所有点构成的集合；平滑度最大的点提取为边缘点，平滑度最小的点提取为平面点。
较优的，平面点的特征残差为：
Figure BDA0003514539940000063
边缘点的特征残差为：
Figure BDA0003514539940000064
本发明还提供了一种手持SLAM设备，包括：激光雷达、惯性测量器件、相机、机载电脑，移动电源，显示屏幕、支架和手柄；其中，所述支架为上下双层结构，且上窄下宽；所述激光雷达固定在支架上层；相机固定在支架下层，且朝向前方；惯性测量器件固定在支架下层，且位于在激光雷达正下方并和相机处于同一水平线；机载电脑和移动电源位于支架下层，且分别位于惯性测量器件的左右两侧；显示屏幕固定在支架后侧；机载电脑与相机、惯性测量器件、激光雷达和显示屏幕连接；惯性测量器件、显示屏幕和相机由机载电脑供电。
较优的，还包括分电板和恒压模块；所述分电板将移动电源输出电流分为两路，一路与激光雷达连接，另一路通过恒压模块与机载电脑连接。
较优的，所述手柄可拆卸。
较优的，机载电脑基于ROS的时间戳实现相机、惯性测量器件、激光雷达的同步。
较优的，所述机载电脑采用上述方法进行即时定位与建图。
有益效果：
本发明将相机、惯性测量器件以及激光雷达的优点相结合，提出一种基于因子图优化的激光雷达、单目相机和惯性导航器件融合的即时定位与建图软件框架，激光雷达为相机视觉提供深度信息和初始化信息，而相机视觉为激光雷达提供特征点与回环检测，后端通过因子图优化实现位姿估计。
与此同时，考虑到测试设备的准确性与便携性问题，本发明设计并完成了一套手持SLAM设备。通过软件和硬件的结合，构成一整套完整的由手持SLAM硬件设备及其搭载的多传感器融合的软件框架组成的***，该***可以在复杂环境下保证测量的准确性和鲁棒性。
附图说明
图1为本发明手持设备电路结构图。
图2为本发明手持设备硬件设计图。
图3为本发明多传感器融合算法框架。
图4为本发明视觉-惯性子***因子图模型。
图5为本发明激光-惯性子***因子图模型。
图6为本发明即时定位与建图***测试效果图。
其中，1-激光雷达，2-惯性测量器件，3-单目相机，4-机载电脑，5-移动电源，6-显示屏幕。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明设计并搭建了一套手持SLAM硬件设备，并且提出了一种基于因子图优化的多传感器融合的即时定位与建图方法，从而构成了一个完整的手持即时定位与建图***，解决了硬件设备的便携性问题以及软件框架的鲁棒性问题。
其中，硬件部分设备采用32线机械激光雷达、单目相机和惯性导航单元(InertialMeasurement Unit，IMU)，通过机载电脑实现嵌入式开发，并配备相应的显示屏和外设接口。本发明的硬件设备采用SoildWorks进行建模，并设计硬件设备传感器的摆放位置，根据不同传感器的供电需求设计电路以保证在实际运行的过程中设备的各个模块正常工作。最后考虑到各个传感器由于摆放位置以及惯用坐标系的不同而导致坐标系不统一的问题，本发明通过对传感器进行标定以解决该问题。
软件部分主要分为激光-惯性子***(Lidar-IMU subsystem，LIS)和视觉-惯性子***(Visual-IMU subsystem，VIS)，激光-惯性子***为视觉-惯性子***提供深度信息，而视觉-惯性子***为激光-惯性子***提供环境中的特征点先验信息以及回环检测信息，两个子***分为两个线程独立工作以保证算法的实时性和鲁棒性，通过融合优化以保证***的准确性。由于激光-惯性子***和视觉-惯性子***分别存在对应的累计误差，本发明提出一种基于因子图优化的软件框架将多传感器的数据进行融合。本发明分别针对激光-惯性子***和视觉-惯性子***建立各自的因子图模型，并根据对应的约束项，求解对应的最大后验概率(maximum a posteriori，MAP)问题，得到更为精确的姿态估计。
下面就上述两个部分分别进行详细说明，如下：
一、手持设备的硬件设计
手持设备的电路结构图如图1所示，该设备采用6S锂电池进行供电。由于激光雷达的功耗较大，因此采用分电板设计将激光雷达设置成单独一路进行供电，这样可以避免恒压模块负载过大；另一路通过恒压模块对机载电脑进行供电，机载电脑通过USB接口与单目相机、惯性测量器件和显示屏幕连接及供电。
手持设备的硬件设计图如图2所示，针对手持设备的硬件设计，其所面临的难点主要是传感器位置摆放的合理性以及设备自身的便携性与平衡性。针对以上难点，本发明在手持设备的设计上着重体现以下几点：
1.激光雷达测量范围为水平360°，竖直45°，为确保激光雷达不受遮挡，设计了双层硬件安装方式，其中激光雷达独立放置于支架上层，保证水平方向测量不受遮挡，同时，支架上层平面小、下层平面宽的设计保证了激光雷达竖直方向测量不受遮挡。
2.双层支架设计方式保证激光雷达、单目相机以及惯性测量器件的Z轴方向相同并且尽量靠近，激光雷达位于设备正上方，单目相机位于支架下层并朝向前方，惯性测量器件在激光雷达正下方且与相机处于同一水平线，保证了后续软件标定工作的准确性；
3.为了保持设备的重心尽量保持在设备中间，将机载电脑和锂电池分别放在设备的两侧，以保持设备自身的平衡性；
4.显示屏幕位于设备的后侧便于进行人机交互，并配备可拆卸手柄，在需要时可将设备移植到移动机器人平台，操作较为简单方便，由此来保证手持设备操作的便携性以及多平台的可移植性。
特别要说明的是，由于本设备的激光雷达、单目相机和惯性测量器件自身的摆放位置和惯用坐标系不同，因此需要对传感器进行标定以实现坐标系间的相互转换。与此同时，使用基于ROS的时间戳来实现软件时间同步。
二、即时定位与建图框架的软件设计
本发明设计了一套多传感器融合的即时定位与建图方法，下面将从总体算法框架设计、激光-惯性子***设计和视觉-惯性子***设计三个方面进行详细阐述。
(1)总体算法框架设计
本发明的总体算法框架设计如图3所示，该框架是紧耦合的激光、视觉和惯性里程计，以两个独立的子***，即激光-惯性子***和视觉-惯性子***，分别工作。
针对于激光-惯性子***，首先通过惯性测量器件给出的测量数据对激光得到的点云数据进行去畸变处理。在做激光点云特征提取时，根据待分类的特征点相对于其邻域内其他点的平滑度进行特征分类，并根据特征点的类型建立相应残差项；同时，接受来自视觉-惯性子***的姿态信息作为先验信息进行优化。在激光里程计部分，接受来自视觉-惯性子***的回环检测信息，有利于更好地进行姿态估计和地图构建工作。
针对于视觉-惯性子***，在做特征提取时，接受来自激光-惯性子***给出的深度信息以便更好地进行匹配。
以上是***的总体性介绍，为了便于后续分模块化对两个子***进行更加详细地论述，本实施例先对待求解最大后验概率问题进行数学表示，并对整个***的状态变量进行定义。首先，定义最大后验概率问题形式如下：在存在观测量Z＝{zi|i＝1,2,…,k}和状态量X＝{xi|i＝1,2,…,k}时，求解最优的状态量X*使得概率P(X|Z)最大，用公式表示为：
Figure BDA0003514539940000101
由贝叶斯公式，可以得到：
Figure BDA0003514539940000111
而对于即时定位与建图算法而言，其观测量的概率分布P(Z)由传感器自身参数决定，与算法无关，可以看作是归一化参数，于是最大后验概率问题可以表示为：
Figure BDA0003514539940000112
因此该问题转化为：构建相应的优化指标，求解出最优状态估计X*。基于以上所定义的最大后验概率问题和状态变量，本发明分别对视觉-惯性子***和激光-惯性子***进行详细阐述。
(2)视觉-惯性子***
视觉-惯性子***的因子图模型如图4所示，其主要流程总共分为两步：视觉特征的提取和追踪和视觉-惯性的紧耦合优化。
对于视觉特征提取和追踪，首先使用KLT稀疏光流算法提取帧图像特征点，并得到相邻两帧之间的旋转和平移变量；通过判断两帧图像之间的视差和跟踪质量，将测量得到的结果用于对视觉关键帧的提取。
对于视觉-惯性的紧耦合优化，其主要的算法流程如下：
(a).优化指标设计
针对视觉-惯性子***的特点，将优化指标分成三个部分，分别是：视觉特征点优化指标rC、惯性测量器件优化指标rI和回环检测优化指标rS。将世界坐标系用W表示，惯性测量器件坐标系用I表示，单目相机坐标系用C表示。
为了实现联合优化，需要将状态变量进行拓展以实现紧耦合，在一个滑动窗口下，定义新的状态变量Y如下：
Figure BDA0003514539940000113
Figure BDA0003514539940000121
Figure BDA0003514539940000122
其中，p,v,q为惯性测量器件测量的姿态信息：p为位移，v为速度，q为用四元素表示的旋转；b表示惯性测量器件的偏差：ba为加速度计偏差，bw为陀螺仪偏差；i表示相机采集的第i张图像；yi表示设备在i时刻的状态；n表示关键帧的数量；m表示特征点的数量；dl表示第l个特征点的深度信息，由激光-惯性子***给出，以减小单目相机的尺度不确定性。
(b).惯性测量器件优化指标
考虑在一个滑动窗口下，连续两帧Ii和Ii+1之间的惯性测量器件的测量值，并由此得到惯性测量器件预积分的残差项
Figure BDA0003514539940000123
其中，[·]xyz提取四元数q的向量部分以表示误差状态。
Figure BDA0003514539940000124
是四元数的三维误差状态表示。
Figure BDA0003514539940000125
是在两个连续图像帧之间的时间间隔内仅使用噪声加速计和陀螺仪测量的预集成IMU测量项。加速计和陀螺仪偏差也包含在在线校正的剩余项中。
(c).视觉特征优化指标
与传统针孔相机模型不同，传统针孔相机模型定义了广义图像平面上的重投影误差，本发明定义了单位球面上的相机测量残差。考虑第i帧图像中第一次观察到的第l个特征点，那么在第j帧图像中的观察到该特征的残差定义为
Figure BDA0003514539940000131
Figure BDA0003514539940000132
Figure BDA0003514539940000133
其中，
Figure BDA0003514539940000134
是在i帧图像中第一次观察到的第l个特征点所对应的观测量；
Figure BDA0003514539940000135
是同样的特征在第j帧图像观察得到的观测量；
Figure BDA0003514539940000136
是一种反投影函数，它使用相机的固有参数将像素位置转换为单位向量。由于视觉残差的自由度为2，将残差向量投影到切平面
Figure BDA0003514539940000137
上；b1,b2是切平面
Figure BDA0003514539940000138
两个任意选择的正交基；
Figure BDA0003514539940000139
是切平面中具有固定长度的标准协方差。
(d).回环检测优化指标
假设当相机检测到重复环境时进行重定位，将回环时刻的相机坐标系表示为V。在坐标系V的基础之上套用和建立视觉特征一样的方法建立回环检测优化指标，具体表示为：
Figure BDA00035145399400001310
其中，
Figure BDA00035145399400001311
从上一时刻的里程计输出得到的位姿，并将其视为常量。
最后，为针对单目相机尺度不确定性问题，建立边缘化信息优化指标||rp-HpY||2作为最终优化指标的其中一项。根据以上推导，最终建立起视觉-惯性子***的最大后验概率问题求解如下：
Figure BDA0003514539940000141
其中，
Figure BDA0003514539940000142
和
Figure BDA0003514539940000143
分别为惯性测量器件残差项和视觉特征残差项；
Figure BDA0003514539940000144
是所有惯性测量器件测量值的集合；
Figure BDA0003514539940000145
是所有在当前滑动窗口下至少被观察到两次的特征点的集合；
Figure BDA0003514539940000146
为所有产生回环检测的关键帧集合；(l,j)代表在第j帧图像观测第l个特征点；(l,V)代表着在坐标系V下观测第l个特征点；函数ρ(·)表示为
Figure BDA0003514539940000147
可以使用C++的Ceres库中的高斯-牛顿迭代法求出该非线性优化问题的最优解，该解即为当前滑动窗口下视觉-惯性里程计得到的姿态估计的最优解。
(3)激光-惯性子***
激光-惯性子***的因子图模型如图5所示，该***首先基于惯性测量器件对激光采集到的点云数据进行去畸变处理，然后对去畸变处理后的点云数据进行特征提取，找到关联点并构建残差优化。其主要算法流程如下：
(a).点云特征提取
本发明参考LOAM算法进行特征点提取。具体的，本发明采用OUSTER的32线激光雷达，对每一线按照如下的曲率公式计算平滑度，由此提取出点云数据中的平面点和边缘点：
Figure BDA0003514539940000148
其中，
Figure BDA0003514539940000149
为激光坐标系L下第k次扫描的第i个约束点，s表示该次扫描内i点邻域内所有点构成的集合。其中，平滑度最大的点提取作为边缘点集合εk，平滑度最小的点提取为平面点集Hk。本次扫描中所有的边缘点组成的集合为ε，所有的平面点组成的集合为H。
(b).特征残差计算
边缘点残差：对于当前帧中的某点i∈εk+1，找到最近点j∈εk，找到j点相邻扫描线最近点l∈εk，则(j,l)为其关联的边缘线，使用点到直线的距离可以表示特征残差如下：
Figure BDA0003514539940000151
平面点残差：对于当前帧中的某点i∈Hk+1，同样找到最近点j∈Hk，找到与点j相同扫描线的最近点l找到j∈Hk，再寻找点j相邻线束最近点m∈Hk，则(j,l,m)为其关联的平面，使用点到平面的距离可以表示特征残差如下：
Figure BDA0003514539940000152
(c).优化指标构建
针对于激光雷达，关注的是其自身的位姿估计，因此针对于最大后验概率问题，选取T＝[R p]作为状态变量，假设第i次扫描对应的状态变量为Ti＝[Ri pi]，由此构建出优化问题如下：
Figure BDA0003514539940000153
由于该优化问题带有SO(3)旋转，非线性程度较强，可能存在局部最优解，求解的先验精度对于解的质量由很大影响。因此需要更加准确的先验，本发明从视觉-惯性里程计得到的结果作为该优化问题的先验估计，使得求解优化问题的时间复杂度降低，得到的结果更为精确。在视觉-惯性里程计部分，得到了最优状态估计Y*，当激光-惯性子***进行姿态优化时，则提取最近时刻状态变量作为先验估计
Figure BDA0003514539940000161
如下：
Figure BDA0003514539940000162
其中，先验估计
Figure BDA0003514539940000163
由视觉-惯性子***得到的最优状态变量估计Y*中的
Figure BDA0003514539940000164
给出，
Figure BDA0003514539940000165
为四元数
Figure BDA0003514539940000166
对应的旋转矩阵；
Figure BDA0003514539940000167
为平移量
Figure BDA0003514539940000168
在视觉-惯性子***的得到的粗里程计在经过激光-惯性子***的匹配之后，得到更为精确的位姿估计，并根据该结果将去畸变后的点云数据拼接到点云地图上，由此来建立环境的点云地图。
针对大范围建图定位问题，需要回环检测来保证算法建立点云地图的全局一致性，由于错误回环会严重影响建图效果，因此采用了两阶段回环检测方法保证回环检测的有效性。首先，基于视觉-惯性子***，采用DBoW2词袋模型进行回环检测，当检测到tn时刻和tm的图像满足回环约束时，会基于定位结果和约束关系得到两图像帧对应的坐标变换
Figure BDA0003514539940000169
其中上标c表示相机；其次，基于激光-惯性子***，寻找tn和tm时刻的激光点云，基于ICP算法计算出点云对应的坐标变换
Figure BDA00035145399400001610
其中上标L表示激光。如果两个坐标变换
Figure BDA00035145399400001611
和
Figure BDA00035145399400001612
基本一致，则认为tn和tm时刻为一个可靠回环，否则，认为不是一个可靠回环。
在回环优化过程中，为了降低计算开销，本发明不再对特征点优化，仅对位姿进行优化，构成位姿图模型。根据历史定位信息，在tm到tn中共有n-m+1个定位信息Ti＝[Ri pi],{i＝m,m+1,...,n}，通过定位信息可以计算得到相邻时刻的位姿增量
Figure BDA00035145399400001613
将激光得到的回环约束
Figure BDA00035145399400001614
加入位姿图优化模型中，可以构建出位姿图优化问题：
Figure BDA00035145399400001615
通过求解该优化问题，可以得到回环时刻中所有位姿的最优解。
最后，本发明使用该即时定位与建图方法对复杂环境进行测试，得到的结果如图6所示。可以看出，本发明即时定位与建图方法取得了较为可观的效果。总的来说，激光-惯性子***为视觉-惯性子***提供深度信息，由此消除单目相机存在的尺度不确定性；而视觉-惯性子***为激光-惯性子***提供先验估计和回环检测信息，进而得到更加精确的位姿估计。两个***优势互补，大大提高了即时定位与建图***的精确性和鲁棒性。并且相比于文献[3]中的方法，本发明通过两个独立的子***分别在不同的计算机进程中并行工作，而不是在同一个框架中进行数据更新，在提高特征提取的效率的同时降低求解优化问题复杂度，这样就保证了同时处理视觉和激光两种数据的实时性和高效性，达到了针对于大量数据存在时***的实时性要求。
需要说明的是，本发明即时定位与建图方法并不限于本发明设计的硬件设备，只需要机器人同时配备激光雷达、相机和惯性测量器件即可，激光雷达、相机和惯性测量器件的位置偏差可以通过坐标系标定的方式予以消除。而本发明设计的手持SLAM设备硬件，便于携带，并为定位的准确性奠定基础，其采用的软件部分可以不限于本发明设计的即时定位与建图方法。当然，同时采用本发明提供的硬件设备和软件框架，构成完整的手持即时定位与建图***，能够同时解决传统SLAM设备的准确性和便携性问题，设备的操作和运行相比于传统SLAM设备而言有了明显的提升。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (16)

1.一种机器人即时定位与建图方法，采用激光雷达、相机和惯性测量器件进行即时定位与建图，其特征在于，包括：
采用相机和惯性测量器件，基于视觉SLAM技术，对构建的状态变量进行最优估计，获得视觉-惯性里程计的姿态信息和视觉回环；其中，所述构建的状态变量包含惯性测量器件提供的姿态信息与测量偏差，以及由激光雷达提供的特征点的深度信息；
采用激光雷达和惯性测量器件，以视觉-惯性里程计的姿态信息和视觉回环作为先验信息，基于激光SLAM技术，获得激光-惯性里程计的姿态信息；基于激光-惯性里程计的姿态信息，建立环境的点云地图。
2.如权利要求1所述的机器人即时定位与建图方法，其特征在于，所述特征点采用KLT稀疏光流算法获得。
3.如权利要求1所述的机器人即时定位与建图方法，其特征在于，所述视觉SLAM技术中，采用DBoW2词袋模型进行回环检测。
4.如权利要求1或2或3所述的机器人即时定位与建图方法，其特征在于，所述步骤1中，所述构建的状态变量Y为：
Figure FDA0003514539930000011
Figure FDA0003514539930000012
Figure FDA0003514539930000013
其中，角标W表示世界坐标系，角标I表示惯性测量器件坐标系，角标C表示相机坐标系；
Figure FDA0003514539930000014
为惯性测量器件测量的姿态信息，
Figure FDA0003514539930000015
为惯性测量器件的偏差；n为关键帧的数量；
Figure FDA0003514539930000016
为相机到惯性测量器件的姿态变换；dl为第l个特征点的深度信息，由激光激光雷达给出，l∈[1，m]，m为特征点的数量。
5.如权利要求4所述的机器人即时定位与建图方法，其特征在于，所述最优估计采用最大后验概率模型。
6.如权利要求5所述的机器人即时定位与建图方法，其特征在于，所述基于视觉SLAM技术中，基于最大后验概率模型的优化求解为：
Figure FDA0003514539930000021
其中，
Figure FDA0003514539930000025
为所有惯性测量器件测量值的集合；
Figure FDA0003514539930000026
为所有在当前滑动窗口下至少被观察到两次的特征点的集合；ν为所有产生回环检测的关键帧集合；(l，j)表示在第j帧图像观测第l个特征点；(l，V)表示在回环时刻的相机坐标系V下观测第l个特征点；
||rp-HpY||2为图像边缘化对应的残差项，
Figure FDA0003514539930000022
和
Figure FDA0003514539930000023
分别为惯性测量器件残差项和视觉特征残差项；
Figure FDA0003514539930000024
Figure FDA0003514539930000031
Figure FDA0003514539930000032
Figure FDA0003514539930000033
其中，
Figure FDA0003514539930000034
为坐标系W到坐标系Ii的旋转矩阵，角标i为i时刻；Δti为两次惯性测量器件测量值之间的时间差，gW为重力加速度，[·]xyz提取四元数q的向量部分以表示误差状态，
Figure FDA0003514539930000035
为四元数乘法；
Figure FDA0003514539930000036
是在两个连续图像帧之间的时间间隔内仅使用噪声加速计和陀螺仪测量的预集成IMU测量项；
Figure FDA0003514539930000037
是在i帧图像中第一次观察到的第l个特征点所对应的观测量；
Figure FDA0003514539930000038
是同样的特征在第j帧图像观察得到的观测量；
Figure FDA0003514539930000039
是一种反投影函数，它使用相机的固有参数将像素位置转换为单位向量；λl是首次观测到的第l个特征点的反深度；
Figure FDA00035145399300000310
是残差向量对应的切平面；b1，b2是切平面
Figure FDA00035145399300000311
两个任意选择的正交基；
Figure FDA00035145399300000312
是切平面中具有固定长度的标准协方差；
函数ρ(·)表示为：
Figure FDA00035145399300000313
7.如权利要求1所述的机器人即时定位与建图方法，其特征在于，激光-惯性里程计的姿态信息的获取方法为：
基于惯性测量器件测量的姿态信息，对激光云图进行去畸变处理；
将去畸变处理后的激光云图中的点云数据进行特征分类，分为平面点和边缘点；分别计算平面点和边缘点的特征残差；
以视觉-惯性里程计的姿态信息和视觉回环作为先验信息，对平面点和边缘点的特征残差和进行优化，获取最优激光-惯性里程计的姿态信息。
8.如权利要求1或7所述的机器人即时定位与建图方法，其特征在于，所述对平面点和边缘点的特征残差和进行优化中，当视觉-惯性里程计的视觉回环和激光-惯性里程计的激光回环一致时，认为是一个可靠的回环，否则，不是一个可靠的回环。
9.如权利要求1或7所述的机器人即时定位与建图方法，其特征在于，所述激光-惯性里程计的激光回环基于ICP算法计算得到。
10.如权利要求7所述的机器人即时定位与建图方法，其特征在于，所述点云数据进行特征分类时，首先对每一线激光雷达上的点云数据，按照如下的曲率公式计算平滑度：
Figure FDA0003514539930000041
其中，
Figure FDA0003514539930000042
为激光坐标系L下第k次扫描的第i个约束点，s表示该次扫描内i点邻域内所有点构成的集合；平滑度最大的点提取为边缘点，平滑度最小的点提取为平面点。
11.如权利要求10所述的机器人即时定位与建图方法，其特征在于，平面点的特征残差为：
Figure FDA0003514539930000051
边缘点的特征残差为：
Figure FDA0003514539930000052
12.一种手持SLAM设备，其特征在于，包括：激光雷达(1)、惯性测量器件(2)、相机(3)、机载电脑(4)，移动电源(5)，显示屏幕(6)、支架和手柄；其中，所述支架为上下双层结构，且上窄下宽；所述激光雷达(1)固定在支架上层；相机(3)固定在支架下层，且朝向前方；惯性测量器件(2)固定在支架下层，且位于在激光雷达正下方并和相机(3)处于同一水平线；机载电脑(4)和移动电源(5)位于支架下层，且分别位于惯性测量器件(2)的左右两侧；显示屏幕(6)固定在支架后侧；机载电脑(4)与相机(3)、惯性测量器件(2)、激光雷达(1)和显示屏幕(6)连接；惯性测量器件(2)、显示屏幕(6)和相机(3)由机载电脑(4)供电。
13.如权利要求12所述的手持SLAM设备，其特征在于，还包括分电板和恒压模块；所述分电板将移动电源(5)输出电流分为两路，一路与激光雷达(1)连接，另一路通过恒压模块与机载电脑(4)连接。
14.如权利要求12所述的手持SLAM设备，其特征在于，所述手柄可拆卸。
15.如权利要求12所述的手持SLAM设备，其特征在于，机载电脑(4)基于ROS的时间戳实现相机(3)、惯性测量器件(2)、激光雷达(1)的同步。
16.如权利要求12～15任一所述的手持SLAM设备，其特征在于，所述机载电脑(4)采用如权利要求1～11任一所述的方法进行即时定位与建图。
CN202210160734.0A 2022-02-22 2022-02-22 一种手持slam设备以及机器人即时定位与建图方法 Active CN114608554B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210160734.0A CN114608554B (zh) 2022-02-22 2022-02-22 一种手持slam设备以及机器人即时定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210160734.0A CN114608554B (zh) 2022-02-22 2022-02-22 一种手持slam设备以及机器人即时定位与建图方法

Publications (2)

Publication Number Publication Date
CN114608554A true CN114608554A (zh) 2022-06-10
CN114608554B CN114608554B (zh) 2024-05-03

Family

ID=81859728

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210160734.0A Active CN114608554B (zh) 2022-02-22 2022-02-22 一种手持slam设备以及机器人即时定位与建图方法

Country Status (1)

Country Link
CN (1) CN114608554B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115290084A (zh) * 2022-08-04 2022-11-04 中国人民解放军国防科技大学 基于弱尺度监督的视觉惯性组合定位方法和装置
CN116184430A (zh) * 2023-02-21 2023-05-30 合肥泰瑞数创科技有限公司 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
TWI822423B (zh) * 2022-07-22 2023-11-11 杜宇威 運算裝置及模型產生方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107179086A (zh) * 2017-05-24 2017-09-19 北京数字绿土科技有限公司 一种基于激光雷达的制图方法、装置及***
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN108827306A (zh) * 2018-05-31 2018-11-16 北京林业大学 一种基于多传感器融合的无人机slam导航方法及***
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及***
CN111932674A (zh) * 2020-06-30 2020-11-13 博雅工道（北京）机器人科技有限公司 一种线激光视觉惯性***的优化方法
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及***
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及***
WO2021249387A1 (zh) * 2020-06-08 2021-12-16 杭州海康机器人技术有限公司 一种视觉地图构建方法和移动机器人

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107179086A (zh) * 2017-05-24 2017-09-19 北京数字绿土科技有限公司 一种基于激光雷达的制图方法、装置及***
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN108827306A (zh) * 2018-05-31 2018-11-16 北京林业大学 一种基于多传感器融合的无人机slam导航方法及***
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及***
WO2021249387A1 (zh) * 2020-06-08 2021-12-16 杭州海康机器人技术有限公司 一种视觉地图构建方法和移动机器人
CN111932674A (zh) * 2020-06-30 2020-11-13 博雅工道（北京）机器人科技有限公司 一种线激光视觉惯性***的优化方法
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及***
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
卫文乐 等: "利用惯导测量单元确定关键帧的实时SLAM算法", 计算机应用, vol. 40, no. 4 *
张伟伟 等: "融合激光与视觉点云信息的定位与建图方法", 计算机应用与软件, vol. 37, no. 7 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TWI822423B (zh) * 2022-07-22 2023-11-11 杜宇威 運算裝置及模型產生方法
CN115290084A (zh) * 2022-08-04 2022-11-04 中国人民解放军国防科技大学 基于弱尺度监督的视觉惯性组合定位方法和装置
CN115290084B (zh) * 2022-08-04 2024-04-19 中国人民解放军国防科技大学 基于弱尺度监督的视觉惯性组合定位方法和装置
CN116184430A (zh) * 2023-02-21 2023-05-30 合肥泰瑞数创科技有限公司 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
CN116184430B (zh) * 2023-02-21 2023-09-29 合肥泰瑞数创科技有限公司 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法

Also Published As

Publication number Publication date
CN114608554B (zh) 2024-05-03

Similar Documents

Publication Publication Date Title
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
US8761439B1 (en) Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
US9355453B2 (en) Three-dimensional measurement apparatus, model generation apparatus, processing method thereof, and non-transitory computer-readable storage medium
US20160260250A1 (en) Method and system for 3d capture based on structure from motion with pose detection tool
CN113052908B (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111707261A (zh) 一种微型无人机高速感知和定位方法
WO2015134795A2 (en) Method and system for 3d capture based on structure from motion with pose detection tool
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN110675455A (zh) 一种基于自然场景的车身环视相机自标定方法和***
CN111307146A (zh) 一种基于双目相机和imu的虚拟现实头戴显示设备定位***
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN112580683A (zh) 一种基于互相关的多传感器数据时间对齐***及其方法
Hinzmann et al. Flexible stereo: constrained, non-rigid, wide-baseline stereo vision for fixed-wing aerial platforms
CN116772844A (zh) 一种基于动态环境下的视觉惯性室内机器人的导航方法
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算***
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法

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