CN112734765B - 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 - Google Patents
基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 Download PDFInfo
- Publication number
- CN112734765B CN112734765B CN202011393477.2A CN202011393477A CN112734765B CN 112734765 B CN112734765 B CN 112734765B CN 202011393477 A CN202011393477 A CN 202011393477A CN 112734765 B CN112734765 B CN 112734765B
- Authority
- CN
- China
- Prior art keywords
- camera
- pose
- image
- segmentation
- imu
- 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
Links
- 230000011218 segmentation Effects 0.000 title claims abstract description 62
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004927 fusion Effects 0.000 title claims abstract description 29
- 238000005457 optimization Methods 0.000 claims abstract description 28
- 230000010354 integration Effects 0.000 claims abstract description 24
- 238000000605 extraction Methods 0.000 claims abstract description 23
- 230000000007 visual effect Effects 0.000 claims abstract description 22
- 230000003068 static effect Effects 0.000 claims abstract description 13
- 238000004364 calculation method Methods 0.000 claims abstract description 12
- 238000005259 measurement Methods 0.000 claims description 30
- 230000033001 locomotion Effects 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000003287 optical effect Effects 0.000 claims description 10
- 230000006870 function Effects 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 230000008878 coupling Effects 0.000 claims description 6
- 238000010168 coupling process Methods 0.000 claims description 6
- 238000005859 coupling reaction Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 241001465754 Metazoa Species 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 239000013598 vector Substances 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000003384 imaging method Methods 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 2
- 238000013527 convolutional neural network Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 5
- 241000282414 Homo sapiens Species 0.000 description 4
- 238000013528 artificial neural network Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 238000013135 deep learning Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 241000282412 Homo Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000001310 location test Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/11—Region-based segmentation
-
- 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
-
- 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/207—Analysis of motion for motion estimation over a hierarchy of resolutions
-
- 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
- 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/10004—Still image; Photographic 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/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
Abstract
本发明公开了一种基于实例分割与多传感器融合的移动机器人定位方法、系统及介质，方法包括下述步骤：对相机图像进行目标实例分割；对分割后的图像提取并追踪特征点；计算特征点深度值并使用EPnP算法进行位姿解算；基于IMU预积分的位姿解算；根据实例分割信息和特征点提取情况，选择位姿初值的更新方式；视觉传感器与IMU进行紧耦合的位姿优化，获取精准的姿态；本发明获取图像的实例分割信息，选取复杂环境中的静态特征点作为基准，估计自身位姿状态，并且可以在相机定位不理想时切换至IMU更新定位初值信息，使得机器人的定位在高度动态环境下也具有精准度和鲁棒性。
Description
技术领域
本发明属于智能移动机器人感知领域，具体涉及一种基于实例分割与多传感器融合的定位方法、系统及介质。
背景技术
机器人发明的初衷是帮助人类完成一些琐碎、服务性、机械性、甚至具有一定危险性的任务，使人们的生活更加轻松美好。本世纪以来，机器人技术得到了迅猛发展。各色各类的机器人开始出现在人们的生活中，如巡逻机器人、扫地机器人、勘测机器人、快递分敛机器人等。移动机器人作为机器人中的大类，很多情况下需要在移动的过程中完成自身的任务。这要求它们需要像人类一样，能够辨别周边的环境，根据环境信息进行自主导航。其中，获取自身在周围环境中的位置便是解决移动机器人自主导航问题的基础。
目前已知用于移动机器人定位的主要传感器包括激光雷达、声纳传感器、GPS、视觉传感器和惯性测量单元。其中，激光雷达和声呐传感器因为其可以直接获取环境中物体精准的三维空间位置，减轻了算法负担，研究已经取得重大突破。但此类传感器价格比较昂贵，不符合低成本的需求。若使用GPS导航仪，其精度的高低深受环境的影响，在室内环境下会出现严重偏差。再者，即使在室外环境下，GPS最好的精度也只能达到分米制级别。对于车辆的导航可能有不错的效果，但对于移动机器人这种小范围的活动，这种程度的误差并不能被接受。随着计算机视觉的飞速发展，基于视觉传感器的定位方法逐渐成为重点研究方向。视觉传感器拥有丰富的像素信息，且定位原理上与人类自身的定位机理极为相似，有着不错的定位精度。但绝多数的视觉定位方法都以假定环境为不发生变化的刚体为前提来进行定位，当图像中出现正在移动的物体时，误以该物体上的特征作为定位基准将会对机器人的定位造成重大误差，高度动态环境下甚至导致定位失败。为了解决上述问题，本发明将深度学习中的实例分割技术应用到机器人的视觉定位方法中，剔除图像中的动态物体。同时，融合相机和IMU的传感器信息，获取更加精确的定位数据。
发明内容
本发明的主要目的在于克服传统视觉定位方法在动态环境下的不足，提供一种基于实例分割与多传感器融合的移动机器人定位方法、系统及介质，根据该定位方法可计算出较为精准的机器人位置与姿态，且该定位系统具有较强的鲁棒性。
为了达到上述目的，本发明采用以下技术方案：
本发明提供的一种基于实例分割与多传感器融合的移动机器人定位方法，包括下述步骤：
对相机图像进行目标实例分割，得到分割掩膜；
根据语义标签和分割掩膜，对目标实例分割后的静态物体部分进行特征点的提取和追踪；
基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
建立IMU测量模型，使用预积分的方式进行位姿解算；
根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
优先的，采用Mask R-CNN算法对相机图像进行目标实例分割，将相机图像中的背景、人物、动物和车辆分割开，并获取对应的语义标签，根据语义标签信息，将常识中被认为可移动的物体上的像素标记为1，不可移动的物体上的像素标记为0，得到由0、1组成的分割掩膜。
优先的，所述对目标实例分隔后的静态物体部分进行特征点的提取和追踪，具体为：
根据得到的分割掩膜，在标记为0的像素区域提取FAST角点作为特征点，在往后图像帧和左右图像帧中使用KLT稀疏光流算法对特征点进行追踪，追踪到的特征点和原先的特征点标记为同一点。4、根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，使用MyntEye-D相机，根据其成像原理构建针孔相机的立体视觉模型，数学转化关系如下：
其中，u和v分别为特征点在图像上的横纵坐标系，X,Y,Z为特征点在相机坐标系下的三维坐标，fx，fy为相机的横向焦距和纵向焦距，u0，v0为相机中心点的坐标参数；
基于针孔相机立体视觉模型，根据同一个特征点在左右相机图像上的视差计算出该点的深度信息，其计算公式为：
d＝uL-uR
其中，b为左右相机光心的距离，d为视差，uL和uR分别为特征点在左相机和右相机上投影的横坐标，fx为横向焦距；
已知前一帧的特征点在相机坐标系下的三维坐标以及相机的位姿，使用坐标系转化公式获取特征点在世界坐标系下的三维坐标，基于特征点的三维坐标和当前帧的投影坐标使用EPnP算法求解出当前帧的相机位姿。
优先的，所述IMU测量模型，具体为：
其中，为机器人运动过程中加速度计和陀螺仪传感器上的测量值，at，ωt为机器人运动的实际加速度和角速度值，na，nω分别为两种传感器测量的随机噪声，为t时刻两个传感器测量的偏差噪声，/>为t时刻IMU在世界坐标系下的姿态，gw表示重力向量；
所述使用预积分的方式进行位姿解算，具体为：
其中：
分别为位置、速度和姿态的预积分值，/>表示第i-1帧时刻世界坐标系转化为机器人坐标系的旋转矩阵，/>和/>分别表示机器人在t时刻相对于第i-1帧时刻的旋转变化的旋转矩阵形式和四元数形式，/>表示第i帧时刻的机器人相对于世界坐标的位置与姿态的四元数表示，/>表示第i帧时刻的速度，同理，/>表示第i-1时刻，Δt表示IMU数据之间的时间间隔。根据上式可由IMU测量获得机器人当前时刻的位置，姿态和速度。
优先的，选取位姿初值的更新方式具体为：
根据相机图像的状态进行选择，提取当前帧图像的像素平均灰度值I、动态物体区域占完整图像的比率r、特征点提取数目N和平均深度d，若同时满足50＞I＞220，r＜50％，N＞50，d＜20，则认为图像状态良好，适宜相机进行初值更新，故选择使用EPnP算法对当前相机的位姿进行求解，反之如有其中一个条件不满足，则使用预积分的方式进行位姿解算。
优先的，所述非线性优化具体为：
需要优化的变量包括不同时刻关键帧的位置姿态/>速度/>加速度计偏差/>和陀螺仪偏差/>
χ＝[x0,x1,…,xn]
其中，χ和xi为这些优化变量的集合，n为选取参与优化的帧数量，i表示每一帧图像在这组图像帧中的序号；
IMU预积分的测量残差定义为：
视觉重投影测量残差定义为：
其中，和/>分别为特征点在第j帧相机坐标系下的齐次三维坐标的实际值和投影值，Tji表示j帧和j帧之间的变换矩阵，Rji和tji表示旋转矩阵和平移量；
将IMU预积分测量残差和视觉重投影残差进行紧耦合优化，构建目标函数：
使用Levenberg–Marquadt算法可对X优化，求解最优的机器人位姿。
本发明还提供了一种基于实例分割与多传感器融合的移动机器人定位系统，应用于所述的基于实例分割与多传感器融合的移动机器人定位方法，包括分割模块、特征点的提取和追踪模块、第一位姿求解模块、第二位姿求解模块、选择模块以及优化模块；
所述分割模块，用于对相机图像进行目标实例分割，得到分割掩膜；
所述特征点的提取和追踪模块，用于根据语义标签和分割掩膜，对目标实例分隔后的静态物体部分进行特征点的提取和追踪；
所述第一位姿求解模块，用于基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
所述第二位姿求解模块，用于建立IMU测量模型，使用预积分的方式进行位姿解算；
所述选择模块，用于根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
所述优化模块，用于构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
本发明还提供了一种存储介质，存储有程序，所述程序被处理器执行时，实现所述的基于实例分割与多传感器融合的移动机器人定位方法。
本发明与现有技术相比，具有如下优点和有益效果：
1、本发明将深度学习中的像素级实例分割算法应用到了视觉定位方法前端，为机器人的定位提供了动态物体检测的功能。根据分割掩膜信息，可以将相机图像中动态物体区域的像素剔除，在静态区域提取特征点作为定位基准，使机器人的定位在高度动态的环境下也能保持较高的精度，不容易发生定位失败的情况。同时，该部分的物体语义信息可应用于机器人的其他智能工作。
2、本发明根据图像的亮度条件、环境中动态物体覆盖情况和特征点的提取情况，自动切换定位初值的更新方式，在条件良好时，使用精度较好的视觉定位更新，条件较差时，采用不受环境影响的IMU预积分更新，充分发挥两种传感器各自的优势。使机器人的定位更具鲁棒性，在多种条件下都能存活。
3、本发明采用紧耦合方式融合双目相机的图像数据和IMU测量数据，充分利用了IMU在短时间对快速运动的捕捉特性和相机在静止条件下的零漂移特性，使机器人可以同时适应低速和高速的运动场合，获取更为精确的定位数据。
附图说明
图1为基于实例分割与多传感器融合的移动机器人定位方法的流程示意图。
图2为图像金字塔示意图。
图3为双目相机视差原理示意图。
图4为MyntEye-D相机实际场景定位测试结果。
图5是基于实例分割与多传感器融合的移动机器人定位系统的结果示意图。
图6是本发明实施例的存储介质的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
实施例
Mask R-CNN已经成为目标检测和实例分割领域最为重要的神经网络之一，MaskR-CNN总体来说是从Faster R-CNN改进而来，但是它替换了Faster R-CNN使用的VGG网络，使用了特征提取能力更强的Res Net残差神经网络为主干网络。Mask R-CNN对不同尺度的特征图经过ROI Align操作得到的感兴趣区域的固定大小的特征图进行Concat的操作，随即进入分类和预测的网络。网络分为三部分，一部分为预测类别的全连接网络，一部分为预测边界框的全连接网络，一部分是为预测mask的全卷积神经网络，这三部分是平行结构。在得到Mask R-CNN的预测结果之后，可以根据预测结果和实际结果对神经网络进行训练。
多传感器信息融合(Multi-sensor Information Fusion,MSIF)，就是利用计算机技术将来自多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。多传感器信息融合技术的基本原理是将各种传感器进行多层次、多空间的信息互补和优化组合处理，最终产生对观测环境的一致性解释。在这个过程中要充分地利用多源数据进行合理支配与使用，而信息融合的最终目标则是基于各传感器获得的分离观测信息，通过对信息多级别、多方面组合导出更多有用信息。这不仅是利用了多个传感器相互协同操作的优势，而且也综合处理了其它信息源的数据来提高整个传感器系统的智能化。
如图1所示，本实施例基于实例分割与多传感器融合的移动机器人定位方法，具体实施步骤如下所述：
步骤(1)：对相机图像进行目标实例分割。
为节省程序运行时间，本步骤仅对左相机的图像进行处理。使用一种名为MaskR-CNN的算法模型对图像进行实例分割，该模型使用MSCOCO数据集进行训练，可以识别出常见的人、动物、交通工具等物体，并将其轮廓提取出来。将获取的语义标签进行分类，其中，人、动物类等可以自主移动的物体标记为动态物体，而天空、房子等不能自主移动的物体标记为静态物体。创建大小与原图像一致的分割掩膜，原图像中被标记为动态物体的像素坐标上掩膜值设为1，标记为静态物体的像素坐标上掩膜值设为0。
步骤(2)：对分割后的图像提取并追踪特征点。
根据步骤(1)中获取的分割掩膜，在掩膜值为0的区域提取FAST角点。设定FAST角点的阈值t为20，遍历掩膜值为0区域的所有像素点，对于每个像素点，其灰度值为Ip，获取其周围圆上的16个点的灰度值，若有连续9个像素点的灰度值在(Ip-t,Ip+t)之间，则该点被认为是一个角点。为充分利用整张图像，达到良好的定位效果，设定相邻两角点之间的间距不小于30个像素点，整张图像的角点提取数目设置为150个，使特征点遍布整张图像而不会出现某个区域密集的情况。
接着需要将前后两帧和左右相机图像的特征点进行关联，本发明使用KLT稀疏光流算法对特征点进行追踪。为达到最好的匹配效果，建立缩放倍率为0.5倍的4层图像金字塔，如图2所示。由顶层的图像开始计算，然后把上一层的追踪结果作为下一层光流的初始值。同时，运用逆向光流法对追踪结果进行检验，提高追踪结果的准确性。最终，由左相机图像中提取的角点追踪到右相机图像和下一阵图像的相应坐标，将该坐标点和原点标记为同一点。
步骤(3)：计算特征点深度值并使用EPnP算法进行位姿解算。
使用MyntEye-D相机，根据其成像原理，构建针孔相机模型如下：
其中，u和v分别为特征点在图像上的横纵坐标系，X,Y,Z为特征点在相机坐标系下的三维坐标，fx，fy为相机的横向焦距和纵向焦距，u0，v0为相机中心点的坐标参数。
如图3所示，一般情况下双目相机的两个光心水平方向一致，故同一个空间点P投影到两个相机上时，纵坐标保持一致，横坐标有差异。令该空间点在左右相机图像中的投影纵坐标为uL、uR，左右相机的光心距离即基线大小为b，由相似三角形的关系即可得到：
深度值Z的求解公式即为：
d＝uL-uR
当深度值已知，即可由针孔相机模型求解出空间点在相机坐标系下的三维坐标(X,Y,Z)，再由上一阵图像解算出的相机位姿可将这些空间点转化为世界坐标系下的三维坐标。由步骤(3)中的光流法追踪，可以将当前帧的特征点和这些空间点一一对应起来，接下来应用EPnP算法可对当前帧的相机位姿进行求解。
步骤(4)：基于IMU预积分的位姿解算。
IMU由陀螺仪和加速度计组成，对加速度数据进行时间的二次积分可得到机器人的位移，对角速度进行时间的一次积分可得到机器人旋转的欧拉角。然而IMU有严重的漂移误差且外界干扰噪声不客观，使得IMU无法作为一个单独使用的定位传感器。考虑到IMU中可预测的偏移噪声和不可预测的随机噪声，以及重力的影响，构建IMU测量模型如下：
其中，为机器人运动过程中加速度计和陀螺仪传感器上的测量值，at，ωt为机器人运动的实际加速度和角速度，na，nω为两种传感器的随机测量噪声(服从均值为0的正态分布)，/>为t时刻两个传感器测量的偏差噪声。/>为t时刻IMU在世界坐标系下的姿态，gw表示重力向量。
为减少后端紧耦合优化的计算量，IMU的位姿更新方式选择预积分的方式，关系式如下：
其中：
分别为位置、速度和姿态的预积分值，/>表示第i-1帧时刻世界坐标系转化为机器人坐标系的旋转矩阵，/>和/>分别表示机器人在t时刻相对于第i-1帧时刻的旋转变化的旋转矩阵形式和四元数形式，/>表示第i帧时刻的机器人相对于世界坐标的位置与姿态的四元数表示，/>表示第i帧时刻的速度，同理，/>表示第i-1时刻，Δt表示IMU数据之间的时间间隔。根据上式可由IMU测量获得机器人当前时刻的位置，姿态和速度。
步骤(5)：根据实例分割信息和特征点提取情况，选择位姿初值的更新方式。
在步骤(3)和步骤(4)中分别是相机和IMU预积分进行两种方式的位姿估计。一般情况下，图像数据良好的视觉定位在精度上优于IMU预积分定位，故具体选择哪种方式进行位姿初值的更新主要取决于相机图像的状态。首先提取当前帧图像的像素平均灰度值I，平均灰度值主要体现了整幅图像的明暗程度，若I＞220,则认为光线过于充足，相机处于过亮的环境下，若I＜50,则认为相机处于过暗的情况下，两种情况都不适合进行视觉定位。接着计算动态物体区域占完整图像的比率r：
其中，ndynamic表示动态区域像素点总量，nall表示图像像素点总量。当r＞50％,表示当前环境处于高度动态条件下，不适合使用视觉定位。
然后计算特征点提取数目N和平均深度d，由于本发明设定的特征点提取数量为150，当去除动态区域后，倘若剩余区域提取的特征点数目少于50个，则不足以计算出较为准确的视觉定位信息。同时，由于双目相机的视差最小为1个像素，使得特征点的深度估计存在一个理论最大值且有误差，故当特征点处于较远位置时，特征点的三维坐标存在误差，不适合作为定位基准。所以当d大于20m时定位效果也不好。
综上所述，若同时满足50＞I＞220，r＜50％，N＞50，d＜20则认为图像状态良好，适宜相机进行初值更新，故选择步骤(3)，反之如有其中一个条件不满足，则选择步骤(4)进行更新。
步骤(6)：视觉传感器与IMU进行紧耦合的位姿优化，获取精准的姿态。
为减少优化算法的计算量，保证程序的实时性，本发明采取滑动窗口的策略，每次仅对窗口内10帧的数据进行紧耦合优化。首先，明确需要进行优化的变量包括：关键帧的位置姿态/>速度/>加速度计偏差(ba)，陀螺仪偏差(bω)，构成向量如下：
χ＝[x0,x1,…,xn]
接着构建IMU测量残差和视觉重投影残差。当相邻两帧的初始位姿已知，可以求出两帧之间的位置、速度、姿态、IMU传感器偏差等状态变化量，而通过IMU的数据进行预积分可以获得实际变化量，构建IMU预积分测量残差为：
同理，同一个特征点被两帧同时观测到，即可建立两帧状态变量之间的约束关系。为通过第i帧的运动方程计算得的理论坐标，/>为特征点在第j帧观测方程求得的实际坐标，目标函数通过调整优化变量，使得理论坐标逐渐靠近实际坐标，从而获得最优的优化变量。
最后，将预积分测量残差和视觉重投影残差进行紧耦合优化，构建目标函数：
本发明使用Levenberg-Marquadt算法对X进行优化，获得较为精准的位姿状态。
步骤(7)：使用MyntEye-D相机在移动机器人平台上的算法测试。
使用kalibr对相机和IMU进行标定，得到如下表1的参数：
表1
在TurtleBoot2移动机器人平台上进行测试，首先将MyntEye-D相机固定在机器人中心，MyntEye-D相机中已经集成了双目相机与IMU模块。接着将TurtleBoot2的串口控制线连接至笔记本USB口上，MyntEye-D也与笔记本USB口连接，其中TurtleBoot2移动机器人平台由自身的电池供电，MyntEye-D相机通过USB由笔记本供电。在笔记本上开启TurtleBoot2遥控程序与MyntEye-D相机定位程序两个独立的线程，使移动机器人在实验室内移动一周，轨迹呈矩形。同时将定位算法计算得到的位姿保存成TUM数据集的格式(三维坐标+四元数)，最终将保存的数据在EVO评估工具上绘制出机器人的运动轨迹，如图4所示。办公室中有许多人和设备，属于动态环境，机器人的运动没有发生偏移且运行过程中基本识别到了人物，验证了本发明的可行性和鲁棒性。
如图5所示，在在另一个实施例中，提供了一种基于实例分割与多传感器融合的移动机器人定位方法，包括分割模块、特征点的提取和追踪模块、第一位姿求解模块、第二位姿求解模块、选择模块以及优化模块；
所述分割模块，用于对相机图像进行目标实例分割，得到分割掩膜；
所述特征点的提取和追踪模块，用于根据语义标签和分割掩膜，对目标实例分隔后的静态物体部分进行特征点的提取和追踪；
所述第一位姿求解模块，用于基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
所述第二位姿求解模块，用于建立IMU测量模型，使用预积分的方式进行位姿解算；
所述选择模块，用于根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
所述优化模块，用于构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
在此需要说明的是，上述实施例提供的系统仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能，该系统是应用于上述实施例的基于实例分割与多传感器融合的移动机器人定位方法。
如图6所示，在本申请的另一个实施例中，还提供了一种存储介质，存储有程序，所述程序被处理器执行时，实现基于实例分割与多传感器融合的移动机器人定位方法，具体为：
对相机图像进行目标实例分割，得到分割掩膜；
根据语义标签和分割掩膜，对目标实例分隔后的静态物体部分进行特征点的提取和追踪；
基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
建立IMU测量模型，使用预积分的方式进行位姿解算；
根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (8)
1.基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，包括下述步骤：
对相机图像进行目标实例分割，得到分割掩膜；
根据语义标签和分割掩膜，对目标实例分割后的静态物体部分进行特征点的提取和追踪；
基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
建立IMU测量模型，使用预积分的方式进行位姿解算；所述IMU测量模型，具体为：
其中，为机器人运动过程中加速度计和陀螺仪传感器上的测量值，at，ωt为机器人运动的实际加速度和角速度值，na，nω分别为两种传感器测量的随机噪声，/>为t时刻两个传感器测量的偏差噪声，/>为t时刻IMU在世界坐标系下的姿态，gw表示重力向量；
所述使用预积分的方式进行位姿解算，具体为：
其中：
分别为位置、速度和姿态的预积分值，/>表示第i-1帧时刻世界坐标系转化为机器人坐标系的旋转矩阵，/>和/>分别表示机器人在t时刻相对于第i-1帧时刻的旋转变化的旋转矩阵形式和四元数形式，/> 表示第i帧时刻的机器人相对于世界坐标的位置与姿态的四元数表示，/>表示第i帧时刻的速度，同理，/>表示第i-1时刻，Δt表示IMU数据之间的时间间隔；通过IMU测量获得机器人当前时刻的位置，姿态和速度；
根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
2.根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，采用Mask R-CNN算法对相机图像进行目标实例分割，将相机图像中的背景、人物、动物和车辆分割开，并获取对应的语义标签，根据语义标签信息，将常识中被认为可移动的物体上的像素标记为1，不可移动的物体上的像素标记为0，得到由0、1组成的分割掩膜。
3.根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，所述对目标实例分隔后的静态物体部分进行特征点的提取和追踪，具体为：
根据得到的分割掩膜，在标记为0的像素区域提取FAST角点作为特征点，在往后图像帧和左右图像帧中使用KLT稀疏光流算法对特征点进行追踪，追踪到的特征点和原先的特征点标记为同一点。
4.根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，使用MyntEye-D相机，根据其成像原理构建针孔相机的立体视觉模型，数学转化关系如下：
其中，u和v分别为特征点在图像上的横纵坐标系，X,Y,Z为特征点在相机坐标系下的三维坐标，fx，fy为相机的横向焦距和纵向焦距，u0，v0为相机中心点的坐标参数；
基于针孔相机立体视觉模型，根据同一个特征点在左右相机图像上的视差计算出该点的深度信息，其计算公式为：
d＝uL-uR
其中，b为左右相机光心的距离，d为视差，uL和uR分别为特征点在左相机和右相机上投影的横坐标，fx为横向焦距；
已知前一帧的特征点在相机坐标系下的三维坐标以及相机的位姿，使用坐标系转化公式获取特征点在世界坐标系下的三维坐标，基于特征点的三维坐标和当前帧的投影坐标使用EPnP算法求解出当前帧的相机位姿。
5.根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，选取位姿初值的更新方式具体为：
根据相机图像的状态进行选择，提取当前帧图像的像素平均灰度值I、动态物体区域占完整图像的比率r、特征点提取数目N和平均深度d，若同时满足50＞I＞220，r＜50％，N＞50，d＜20，则认为图像状态良好，适宜相机进行初值更新，故选择使用EPnP算法对当前相机的位姿进行求解，反之如有其中一个条件不满足，则使用预积分的方式进行位姿解算。
6.根据权利要求1所述基于实例分割与多传感器融合的移动机器人定位方法，其特征在于，所述非线性优化具体为：
需要优化的变量包括不同时刻关键帧的位置姿态/>速度/>加速度计偏差和陀螺仪偏差/>
χ＝[x0,x1,…,xn]
其中，χ和xi为这些优化变量的集合，n为选取参与优化的帧数量，i表示每一帧图像在这组图像帧中的序号；
IMU预积分的测量残差定义为：
视觉重投影测量残差定义为：
其中，和/>分别为特征点在第j帧相机坐标系下的齐次三维坐标的实际值和投影值，Tji表示i帧和j帧之间的变换矩阵，Rji和tji表示旋转矩阵和平移量；
将IMU预积分测量残差和视觉重投影残差进行紧耦合优化，构建目标函数：
使用Levenberg–Marquadt算法对X优化，求解最优的机器人位姿。
7.基于实例分割与多传感器融合的移动机器人定位系统，其特征在于，应用于权利要求1-6中任一项所述的基于实例分割与多传感器融合的移动机器人定位方法，包括分割模块、特征点的提取和追踪模块、第一位姿求解模块、第二位姿求解模块、选择模块以及优化模块；
所述分割模块，用于对相机图像进行目标实例分割，得到分割掩膜；
所述特征点的提取和追踪模块，用于根据语义标签和分割掩膜，对目标实例分隔后的静态物体部分进行特征点的提取和追踪；
所述第一位姿求解模块，用于基于针孔相机的立体视觉模型，由同一特征点在左右相机中的投影视差计算出该点的深度值，从而获取特征点的三维坐标，根据特征点在相机上的投影坐标和在世界坐标下的三维坐标，使用EPnP算法对当前相机的位姿进行求解；
所述第二位姿求解模块，用于建立IMU测量模型，使用预积分的方式进行位姿解算；
所述选择模块，用于根据图像亮度、动态物体覆盖情况、特征点提取情况和平均深度，选取位姿初值的更新方式；
所述优化模块，用于构建IMU预积分的测量残差、视觉重投影残差，进行紧耦合的非线性优化，获取更加准确的位姿状态。
8.一种存储介质，存储有程序，其特征在于，所述程序被处理器执行时，实现权利要求1-6任一项所述的基于实例分割与多传感器融合的移动机器人定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011393477.2A CN112734765B (zh) | 2020-12-03 | 2020-12-03 | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011393477.2A CN112734765B (zh) | 2020-12-03 | 2020-12-03 | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112734765A CN112734765A (zh) | 2021-04-30 |
| CN112734765B true CN112734765B (zh) | 2023-08-22 |
Family
ID=75598408
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011393477.2A Active CN112734765B (zh) | 2020-12-03 | 2020-12-03 | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112734765B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113240696B (zh) * | 2021-05-20 | 2022-02-08 | 推想医疗科技股份有限公司 | 图像处理方法及装置，模型的训练方法及装置，电子设备 |
| CN113506346B (zh) * | 2021-07-06 | 2023-08-29 | 广东工业大学 | 一种应用于弱纹理大旋转场景下的相机的位姿求解方法 |
| CN113470121B (zh) * | 2021-09-06 | 2021-12-28 | 深圳市普渡科技有限公司 | 自主移动平台、外参优化方法、装置及存储介质 |
| CN114549549B (zh) * | 2022-02-10 | 2024-04-02 | 北京工业大学 | 一种动态环境下基于实例分割的动态目标建模跟踪方法 |
| CN115468560B (zh) * | 2022-11-03 | 2023-03-24 | 国网浙江省电力有限公司宁波供电公司 | 基于多传感信息融合的质检方法、机器人、设备及介质 |
| CN116442248A (zh) * | 2023-06-19 | 2023-07-18 | 山东工程职业技术大学 | 一种基于目标检测的机器人视觉定位模块及方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN111156998A (zh) * | 2019-12-26 | 2020-05-15 | 华南理工大学 | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 |
| CN111354043A (zh) * | 2020-02-21 | 2020-06-30 | 集美大学 | 一种基于多传感器融合的三维姿态估计方法及装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107747941B (zh) * | 2017-09-29 | 2020-05-15 | 歌尔股份有限公司 | 一种双目视觉定位方法、装置及系统 |
-
2020
- 2020-12-03 CN CN202011393477.2A patent/CN112734765B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN111156998A (zh) * | 2019-12-26 | 2020-05-15 | 华南理工大学 | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 |
| CN111354043A (zh) * | 2020-02-21 | 2020-06-30 | 集美大学 | 一种基于多传感器融合的三维姿态估计方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| 基于Vision-IMU的机器人同时定位与地图创建算法;姚二亮;张合新;张国良;徐慧;赵欣;;仪器仪表学报(04);第233-241页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112734765A (zh) | 2021-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112734765B (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
| CN110956651B (zh) | 一种基于视觉和振动触觉融合的地形语义感知方法 | |
| Heng et al. | Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system | |
| US10192113B1 (en) | Quadocular sensor design in autonomous platforms | |
| CN109509230B (zh) | 一种应用于多镜头组合式全景相机的slam方法 | |
| CN110497901B (zh) | 一种基于机器人vslam技术的泊车位自动搜索方法和系统 | |
| US10496104B1 (en) | Positional awareness with quadocular sensor in autonomous platforms | |
| Levinson et al. | Traffic light mapping, localization, and state detection for autonomous vehicles | |
| Senlet et al. | Satellite image based precise robot localization on sidewalks | |
| CN111862673A (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN115115859A (zh) | 基于无人机航拍的长线性工程施工进度智能识别与分析方法 | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| Jun et al. | Autonomous driving system design for formula student driverless racecar | |
| Mueller et al. | Continuous stereo camera calibration in urban scenarios | |
| CN115900710A (zh) | 基于视觉信息的动态环境导航方法 | |
| CN117152249A (zh) | 基于语义一致性的多无人机协同建图与感知方法及系统 | |
| Manivannan et al. | Vision based intelligent vehicle steering control using single camera for automated highway system | |
| Akopov et al. | Choosing a Camera for 3D Mapping | |
| CN113701750A (zh) | 一种井下多传感器的融合定位系统 | |
| CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN112907625A (zh) | 应用于四足仿生机器人的目标跟随方法及系统 | |
| Mueller et al. | Continuous stereo self-calibration on planar roads | |
| Wang | Autonomous mobile robot visual SLAM based on improved CNN method |
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 |