CN116295412A - 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 - Google Patents

一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 Download PDF

Info

Publication number
CN116295412A
CN116295412A CN202310188359.5A CN202310188359A CN116295412A CN 116295412 A CN116295412 A CN 116295412A CN 202310188359 A CN202310188359 A CN 202310188359A CN 116295412 A CN116295412 A CN 116295412A
Authority
CN
China
Prior art keywords
map
camera
mobile robot
dense
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.)
Pending
Application number
CN202310188359.5A
Other languages
English (en)
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202310188359.5A priority Critical patent/CN116295412A/zh
Publication of CN116295412A publication Critical patent/CN116295412A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • 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
    • 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/10016—Video; Image sequence
    • 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/10024—Color image
    • 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
    • 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/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • 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/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于深度相机的室内移动机器人稠密建图与自主导航方法，属于机器人同时定位与建图、机器人导航领域。本发明使用深度相机并基于对图像ORB特征的帧间匹配来实现对机器人进行实时定位，通过融合彩色图像与深度图像，并为了剔除冗余的视频帧引入了空间域上的关键帧提取方法，实现了实时的稠密三维点云地图构建，并将其转化成适用于导航的八叉树地图格式以及栅格地图格式，之后通过ROS Navigation功能包将其与现有移动机器人导航方法结合，实现基于纯视觉方案的室内移动机器人自主导航方案。

Description

一种基于深度相机的室内移动机器人稠密建图与自主导航一 体化方法
技术领域
本发明属于机器人同时定位与建图、机器人导航领域，具体涉及一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法。
背景技术
同步定位与地图创建(Simultaneous Localization and Mapping,SLAM)是指移动机器人在没有环境先验信息的情况下，通过自身搭载的传感器，于运动过程中建立所处环境的地图模型，同时估计自身的运动。SLAM同时包含定位与建图两个问题，被认为是实现机器人自主性的关键问题之一，对机器人的导航、控制、任务规划等领域有重要的研究意义。
根据选择传感器的不同，可大致分为基于激光雷达的激光SLAM以及基于相机的视觉SLAM。激光SLAM目前已经相当成熟，但是激光雷达硬件成本较高，同时难以维护的缺点造成了其成本高昂的问题。相比之下，视觉SLAM方案则硬件成本较低，且使用方便，渐渐成为SLAM的主流研究方向。
现有的SLAM***，大都研究的是定位问题，包括通过特征点的定位、直接法的定位，以及后端优化等，对建图模块不是很重视，建立的稀疏特征点地图也主要是服务于定位问题。但是在具体应用中，地图的用途不仅仅用于辅助定位，其明显还带有许多其他的需求，如：对机器人进行路径规划(即导航)、避障等。需要告知机器人地图中哪些地方可以通行，而哪些地方不能通行，这就超出了稀疏特征点地图的能力范围，需要建立一种稠密的地图；同时如何将建立的稠密地图与现有的移动机器人路径规划方案相结合也是需要解决的重要问题。
发明内容
本发明提供了一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，解决了对室内移动机器人周围环境的稠密建图问题，同时与移动机器人路径规划功能相结合，实现了室内移动机器人基于纯视觉方案自主导航的一体化实现。
为达到以上目的，本发明采用以下技术方案：
一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，包括以下步骤：
1)建立基于ORB特征的移动机器人定位与稀疏建图的SLAM方案；
2)通过对步骤1)中选取的关键帧进行适用于移动机器人自主导航的稠密地图构建以及栅格地图转换；
3)实现基于ROS的移动机器人稠密建图与自主导航一体化。
以上所述步骤中，步骤1)中基于ORB特征的移动机器人定位与稀疏建图包括以下步骤：
(a)提取图像ORB特征
对深度相机输入的彩色图像流进行ORB特征的提取，以提取的ORB特征作为路标贯穿SLAM***的全流程，之后对路标点进行特征匹配而非处理整张图像以加快运行速度：
(b)基于ORB特征点的跟踪定位
对连续图像帧提取ORB特征并进行匹配，并将彩色图与深度图融合，获得ORB地图点的深度和三维坐标，后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿，通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧；
(c)服务于定位的局部建图
处理上一步创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点，当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度。
(d)用于地图重定位的闭环检测
处理局部建图过程中***的关键帧，主要包含三个过程，分别是闭环检测、计算相似变换矩阵和闭环矫正，闭环检测通过计算词袋相似得分选取候选关键帧。随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(Essential Graph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹，优化位姿公式如下:
Figure BDA0004104601530000021
其中ei，j为：
Figure BDA0004104601530000031
其中，Xi，j是位姿Xw，j到Xw，i的相似变换，logsim(3)将转移矩阵的位姿误差映射到7维欧氏空间，Δi，j为边的信息矩阵。
步骤2)中通过对步骤1)中选取的关键帧进行三维稠密建图来实现，具体包括以下步骤：融合关键帧的彩色图与深度图计算得到三维点云在世界坐标系下的空间坐标，计算公式如下：
Figure BDA0004104601530000032
其中，fx,fy,cx,cy为相机内参，(u，v)为图像坐标，(x，y，z)为图像坐标系坐标，即当前关键帧的相机位姿，d为深度相机测得像素点的距离，单位为毫米，s为实际距离和测得深度d之间的比例系数，从相机坐标系点云到全局坐标系点云的变换公式如下:
Figure BDA0004104601530000033
其中，
Figure BDA0004104601530000034
为第i个关键帧位姿，/>
Figure BDA0004104601530000035
为在第i个关键帧坐标系上的点云，Ww，j是变换后获得的在全局坐标系上的点云；
之后使用外点滤波器以及降采样滤波器对三维点云进行滤波去除外点以及重复的点，方法为：当相机在相邻帧运动了一定大小时才把该帧视为关键帧，并把它的点云叠加到现有的地图中去，其计算公式如下:
min_norm≤||Δt||+min(2π-||r||，||r||)≤max_norm (5)
其中，Δt为相机在相邻帧之间的位移矢量；r为相机在相邻帧之间的旋转角度，用它们的范数和来描述相机运动大小；min_norm为相机的最小运动，max_norm为相机的最大运动，即当运动大于max_norm时，认为是相机位姿估计错误，剔除该帧；
最后使用ICP方法将两关键帧之间的点云进行配准以及点云地图的拼接来构建稠密地图，使用Octomap功能包将稠密点云地图压缩并滤除地面信息转换成占用资源更小的八叉树地图格式，之后为了实现一体化自主导航功能，再将八叉树地图通过2D投影转换成占据栅格地图格式。
步骤3)通过调用ROS Navigation来实现，具体流程为：
首先，将步骤1)以及步骤2)的整个SLAM***封装成ROS节点，将构建的栅格地图以/projected_map话题发布；其次，使用ROS Navigation功能包中的costmap_2d功能包订阅栅格地图的话题，根据栅格地图生成全局代价地图costmap，然后使用move_base功能包根据costmap使用A*算法生成全局规划路径并以nav_msgs/Path话题格式发布，最后，base_controller功能包根据生成的路径解算出差速底盘电机所需的速度指令并以cmd_vel的话题格式发布给底层电机，驱动移动机器人运动，实现自主导航。
有益效果：本发明提供了一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，设计了一种基于ORB特征的室内移动机器人定位与稠密建图***，通过对ORB特征的帧间跟踪实现了对移动机器人的实时定位，确认了移动机器人的实时位姿，并融合深度相机的彩色图像与深度图像来获得每一关键帧的稠密点云，构建了场景的稠密地图，同时为了剔除冗余的视频帧引入了空间域上的关键帧提取方法，并针对下游的路径规划任务优化了建图效果，构建了室内场景的稠密地图并转化成数据量更小的八叉树地图格式，以及能够被move_base路径规划功能包所直接使用的占据栅格地图格式；最后，将所提的稠密建图方法与ROS Navitation功能包相结合，使稠密建图与自主导航的一体化，实现了基于纯视觉方案的室内移动机器人自主导航方案。
附图说明
图1为本发明实施例中图像ORB特征提取流程图；
图2为本发明实施例中ORB特征提取实际效果图；
图3为本发明实施例中基于ORB特征点的跟踪定位流程图；
图4为本发明实施例中服务于定位的局部建图流程图；
图5为本发明实施例中局部建图效果；
图6为本发明实施例中用于地图重定位的闭环检测流程图；
图7为本发明实施例中闭环检测位姿图优化；
图8为本发明实施例中Interl Realsense D435i相机结构图；
图9为本发明实施例中实验室场地稠密建图效果；
图10为本发明实施例中Octomap数据格式；
图11为本发明实施例中稠密建图转换为八叉树地图效果图；
图12为本发明实施例中八叉树地图转化为占据栅格地图效果；
图13为本发明实施例中实验规划路径图；
图14为本发明实施例中基于ORB特征的实时定位与稠密建图***框图；
图15为本发明实施例中移动机器人自主导航示意图；
图16为本发明实施例中稠密建图与移动机器人自主导航一体化***框图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明：
一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，包括以下步骤：
1、构建基于ORB特征的移动机器人视觉SLAM方案
1.1图像ORB特征提取
ORB特征由“Oriented FAST”关键点和“Rotated BRIEF”描述子两部分组成。针对FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并再金字塔的每一次上检测角点来实现，而特征的旋转由灰度质心法来实现，其具体计算方法如图1流程所示；
在构建图像金字塔之后，逐层遍历提取FAST角点，并进行八叉树筛选来挑选出均匀分布在图像上的特征点，然后计算每个特征点的主方向以及BRIEF描述子；其中FAST特征点的提取方法为：在图像中选取像素p，设亮度为Ip，以Ip的20％为阈值T，以像素p为中心，根据需求设定半径，选取圆上的若干个像素点，如果圆上至少有连续11个点的亮度大于Ip+T或小于Ip-T，像素p就是一个特征点，特征点的主方向由灰度质心法来计算，首先计算图像的矩：
Figure BDA0004104601530000051
其中，(x，y)是目标点像素坐标，I(x，y)为当前像素的灰度值，p，q取0或1，mpq表示图像的矩，在半径为R的圆形图像区域内，沿两个坐标轴x，y方向的图像矩分别为：
Figure BDA0004104601530000061
m00是圆形区域内所有像素的总灰度值，根据图像的矩找到图像的质心：
Figure BDA0004104601530000062
特征点方向与向量
Figure BDA0004104601530000063
方向相同，关键点的旋转角度记为：
Figure BDA0004104601530000064
BRIEF描述子是一种二进制编码的描述子，用256位二进制数描述每一个特征点，具体计算方法是比较特征点附近图像窗口p中随机选取的256对像素(p，q)的大小关系，进行如下二进制赋值
Figure BDA0004104601530000065
式中，p(x)表示像素x在窗口p内的灰度值，实际提取ORB效果如图2所示，提取的ORB特征由半径为16个像素的外矩形框以及中心的特征点来表示，提取的特征点多在图像内物体的边缘以及明显的轮廓边缘等对像素变化敏感的位置，方便后续基于ORB特征点的跟踪定位。
1.2基于ORB特征点的跟踪定位
如图3所示，在对输入图像提取ORB特征后，若提取的特征点个数大于500，则对SLAM***进行初始化，用以加载BOW词袋模型以及初始化相机位姿及创建初始地图。
在初始化SLAM***之后，会根据接收到的图像估计相机位姿，再根据估计出的初始位姿跟踪局部地图并进一步优化位姿；初始位姿估计有三种手段:
(a)根据恒速运动模型估计位姿：
假定连续几帧间的运动速度是恒定的，根据运动速度与上一帧的相机位姿计算出位姿的估计值，在地图初始化后，根据最初的两个关键帧可以得到运动速度V:
V＝Tcl＝TcwTwl (II)
式中，c表示current，即当前帧，l表示last，也就是上一帧，w表示world，也就是世界坐标系，当新的一帧进入时，假设位姿是匀速变化的，也就是说上一帧到当前最新帧之间的位姿变换和上上帧到上一帧的位姿变换相同，因此可以估计出当前帧在世界坐标系下的位姿：
TcwVTlw (12)
当然，由于机器人并不是一直匀速运动，所以根据恒速运动模型估计出的相机位姿需要使用Motion-based BA进行优化，将在上一帧生成的3D地图点投影在当前帧图像的像素坐标上，在投影点附近根据描述子汉明距离进行匹配确定两帧间匹配的特征点，之后利用3D-2D的投影关系，使用G2O图优化库，构建最小二乘问题，使用列文伯格-马夸尔特方法优化当前帧位姿，具体过程为：首先构造G2o优化器，其次将待优化的当前帧的位姿作为图的顶点添加到图中，然后将观测的特征点坐标和地图点在当前帧中的投影之差作为一元边加入图中，将顶点与边相连，然后调用L-M方法迭代优化重投影误差，最后用优化后的位姿更新当前帧的位姿；
(b)根据参考帧估计位姿：
当恒速模型跟踪失败后，将使用参考关键帧进行位姿估计，首先将当前帧的BRIEF描述子转化为词袋(BOW)向量,，然后从关键帧中查找BOW向量汉明距离最近的帧，找到对应的关键帧，然后和恒速模型一样使用BA进行位姿优化；词袋向量的计算过程为：对特征点的BRIEF描述子，在提前离线训练好的词袋字典树自上而下开始使用广度优先遍历寻找自己的位置，从根节点开始，用该描述子和每个节点的描述子计算汉明距离，选择汉明距离最小的节点作为自己所在的节点，一直遍历到叶子节点。最终把叶子的单词id和权重等属性赋予这个特征点，作为该点的词袋向量；
(c)通过重定位估计位姿：
当前两种位姿估计方法都失效时，通过重定位方法来估计位姿，具体流程为：首先计算当前帧特征点的词袋向量，利用词袋找到与当前帧相似的重定位候选关键帧，然后遍历所有候选关键帧，通过词袋进行快速匹配，得到匹配的2D点对，再利用EPnP迭代得到粗略的相机位姿估计，再将得到的粗略位姿与点对构建最小二乘问题，使用恒速运动模型中的方法估计得到优化的位姿。
成功估计当前帧的初始位姿后，基于当前位姿更新局部地图并优化当前帧位姿，主要流程：
(1)更新共视图以及局部地图，包括局部关键帧以及局部地图点；
(2)将局部地图点投影到当前帧上，舍弃越界地图点；
(3)将局部地图点与当前帧特征点进行位姿优化，优化当前帧位姿；
(4)更新地图点观测数值，统计内点数并以此判断是否跟踪成功；
选取关键帧方法为：根据当前帧的位姿判断是否筛选为关键帧，若当前帧和上一关键帧之间相对运动距离大于阈值则舍弃该帧；若当前帧和上一关键帧匹配的特征点数小于阈值则舍弃该帧，通过这些条件最后筛选出关键帧。
1.3服务于定位的局部建图
在筛选出关键帧之后，回环检测以及稠密建图等步骤都以筛选出的关键帧为对象以节省计算资源，***关键帧后，计算关键帧的词袋向量以及共视图信息，并在地图中添加当前关键帧的相机位姿；
在不断处理关键帧构建局部地图的过程中，需要对冗余的坏地图点进行去除，当跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25％，从地图中删除，或者该地图点不能被创建的关键帧及其所相邻的2个关键帧中同时观测到，则认为是坏地图点，删除。若地图点经过了连续3个关键帧仍未被剔除,则被认为是好的地图点；
在剔除坏的地图点之后，根据构建的地图中的关键帧，将当前关键帧分别与共视程度最高的前10个共视关键帧两两进行特征匹配，通过三角化生成新地图点以弥补剔除地图点后地图点过少对跟踪产生的不好影响，再将关键帧以及该关键帧的公式关键帧及地图点进行局部BA优化，同时优化地图中关键帧和地图点的位姿，在构建局部地图的过程中，地图点和关键帧是冗余的，那么需要进一步更为严苛地筛选它们，出现以下情况的地图点就会被剔除:1)这些点在接下来的图像帧中无法跟踪和匹配；2)投影光线通过三角化计算点处于低视差；3)三角测量点会产生的重投影误差较大，这个严格地图点筛选过程保证了构建地图的鲁棒性。为了保证重构地图的简洁性，降低BA过程中的复杂度，检测冗余关键帧并删除它们，在关键帧集中，如果一个关键帧的90％的地图点至少在其他的三个关键帧中被检测到，那么则剔除该关键帧，最终的局部稀疏建图效果如图5所示，其中矩形框为跟踪定位过程中的相机位姿，点为相机在各位姿时观测到的路标点。
1.4用于地图重定位的闭环检测
闭环检测用于降低位姿跟踪的累计误差以及对地图的重定位，其流程如图6所示，闭环检测流程和局部建图流程类似，都是对跟踪过程中提取的关键帧进行处理，闭环检测的第一步是判断是否发生闭环，若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组，且这些闭环匹配关键帧组间是连续的,则认为发生闭环，具体过程为：
(1)找到当前关键帧的闭环候选关键帧，闭环候选关键帧取自于与当前关键帧具有相同的BOW向量但不存在直接连接的关键帧；
(2)将闭环候选关键帧与其共视关键帧组合成候选关键帧组；
(3)在当前关键组和之前的连续关键组间寻找连续关系，若当前关键帧组在之前的连续关键帧组中找到连续关系，则当前的连续关键帧组的连续长度加1，关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中，若某关键帧组的连续长度达到3，则认为该关键帧发生闭环。
在根据词袋向量匹配确定发生回环之后，需要进行Sim3变换，将当前发生回环的关键帧与匹配的关键帧进行位姿闭环融合，将闭环关键帧组地图点投影到当前关键帧上，之后进行本质图优化。
本质图优化即为了降低视觉里程计过程中的积累误差，利用各关键帧的相互关联性进行有效的闭合回环，在本质视图上优化位姿，这样就可以将累计的误差分散到位姿图中，并通过相似变换矫正尺度偏移，通过回环检测优化位姿之后，根据优化后的关键帧矫正地图点云。首先计算关键帧的词袋和其数据关联视图附近关键帧的相似度，并提出相似度较低的关键帧，同时删除和关键帧Ki直接连接的关键帧，最终获得闭合回环，然后当闭合回环达到一定程度时，利用通用图优化框架(General Graph optimization，G2O)优化回环位姿图，如图7所示，最后更新地图点，融合重复的地图点，位姿图优化公式如下:
Figure BDA0004104601530000101
其中ei，j为待优化的重投影误差：
Figure BDA0004104601530000102
其中，Xi，j是位姿Xw，j到Xw，i的相似变换，logsim(3)将转移矩阵的位姿误差映射到7维欧氏空间，Δi，j为边的信息矩阵。
2、适用于移动机器人自主导航的稠密地图构建以及栅格地图转换
2.1稠密点云地图构建
在步骤1中，构建的地图是三维稀疏点云地图，无法在实际的机器人领域中应用，如机器人导航、路径规划等，为了克服这一不足之处，本发明提出了利用深度摄像头IntelRealsense D435i作为传感器设备来构建三维稠密地图，如果将每一帧的点云都融合到地图中，那么地图的容量会很大，从而降低了***实时性能，由于机器人在运动过程中相邻图像帧具有连续性，即相邻帧的位姿在空间上变化较小，提出了空间域上的关键帧提取方法来对步骤1中的关键帧进一步筛选合适的图像帧。
Intel Realsense D435i摄像头同时集成了单目、双目以及RGBD相机和惯性测量单元IMU，其中深度传感器由红外发射器和红外接收器两部分构成，如图8所示。彩色相机可以获得每个像素点的RGB值即彩色图像，而深度传感器则可以测量到像素点的距离信息即深度图像，融合彩色图和深度图像计算获得三维点云，计算公式如下：
Figure BDA0004104601530000103
其中，fx,fy,cx,cy为相机内参，(u，v)为图像坐标，(x，y，z)为图像坐标系坐标，即当前关键帧的相机位姿，d为深度相机测得像素点的距离，单位为毫米，s为实际距离和测得深度d之间的比例系数，为1000，从相机坐标系点云到全局坐标系点云的变换公式如下:
Figure BDA0004104601530000111
其中，
Figure BDA0004104601530000112
为第i个关键帧位姿，/>
Figure BDA0004104601530000113
为在第i个关键帧坐标系上的点云，Xw，j是变换后获得的在全局坐标系上的点云，为了避免三维点云的冗余，造成不必要的计算量，采用基于空间域上的方法来提取关键帧，其思想是只有当相机在相邻帧运动了一定大小时才把该帧视为关键帧，并把它的点云叠加到现有的地图中去，其计算公式如下:
min_norm≤||Δt||+min(2π-||r||，||r||)≤max_norm (17)
其中，Δt为相机在相邻帧之间的位移矢量；r为相机在相邻帧之间的旋转角度，用它们的范数和来描述相机运动大小；min_norm为相机的最小运动，max_norm为相机的最大运动，即当运动大于max_norm时，认为是相机位姿估计错误，剔除该帧，本实施例中的取值分别为0.4和5。
2.2栅格地图格式转换
由于稠密点云地图不能直接用于移动机器人导航，因此需要将稠密点云地图进行转化，转化成适用于平面移动机器人路径规划的占据栅格地图。这一过程将通过Octomap功能包实现。Octomap是一种基于稠密点云的新的地图表达方式，相比点云而言，具有占据空间小，可以方便的更新的优点，可以用于机器人的导航。Octomap通过将正方体空间划分为八个小正方体，从而构造成一个八叉树的结构，大的正方体称为父节点，小的正方体称为子节点，八叉树可以不断地向下拓展，直到达到最小分辨率，称为叶节点，如图9所示，Octomap使用概率描述节点的占据状态，概率大于0.5表示占据，小于0.5表示未占据，使用概率表示占据信息的的方法可以方便的根据传感器数据对节点状态进行更新。同时，当所有子节点状态相同时，可将其中的所有子节点删除，只用父节点表示该空间状态，这种方式大量的节省了存储空间。Octomap使用八叉树格式代替点云来表示三维空间中的信息，大大压缩了数据量。
在实验室场地中的实际稠密建图效果如图10所示，将实验室的三维稠密地图压缩转换成八叉树地图的效果如图11所示，进一步的，通过对Octomap进行2D平面的投影，将Octomap转换成了2D的占据栅格地图，如图12所示，以便平面移动机器人路径规划时直接调用。
3、基于ROS的移动机器人稠密建图与自主导航一体化实现(如图16所示)
ROS作为目前最为流行的机器人***框架，提供了强大的通行框架，为各功能的一体化实现提供了可能。首先将稠密建图过程封装成一个ros节点，并将最后的栅格地图以/projected_map的rostopic发布出来，通过map_server功能包进行地图的保存以及复用；自主导航通过ROS的Navigation功能包实现，该功能包仅针对平面2D差速移动机器人所设计，整个功能包集合以move_base为核心，将里程计信息、传感器信息、定位信息、地图以及目标点输入给move_base，move_base经过规划后会输出速度指令，move_base包括三个关键部分：global_planner(全局规划器)、local_planner(局部规划器)和recovery_behaviors(恢复行为)，这三个部分都是以插件的形式实现的，通过插件机制可以方便地切换不同算法实现的规划器。恢复行为会在机器人移动过程中出现了异常状态时被触发，目的是帮助机器人摆脱异常状态。另外，move_base还包括了global_costmap(全局代价地图)和local_costmap(局部代价地图)，规划器需要在代价地图上进行导航规划，使用costmap_2d功能包订阅发布的/projeed_map栅格地图话题，生成全局代价地图，再使用global_planner功能包读取代价地图，使用A*算法进行规划，再把规划好的路径以nav_msgs/Path话题形式发布，最后base_controller功能包订阅路径并解算成电机所需输出的速度量并发布话题/cmd_vel，通过驱动程序传到底层差速电机完成运动，本次实际的运动轨迹如图13所示。
综上本发明设计的基于深度相机的室内移动机器人稠密建图与自主导航方法在实验中表现良好，能够较好的对移动机器人进行实时定位并能较好的重建周围的三维环境，并转化成栅格地图服务于自主导航***。
以上为本发明的优选实施方式，但是本发明不只限于以上实施方式。对于本技术领域的普通技术人员而言，在其知识范围内以及不脱离本发明原理的前提下，可以对本发明作出若干改进，这些也视作本发明的保护范围。

Claims (8)

1.一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，包括以下步骤：
1)建立基于ORB特征的移动机器人定位与稀疏建图的SLAM方案；
2)通过对步骤1)中选取的关键帧进行适用于移动机器人自主导航的稠密地图构建以及栅格地图转换；
3)实现基于ROS的移动机器人稠密建图与自主导航一体化。
2.根据权利要求1所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，步骤1)中基于ORB特征的移动机器人定位与稀疏建图包括以下步骤：
(a)提取图像ORB特征
对深度相机输入的彩色图像流进行ORB特征的提取，以提取的ORB特征作为路标贯穿SLAM***的全流程，之后对路标点进行特征匹配而非处理整张图像以加快运行速度；
(b)基于ORB特征点的跟踪定位
对连续图像帧提取ORB特征并进行匹配，并将彩色图与深度图融合，获得ORB地图点的深度和三维坐标，后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿，通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧；
(c)服务于定位的局部建图
处理上一步创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点，当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度；
(d)用于地图重定位的闭环检测
处理局部建图过程中***的关键帧，主要包含三个过程：闭环检测、计算相似变换矩阵和闭环矫正。
3.根据权利要求2所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，处理局部建图过程中***的关键帧的具体过程为：闭环检测通过计算词袋相似得分选取候选关键帧，随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(Essential Graph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹，优化位姿公式如下：
Figure FDA0004104601520000021
其中ei，j为：
Figure FDA0004104601520000022
其中，Xi，j是位姿Xw，j到Xw，i的相似变换，logsim(3)将转移矩阵的位姿误差映射到7维欧氏空间，Δi，j为边的信息矩阵。
4.根据权利要求1或2所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，步骤2)具体包括以下步骤：融合关键帧的彩色图与深度图计算得到三维点云在世界坐标系下的空间坐标，计算公式如下：
Figure FDA0004104601520000023
其中，fx，fy，cx，cy为相机内参，(u，v)为图像坐标，(x，y，z)为图像坐标系坐标，即当前关键帧的相机位姿，d为深度相机测得像素点的距离，单位为毫米，s为实际距离和测得深度d之间的比例系数，从相机坐标系点云到全局坐标系点云的变换公式如下：
Figure FDA0004104601520000024
其中，
Figure FDA0004104601520000025
为第i个关键帧位姿，/>
Figure FDA0004104601520000026
为在第i个关键帧坐标系上的点云，Xw，j是变换后获得的在全局坐标系上的点云；
之后使用外点滤波器以及降采样滤波器对三维点云进行滤波去除外点以及重复的点；
最后使用ICP方法将两关键帧之间的点云进行配准以及点云地图的拼接来构建稠密地图，使用Octomap功能包将稠密点云地图压缩并滤除地面信息转换成占用资源更小的八叉树地图格式，再将八叉树地图通过2D投影转换成占据栅格地图格式。
5.根据权利要求4所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，使用外点滤波器以及降采样滤波器对三维点云进行滤波去除外点以及重复的点的方法为：当相机在相邻帧运动了一定大小时才把该帧视为关键帧，并把其点云叠加到现有的地图中去，其计算公式如下：
min_norm≤||Δt||+min(2π-||r||，||r||)≤max_norm (5)
其中，Δt为相机在相邻帧之间的位移矢量；r为相机在相邻帧之间的旋转角度，用它们的范数和来描述相机运动大小；min_norm为相机的最小运动，max_norm为相机的最大运动，即当运动大于max_norm时，认为是相机位姿估计错误，剔除该帧。
6.根据权利要求4所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，使用Octomap功能包将稠密点云地图压缩并滤除地面信息转换成占用资源更小的八叉树地图格式具体过程为：Octomap通过将正方体空间划分为八个小正方体，从而构造成一个八叉树的结构，大的正方体称为父节点，小的正方体称为子节点，八叉树不断地向下拓展，直到达到最小分辨率，当所有子节点状态相同时，可将其中的所有子节点删除，只用父节点表示该空间状态。
7.根据权利要求6所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，Octomap使用概率描述节点的占据状态，概率大于0.5表示占据，小于0.5表示未占据。
8.根据权利要求1所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，步骤3)具体流程为：首先，将步骤1)以及步骤2)的整个SLAM***封装成ROS节点，将构建的栅格地图以/projected_map话题发布；其次，使用ROS Navigation功能包中的costmap_2d功能包订阅栅格地图的话题，根据栅格地图生成全局代价地图costmap，然后使用move_base功能包根据costmap使用A*算法生成全局规划路径并以nav_msgs/Path话题格式发布，最后，base_controller功能包根据生成的路径解算出差速底盘电机所需的速度指令并以cmd_vel的话题格式发布给底层电机，驱动移动机器人运动，实现自主导航。
CN202310188359.5A 2023-03-01 2023-03-01 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法 Pending CN116295412A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310188359.5A CN116295412A (zh) 2023-03-01 2023-03-01 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310188359.5A CN116295412A (zh) 2023-03-01 2023-03-01 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法

Publications (1)

Publication Number Publication Date
CN116295412A true CN116295412A (zh) 2023-06-23

Family

ID=86812554

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310188359.5A Pending CN116295412A (zh) 2023-03-01 2023-03-01 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法

Country Status (1)

Country Link
CN (1) CN116295412A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117197211A (zh) * 2023-09-04 2023-12-08 北京斯年智驾科技有限公司 一种深度图像生成方法、***、装置及介质
CN117495968A (zh) * 2024-01-02 2024-02-02 苏州中德睿博智能科技有限公司 基于3d激光雷达的移动机器人位姿跟踪方法及装置
CN117893693A (zh) * 2024-03-15 2024-04-16 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117906595A (zh) * 2024-03-20 2024-04-19 常熟理工学院 基于特征点法视觉slam的场景理解导航方法及***

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117197211A (zh) * 2023-09-04 2023-12-08 北京斯年智驾科技有限公司 一种深度图像生成方法、***、装置及介质
CN117197211B (zh) * 2023-09-04 2024-04-26 北京斯年智驾科技有限公司 一种深度图像生成方法、***、装置及介质
CN117495968A (zh) * 2024-01-02 2024-02-02 苏州中德睿博智能科技有限公司 基于3d激光雷达的移动机器人位姿跟踪方法及装置
CN117893693A (zh) * 2024-03-15 2024-04-16 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117893693B (zh) * 2024-03-15 2024-05-28 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117906595A (zh) * 2024-03-20 2024-04-19 常熟理工学院 基于特征点法视觉slam的场景理解导航方法及***

Similar Documents

Publication Publication Date Title
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN113269098B (zh) 一种基于无人机的多目标跟踪定位与运动状态估计方法
US10192113B1 (en) Quadocular sensor design in autonomous platforms
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与***
US10496104B1 (en) Positional awareness with quadocular sensor in autonomous platforms
CN106802668B (zh) 基于双目与超声波融合的无人机三维避撞方法及***
CN112197770B (zh) 一种机器人的定位方法及其定位装置
Kim et al. Deep learning based vehicle position and orientation estimation via inverse perspective mapping image
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN106940186A (zh) 一种机器人自主定位与导航方法及***
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN116295412A (zh) 一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法
CN111664843A (zh) 一种基于slam的智能仓储盘点方法
CN114842438A (zh) 用于自动驾驶汽车的地形检测方法、***及可读存储介质
KR20220064524A (ko) 이미지 기반 측위 방법 및 시스템
CN112750203A (zh) 模型重建方法、装置、设备及存储介质
CN115273034A (zh) 一种基于车载多传感器融合的交通目标检测与跟踪方法
CN114549738A (zh) 无人车室内实时稠密点云重建方法、***、设备及介质
CN114969221A (zh) 一种更新地图的方法及相关设备
Saleem et al. Neural network-based recent research developments in SLAM for autonomous ground vehicles: A review
CN113838129B (zh) 一种获得位姿信息的方法、装置以及***
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN113570716A (zh) 云端三维地图构建方法、***及设备
KR102249381B1 (ko) 3차원 영상 정보를 이용한 모바일 디바이스의 공간 정보 생성 시스템 및 방법

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