CN112880687B - 一种室内定位方法、装置、设备和计算机可读存储介质 - Google Patents
一种室内定位方法、装置、设备和计算机可读存储介质 Download PDFInfo
- Publication number
- CN112880687B CN112880687B CN202110084517.3A CN202110084517A CN112880687B CN 112880687 B CN112880687 B CN 112880687B CN 202110084517 A CN202110084517 A CN 202110084517A CN 112880687 B CN112880687 B CN 112880687B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- image
- data
- pose
- acquired
- 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
- 238000000034 method Methods 0.000 title claims abstract description 76
- 238000003860 storage Methods 0.000 title claims abstract description 13
- 239000013598 vector Substances 0.000 claims abstract description 14
- 230000007613 environmental effect Effects 0.000 claims description 48
- 238000000605 extraction Methods 0.000 claims description 35
- 230000003287 optical effect Effects 0.000 claims description 28
- 238000005070 sampling Methods 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 20
- 238000005457 optimization Methods 0.000 claims description 19
- 238000012545 processing Methods 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 13
- 230000010354 integration Effects 0.000 claims description 12
- 230000006870 function Effects 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 4
- 239000000284 extract Substances 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims 1
- 230000000007 visual effect Effects 0.000 description 13
- 230000008569 process Effects 0.000 description 9
- 230000008859 change Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000009826 distribution Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 239000008186 active pharmaceutical agent Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000012952 Resampling Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
本申请涉及机器人领域，提供了室内定位方法、装置、设备和计算机可读存储介质，以相对低廉的成本和较好的鲁棒性，实现在室内的精确定位。所该方法包括：获取环境物图像；提取环境物图像的点、线和边缘特征，建立图像的帧间数据关联；对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐；对数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿；根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差；通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。
Description
技术领域
本发明涉及机器人领域，特别涉及一种室内定位方法、装置、设备和计算机可读存储介质。
背景技术
随着近年来机器人技术的发展，作为一项核心技术，移动机器人定位的相关学术研究与实践落地也得到了广泛的研究与挖掘，其中，基于视觉的室内定位技术取得了较大的进展。视觉定位技术由于其相机传感器采集的数据信息量大，可以较为鲁棒地(robustly)完成感知、重定位等工作需求，因而迅速得到学术界和产业界的广泛研究与运用。
然而，上述基于视觉的室内定位技术其鲁棒性也是相对的，原因在于视觉定位天然地容易受到光照变化、运动模糊和环境物遮挡等不可抗拒因素的影响。因此，尽管具有成本上的优势，基于纯视觉的定位方案仍然无法达到激光雷达定位方案的鲁棒性。
发明内容
本申请提供一种室内定位方法、装置、设备和计算机可读存储介质，以相对低廉的成本和较好的鲁棒性，实现在室内的精确定位。
一方面，本申请提供了一种室内定位方法，包括：
获取图像采集模块采集环境物所得到的环境物图像，所述环境物为移动机器人所处环境中的任何物体；
提取所述环境物图像的点、线和边缘特征，建立所述环境物图像的帧间数据关联；
对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐；
对所述数据对齐后的移动机器人位姿进行预积分，获取所述移动机器人的预估位姿；
根据所述帧间数据关联和所述预估位姿，计算滑动窗口中的约束残差；
通过调节状态向量降低所述滑动窗口中的约束残差，直至所述约束残差最小时输出所述移动机器人的六自由度姿态数据。
另一方面，本申请提供了一种室内定位装置，包括：
图像获取模块，用于获取图像采集模块采集环境物所得到的环境物图像，所述环境物为移动机器人所处环境中的任何物体；
特征提取模块，用于提取所述环境物图像的点、线和边缘特征，建立所述环境物图像的帧间数据关联；
传感器数据处理模块，用于对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐；
预积分模块，用于对所述数据对齐后的移动机器人位姿进行预积分，获取所述移动机器人的预估位姿；
约束残差计算模块，用于根据所述帧间数据关联和所述预估位姿，计算滑动窗口中的约束残差；
优化模块，用于通过调节状态向量降低所述滑动窗口中的约束残差，直至所述约束残差最小时输出所述移动机器人的六自由度姿态数据。
第三方面，本申请提供了一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述室内定位方法的技术方案的步骤。
第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述室内定位方法的技术方案的步骤。
从上述本申请提供的技术方案可知，一方面，针对室内环境存在纹理强弱多变的特点，通过提取环境物图像的点、线和边缘特征，将根据环境物图像的帧间数据关联获取的移动机器人位置作为图优化模块的优化对象，提高了室内弱纹理环境下移动机器人视觉自主定位的鲁棒性；另一方面，针对视觉定位无尺度、快速运动响应差、无纹理区域定位失效的问题，通过对多个传感器之间获取的移动机器人位姿进行数据对齐和预积分，获取移动机器人的预估位姿，将预估位姿作为图优化模块的优化对象，融合了视觉和多传感器的定位算法，提高了视觉自主定位的稳定性和准确性。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的室内定位方法的流程图；
图2是本申请实施例提供的数据对齐示意图；
图3是本申请实施例提供的融合定位过程中以图论方式表示的因子图；
图4是本申请实施例提供的室内定位装置的结构示意图；
图5是本申请实施例提供的设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本说明书中，诸如第一和第二这样的形容词仅可以用于将一个元素或动作与另一元素或动作进行区分，而不必要求或暗示任何实际的这种关系或顺序。在环境允许的情况下，参照元素或部件或步骤(等)不应解释为局限于仅元素、部件、或步骤中的一个，而可以是元素、部件、或步骤中的一个或多个等。
在本说明书中，为了便于描述，附图中所示的各个部分的尺寸并不是按照实际的比例关系绘制的。
本申请提出了一种室内定位方法，可应用于移动机器人，该移动机器人可以是在餐厅作业的机器人，例如，传菜机器人，也可以是在医疗场所，例如医院作业的送药机器人，还可以是在仓库等场所作业的搬运机器人，等等。如附图1所示，室内定位方法主要包括步骤S101至S106，详述如下：
步骤S101：获取图像采集模块采集环境物所得到的环境物图像，其中，环境物为移动机器人所处环境中的任何物体。
在本申请实施例中，环境是指机器人工作的环境，环境物是指机器人工作的环境中的一切物体，包括在环境中设置的路标以及其他物体，例如，某种货物、一棵树、一堵墙或一张桌子，等等，而图像采集模块可以是可以采集图像的设备，例如单目或双目相机等。
步骤S102：提取环境物图像的点、线和边缘特征，建立环境物图像的帧间数据关联。
在本申请实施例中，所谓建立环境物图像的帧间数据关联，指根据环境物的当前帧图像的像素点，确定出该像素点在下一帧图像中的位置或者确定出在下一帧图像中该像素点的关联像素点，目的是实现对当前帧图像的像素点进行有效跟踪。换言之，一旦建立了环境物图像的帧间数据关联，意味着两帧环境物图像之间的某两个像素点是存在对应关系的像素点。
作为本申请一个实施例，提取环境物图像的点、线和边缘特征，建立环境物图像的帧间数据关联可通过步骤S1021至步骤S1024实现，说明如下：
步骤S1021：将环境物图像转化为灰度图像。
考虑到后续主要是通过光流法建立点特征的帧间数据关联、线特征的帧间数据关联和边缘特征的帧间数据关联，而光流法(Optical Flow)的一个基本假设灰度不变假设，即，同一个空间点的像素灰度值，在各个图像中是固定不变的。因此，在本申请实施例中，需要将环境物图像转化为灰度图像。至于具体转化方法，可以是现有技术中的任何一种方法，本申请对此不做限制。
步骤S1022：在保存灰度图像时，对灰度图像进行去畸变处理，得到去畸变灰度图像。
由于图像采集模块(例如单目或双目相机)本身镜头和工艺的原因或者由于拍摄视角的选择存在问题，造成图像采集模块采集环境物得到的环境物图像产生了畸变，例如枕形畸变和桶形畸变等，例如，拍摄建筑物或树木时，若将相机稍微调整拍摄角度，虽然可以在距离建筑物或树木很近的情况也能将整个结构拍摄下来，然而，建筑物或树木平行的线条显得并不平行了。至于具体的去畸变方法，可以是软件方法，例如，可以直接调用图形处理库中封装的去畸变应用程序编程接口(Application Programming Interface，API)，执行该API即可对灰度图像进行去畸变。从上述分析可知，由于图像的畸变主要是指线的畸变，该畸变并不影响对图像进行点特征的提取或边缘特征的提取，因此，对图像进行点特征的提取或边缘特征的提取可以直接在原图像上进行。然而，对图像进行线特征的提取必须在去畸变后的图像上进行，因此，需要对灰度图像进行去畸变处理，得到去畸变灰度图像，同时，可以对灰度图像进行保存，后续进行点特征或边缘特征的提取时，可直接在灰度图像上进行。
步骤S1023：对灰度图像分别进行点特征的提取和边缘特征的提取，通过光流法建立点特征的帧间数据关联和边缘特征的帧间数据关联。
在本申请实施例中，点特征的提取也称为提取特征点，所谓特征点，是一帧图像帧具有鲜明特性并且能够有效反映物体本质特征、表示图像中对应目标部位的像素点，或者，特征点是在一帧图像中能够表示出需要进行位置跟踪的目标物体的各个关键部位的关键特征点。由于图像的边缘是图像最基本的特征，边缘是一个区域和另一个区域的交界处，是区域属性发生突变、不确定性最大的地方，同时也是图像信息最集中的地方(图像的边缘通常包含着丰富的信息)，因此，边缘特征是很好的一种特征表示，本申请实施例也会对灰度图像进行边缘特征的提取。
对于点特征，可以直接对灰度图像进行点特征的提取，然后，通过光流法建立点特征的帧间数据关联，而边缘特征的帧间数据关联的建立是通过如下步骤S1至步骤S4建立：
步骤S1：将灰度图像栅格化，得到灰度图像的若干栅格图像。
在本申请实施例中，将灰度图像栅格化可以采用现有技术，此处不做赘述。
步骤S2：采用canny边缘特征提取算法，对栅格图像进行边缘特征的提取。
具体而言，采用canny边缘特征提取算法，对栅格图像进行边缘特征的提取可以是：使用高斯滤波器对栅格图像进行平滑处理以滤除噪声；计算滤除噪声后栅格图像中每个像素点的梯度强度和方向；采用非极大值抑制方式消除杂散响应；应用双阈值检测的方式来确定经上述处理后的栅格图像中真实和潜在边缘；通过抑制孤立的弱边缘完成栅格图像边缘特征的提取。
步骤S3：若栅格图像中不存在点特征，则在不存在点特征的栅格图像提取到边缘特征时，从提取的边缘特征采样梯度最大的点作为不存在点特征的栅格图像的点特征。
需要说明的是，经过上述实施例的步骤S1和步骤S2，若能够从栅格图像提取到边缘特征，则canny边缘特征提取算法的流程结束，提取到灰度图像的边缘特征。若栅格图像中不存在点特征，则针对不存在点特征的栅格图像进行边缘特征的提取。若在不存在点特征的栅格图像提取到边缘特征，则从提取的边缘特征采样梯度最大的点作为不存在点特征的栅格图像的点特征。
步骤S4：根据从边缘特征提取的点特征，通过光流法建立点特征的帧间数据关联作为边缘特征的帧间数据关联。
步骤S1024：对去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联。
具体地，对去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联可通过如下步骤S’1至步骤S’3实现：
步骤S’1：对去畸变灰度图像进行线特征提取，得到去畸变灰度图像的初次线特征。
在本申请实施例中，对去畸变灰度图像进行线特征提取，得到去畸变灰度图像的初次线特征可以采用线段检测(Line Segment Detection，LSD)线特征算法实现，具体可以是基于LSD线特征提取算法提取线特征，基于线段条带(Line Band Descriptor，LSD)描述符算法计算线特征描述子，对所提取到的线特征，进行断线合并操作。
步骤S’2：采样初次线特征上的若干像素点，将采样所得像素点的集合作为去畸变灰度图像的线特征。
例如，对于经步骤S’1提取的10个初次线特征(这10个初次线特征均是从同一条直线上提取)，可以在每个初次线特征上采样一个点特征，将这10个点特征的集合作为去畸变灰度图像的线特征。
步骤S’3：基于共线约束，通过光流法建立线特征的帧间数据关联。
对于点特征或者边缘特征，通过光流法建立点特征的帧间数据关联可以直接跟踪点特征(边缘特征最后也是转化为点特征的提取)，不会存在误差。然而，对于线特征，其是在线特征上再次进行了采样，将再次采样所得像素点的集合作为去畸变灰度图像的线特征，因此，直接通过光流法建立线特征的帧间数据关联可能会产生误差，原因在于，在上一帧图像中从线特征采样的若干特征点，在下一帧可图像中可能不在同一条直线上。因此，需要进行共线约束，即，将对从初次线特征上采样的若干像素点调整为一条直线上，然后，再通过光流法建立线特征的帧间数据关联。
需要说明的是，尽管上述实施例是通过对环境物图像的点、线和边缘特征的提取，建立图像的帧间数据关联，然而，对于线特征和边缘特征的提取，本质上仍然是点特征的提取，因此，前述实施例中，对去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联以及基于共线约束，通过光流法建立线特征的帧间数据关联，本质上都是通过光流法建立点特征的帧间数据关联，而光流法的基本思想是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性，寻找上一帧与当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。因此，只要在提取了灰度图像或去畸变灰度图像的线特征或边缘特征后，可以比较容易地通过光流法建立图像的帧间数据关联。
步骤S103：对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐。
在本申请实施例中，由于不同的传感器，即使硬件同步以后也存在采样频率不一致的差异，因此，在多传感器融合定位中必然存在传感器获取的数据在时间戳上不同步的问题。以多传感器包含至少一个陀螺仪和至少一个轮式里程计为例，如图2所示，假设陀螺仪在ti时刻获取的移动机器人姿态数据为Dti，理想状态下是轮式里程计也能够在ti时刻获取移动机器人位置数据，然而，由于陀螺仪的采样频率与轮式里程计的采样频率不一致等原因，轮式里程计只能在t'li时刻获取到移动机器人位置数据D'li，即传感器之间数据没有对齐；对于图像采集模块采集的环境物图像数据，也同样存在数据不对齐的情形，即由于图像采集模块的采样频率与陀螺仪的采样频率不一致等原因，陀螺仪在ti时刻获取移动机器人姿态数据为Dti时，图像采集模块只能在t'xi时刻获取到图像数据D'xi。对于上述情形，需要采用数据对齐的方案。
在本申请一个实施例中，对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐可以是：使用与环境物的当前帧图像数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据对移动机器人姿态数据进行插值，以使插值后所得移动机器人姿态数据与环境物的当前帧图像数据对齐；以及使用与环境物的当前帧图像数据前后相邻时间戳轮式里程计获取的移动机器人位置数据对移动机器人位置数据进行插值，以使插值后所得移动机器人位置数据与环境物的当前帧图像数据对齐。仍然以图2为例，使用与环境物的当前帧图像数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据，即，使用ti-1时刻移动机器人姿态数据D'ti-1与ti时刻移动机器人姿态数据Dti对移动机器人姿态数据进行插值，得到t'xi时刻的移动机器人姿态插值数据从图2可以看出，经过上述插值操作，移动机器人姿态插值数据/>与环境物的当前帧图像数据D'xi已经对齐。同样地，使用与环境物的当前帧图像数据前后相邻时间戳轮式里程计获取的移动机器人位置数据，即，使用t'li时刻移动机器人位置数据D'li与t'li+1时刻移动机器人位置数据D'li+1对移动机器人位置数据进行插值，得到t'xi时刻的移动机器人位置插值数据/>从图2可以看出，经过上述插值操作，移动机器人位置插值数据与环境物的当前帧图像数据D'xi已经对齐。
在本申请另一实施例中，对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐可以是：采用陀螺仪获取的移动机器人姿态数据和轮式里程计获取的移动机器人位置数据互相插值的方式，以使至少一个陀螺仪获取的移动机器人姿态数据与至少一个轮式里程计获取的移动机器人位置数据对齐，得到多个传感器的移动机器人对齐位姿数据；使用与环境物的当前帧图像数据前后相邻时间戳移动机器人对齐位姿数据对移动机器人对齐位姿数据进行插值，以使插值后所得移动机器人位姿数据与环境物的当前帧图像数据对齐。本实施例中，使用与环境物的当前帧图像数据前后相邻时间戳移动机器人对齐位姿数据对移动机器人对齐位姿数据进行插值，以使插值后所得移动机器人位姿数据与环境物的当前帧图像数据对齐，其原理与图2实施例中使用与环境物的当前帧图像数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据对移动机器人姿态数据进行插值，以使插值后所得移动机器人姿态数据与环境物的当前帧图像数据对齐；或者其原理与图2实施例中使用与环境物的当前帧图像数据前后相邻时间戳轮式里程计获取的移动机器人位置数据对移动机器人位置数据进行插值，以使插值后所得移动机器人位置数据与环境物的当前帧图像数据对齐的原理相同，可参阅前述实施例的相关说明，此处不做赘述。需要说明的是，本实施例中，所谓采用陀螺仪获取的移动机器人姿态数据和轮式里程计获取的移动机器人位置数据互相插值的方式，指的是可以使用与移动机器人姿态当前数据前后相邻时间戳轮式里程计获取的移动机器人位置数据，对轮式里程计获取的移动机器人位置数据进行插值，以使插值后所得移动机器人位置数据与陀螺仪获取的移动机器人姿态当前数据对齐，或者，也可以使用与移动机器人位置当前数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据，对陀螺仪获取的移动机器人姿态数据进行插值，以使插值后所得移动机器人姿态数据与轮式里程计获取的移动机器人位置当前数据对齐。
步骤S104：对经步骤S103数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿。
在本申请实施例中，移动机器人位姿包括移动机器人在世界坐标系下的位置和姿态，其中，移动机器人在世界坐标系下的位置可由轮式里程计得到，移动机器人在世界坐标系下的姿态可由陀螺仪得到。作为本申请一个实施例，对经步骤S103数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿可通过步骤S1031至步骤S1034实现，说明如下：
步骤S1031：基于轮式里程计测量得到的脉冲信号，获取理想状态下在图像采样模块采集一帧环境物图像的时段内轮式里程计的左轮滚过的距离和右轮滚过的距离/>
由于已知轮式里程计左轮或右轮转动一圈发出的脉冲数，因此，基于轮式里程计测量得到的脉冲信号和轮式里程计左轮或右轮的直径，可以获取在图像采样模块采集一帧环境物图像的时段内轮式里程计的左轮滚过的距离Dl和右轮滚过的距离Dr。假定轮式里程计左右两个编码器各受到一个高斯噪声ηed的干扰，则：
步骤S1032：计算将/>作为ti+1时刻移动机器人的位置，其中，/> 为将轮式里程计坐标系转换为世界坐标系的转换矩阵，为ti时刻移动机器人的位置。
在本申请实施例中，按照计算出来的/>也是对ti+1时刻移动机器人位置的估计，即，/>亦为ti+1时刻移动机器人的预估位置。
步骤S1033：获取理想状态下陀螺仪在陀螺仪坐标系下的角速度值其中，wb为陀螺仪在陀螺仪坐标系下的角速度测量值，bg为陀螺仪的角速度偏置，ng为噪声。
步骤S1034：计算将/>作为ti+1时刻移动机器人的姿态，/>为ti时刻移动机器人的姿态，qbo为陀螺仪坐标系和轮式里程计坐标系之间的外参，dt是ti+1时刻与ti时刻之间的时长。
公式的意义在于：按照/>可在陀螺仪坐标系下对陀螺仪数据进行预积分得到两帧之间的角速度变化信息，再将这个角速度变化信息转换到轮式里程计坐标系下，得到ti+1时刻的移动机器人的预估姿态/>
步骤S105：根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差。
经前述实施例的步骤S1031至步骤S1034，得到了ti+1时刻移动机器人的预估位置和移动机器人的预估姿态/>即移动机器人的预估位姿。基于前述实施例得到的帧间数据关联和移动机器人的预估位姿，本申请实施例可采用滑动窗口的方式来进行传感器之间的紧融合，其中，滑动窗口的大小一般设置为10帧，当接收到的数据帧超过10帧以后，每新来一帧数据则从滑窗中边缘化掉一帧，边缘化的过程就是将一个联合概率分布转换成一个边际概率和一个条件概率分布的过程；边缘化会产生一个先验残差来约束滑动窗中状态量的优化，此处，状态量X记为X＝[p，q，bg，pbc，qbc，λ]T，其中，p为移动机器人的位置，q为移动机器人的姿态，bg为陀螺仪角速度偏置，λ为视觉中地图点的逆深度，pbc和qbc为图像采样模块和陀螺仪之间的外参。作为本申请一个实施例，根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差可通过步骤S1051至步骤S1053实现，说明如下：
步骤S1051：根据帧间数据关联，获取图像采集模块的重投影误差rc1。
如前所述，帧间数据关联描述了一个特征点在两帧环境物图像之间的对应关系。理想状态下，在归一化图像采集模块(例如相机)坐标系下，一个特征点的位置观测值和估计值应该相同，然而，由于移动机器人的移动等因素，造成相机的位姿不会总是那么理想，因此，特征点p的观测值(即观测到的投影位置)与该特征点p按照当前相机的位姿进行投影得到的位置相比，两者之间总是存在一定的误差，这个图像采集模块的重投影误差rc1。从上述分析可知，图像采集模块的重投影误差rc1表面看是相机的位姿不理想造成，然后，相机等图像采集模块是搭载在移动机器人上的设备，因此，通过不断校正移动机器人的位姿，则能使得重投影误差rc1减小到最小或者预设值。换言之，当重投影误差rc1减小到最小或者预设值时，在视觉定位这一环节，移动机器人的位姿其预估值与观测值之间的差值也最小。
步骤S1052：根据移动机器人的预估位姿，获取多个传感器之间获取的移动机器人帧间位姿残差ro。
如前所述，经前述实施例的步骤S1031至步骤S1034，得到了ti+1时刻移动机器人的预估位置和移动机器人的预估姿态/>即移动机器人的预估位姿。因此，将移动机器人的观测位姿与预估位姿相减，即得到多个传感器之间获取的移动机器人帧间位姿残差ro。
步骤S1053：计算将rtotal作为滑动窗口中的约束残差，为rp边缘化留下的先验残差，ρ为鲁棒核函数，rplane为平面运动约束残差。
在本申请实施例中，计算的过程，是融合通过图像采集模块得到的移动机器人位姿和通过多个传感器获取的移动机器人位姿的过程，其可使用图3示例的因子图来表示，其使用图论的方法形象地表示了在融合过程中，各个因子、地图点、相机(图像采集模块)位姿和陀螺仪偏置等之间的关系。由于因子图中涉及平面约束因子，为了更好地理解因子图，以下对平面运动约束进行补充说明。
在实际室内的工作环境下，移动机器人大多数时间的运动是符合平面运动约束的，即，移动机器人在移动过程中，其姿态应该满足俯仰角pitch和横滚角roll的变化量为0，其位置变化中，沿着z轴的移动量为0。在实际的室内视觉定位系统中，基本都不考虑平面运动约束的存在。然而，结合室内定位方法移动机器人定位场景的特点，本申请实施例的定位系统中添加了平面运动约束，即，平面运动约束残差rplane的推导如下：
已知世界坐标系下两帧数据之间的姿态变化量R为：
因此，由上式可知平面运动约束姿态残差rroll&pitch和平面运动约束位置残差rz为：
其中，e1＝[1 0 0]T，e2＝[0 1 0]T，e3＝[0 0 1]T。
上述R、rroll&pitch和rz的表达式中，表示轮式里程计坐标系到世界坐标系之间的转换矩阵；平面运动约束姿态残差rroll&pitch和平面运动约束位置残差rz统称为平面运动约束残差rplane。
从附图3示例的因子图和滑动窗口中的约束残差rtotal可知，整个滑动窗口中存在五种约束即五种参数，其总共关联了五种状态向量即移动机器人的位置p、移动机器人的姿态q、陀螺仪角速度偏置bg、视觉中地图点的逆深度λ以及pbc和qbc为图像采样模块和陀螺仪之间的外参。
步骤S106：通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。
具体可以利用高斯牛顿法，通过图优化的方式调节移动机器人的位置p、移动机器人的姿态q、陀螺仪角速度偏置bg、视觉中地图点的逆深度λ以及pbc和qbc为图像采样模块和陀螺仪之间的外参等状态向量，同时降低rtotal、rp和rplane等残差，直至滑动窗口中的约束残差rtotal最小时输出移动机器人的六自由度姿态数据。需要说明的是，滑动窗口中的约束残差rtotal最小，并不意味着rtotal为0，而是只要rtotal达到预设值即可。
从上述附图1示例的室内定位方法可知，一方面，针对室内环境存在纹理强弱多变的特点，通过提取环境物图像的点、线和边缘特征，将根据环境物图像的帧间数据关联获取的移动机器人位置作为图优化模块的优化对象，提高了室内弱纹理环境下移动机器人视觉自主定位的鲁棒性；另一方面，针对视觉定位无尺度、快速运动响应差、无纹理区域定位失效的问题，通过对多个传感器之间获取的移动机器人位姿进行数据对齐和预积分，获取移动机器人的预估位姿，将预估位姿作为图优化模块的优化对象，融合了视觉和多传感器的定位算法，提高了视觉自主定位的稳定性和准确性。
请参阅附图4，是本申请实施例提供的一种室内定位装置，可以包括图像获取模块401、特征提取模块402、传感器数据处理模块403、预积分模块404、约束残差计算模块405和优化模块406，详述如下：
图像获取模块401，用于获取图像采集模块采集环境物所得到的环境物图像，其中，环境物为移动机器人所处环境中的任何物体；
特征提取模块402，用于提取环境物图像的点、线和边缘特征，建立图像的帧间数据关联；
传感器数据处理模块403，用于对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐；
预积分模块404，用于对数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿；
约束残差计算模块405，用于根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差；
优化模块406，用于通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。
可选地，附图4示例的特征提取模块402可以包括灰度化单元、去畸变单元、第一关联单元和第二关联单元，其中：
灰度化单元，用于将环境物图像转化为灰度图像；
去畸变单元，用于在保存灰度图像时，对灰度图像进行去畸变处理，得到去畸变灰度图像；
第一关联单元，用于对灰度图像分别进行点特征的提取和边缘特征的提取，通过光流法建立点特征的帧间数据关联和边缘特征的帧间数据关联；
第二关联单元，用于对去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联。
可选地，上述第一关联单元可包括栅格化单元、边缘特征提取单元、梯度求取单元和第三关联单元，其中：
栅格化单元，用于将灰度图像栅格化，得到灰度图像的若干栅格图像；
边缘特征提取单元，用于采用canny边缘特征提取算法，对栅格图像进行边缘特征的提取；
梯度求取单元，用于若栅格图像中不存在点特征，则在不存在点特征的栅格图像提取到边缘特征时，从提取的边缘特征采样梯度最大的点作为不存在点特征的栅格图像的点特征；
第三关联单元，用于根据提取的点特征，通过光流法建立点特征的帧间数据关联作为边缘特征的帧间数据关联。
可选地，上述第二关联单元可包括线特征提取单元、采样单元和第四关联单元，其中：
线特征提取单元，用于对去畸变灰度图像进行线特征提取，得到去畸变灰度图像的初次线特征；
采样单元，用于采样初次线特征上的若干像素点，将采样所得像素点的集合作为去畸变灰度图像的线特征；
第四关联单元，用于基于共线约束，通过光流法建立线特征的帧间数据关联。
可选地，附图4示例的装置中，多个传感器包括至少一个陀螺仪和至少一个轮式里程计，传感器数据处理模块403可以包括第一插值单元和第二插值单元，其中：
第一插值单元，用于使用与环境物的当前帧图像数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据对移动机器人姿态数据进行插值，以使插值后所得移动机器人姿态数据与环境物的当前帧图像数据对齐；
第二插值单元，用于使用与环境物的当前帧图像数据前后相邻时间戳轮式里程计获取的移动机器人位置数据对移动机器人位置数据进行插值，以使插值后所得移动机器人位置数据与环境物的当前帧图像数据对齐。
可选地，附图4示例的装置中，多个传感器包括至少一个陀螺仪和至少一个轮式里程计，传感器数据处理模块403可以包括第三插值单元和第四插值单元，其中：
第三插值单元，用于采用陀螺仪获取的移动机器人姿态数据和轮式里程计获取的移动机器人位置数据互相插值的方式，以使至少一个陀螺仪获取的移动机器人姿态数据与至少一个轮式里程计获取的移动机器人位置数据对齐，得到多个传感器的移动机器人对齐位姿数据；
第四插值单元，用于使用与环境物的当前帧图像数据前后相邻时间戳移动机器人对齐位姿数据对移动机器人对齐位姿数据进行插值，以使插值后所得移动机器人位姿数据与环境物的当前帧图像数据对齐。
可选地，附图4示例的装置中，多个传感器包括至少一个陀螺仪和至少一个轮式里程计，预积分模块404可以包括距离获取单元、第一计算单元、角速度获取单元和第二计算单元，其中：
距离获取单元，用于基于轮式里程计测量得到的脉冲信号，获取理想状态下在图像采样模块采集一帧环境物图像的时段内轮式里程计的左轮滚过的距离和右轮滚过的距离/>
第一计算单元，用于计算将/>作为ti+1时刻移动机器人的位置，其中，/> 为将轮式里程计坐标系转换为世界坐标系的转换矩阵，/>为ti时刻移动机器人的位置；
角速度获取单元，用于获取理想状态下陀螺仪在陀螺仪坐标系下的角速度值其中，wb为陀螺仪在陀螺仪坐标系下的角速度测量值，bg为陀螺仪的角速度偏置，ng为噪声；
第二计算单元，用于计算将/>作为ti+1时刻移动机器人的姿态，其中，/>为ti时刻移动机器人的姿态，qbo为陀螺仪坐标系和轮式里程计坐标系之间的外参，dt是ti+1时刻与ti时刻之间的时长。
可选地，附图4示例的装置中，多个传感器包括至少一个陀螺仪和至少一个轮式里程计，预积分模块404可以包括第一误差获取单元、第一残差获取单元、和第三计算单元，其中：
第一误差获取单元，用于根据帧间数据关联，获取图像采集模块的重投影误差rc1；
第一残差获取单元，用于根据移动机器人的预估位姿，获取多个传感器之间获取的移动机器人帧间位姿残差ro；
第三计算单元，用于计算将rtotal作为滑动窗口中的约束残差，其中，rp为边缘化留下的先验残差，ρ为鲁棒核函数，rplane为平面运动约束残差。
从以上技术方案的描述中可知，一方面，针对室内环境存在纹理强弱多变的特点，通过提取环境物图像的点、线和边缘特征，将根据环境物图像的帧间数据关联获取的移动机器人位置作为图优化模块的优化对象，提高了室内弱纹理环境下移动机器人视觉自主定位的鲁棒性；另一方面，针对视觉定位无尺度、快速运动响应差、无纹理区域定位失效的问题，通过对多个传感器之间获取的移动机器人位姿进行数据对齐和预积分，获取移动机器人的预估位姿，将预估位姿作为图优化模块的优化对象，融合了视觉和多传感器的定位算法，提高了视觉自主定位的稳定性和准确性。
请参阅图5，是本申请一实施例提供的设备的结构示意图。如图5所示，该实施例的设备5主要包括：处理器50、存储器51以及存储在存储器51中并可在处理器50上运行的计算机程序52，例如室内定位方法的程序。处理器50执行计算机程序52时实现上述室内定位方法实施例中的步骤，例如图1所示的步骤S101至S106。或者，处理器50执行计算机程序52时实现上述各装置实施例中各模块/单元的功能，例如图4所示图像获取模块401、特征提取模块402、传感器数据处理模块403、预积分模块404、约束残差计算模块405和优化模块406的功能。
示例性地，室内定位方法的计算机程序52主要包括：获取图像采集模块采集环境物所得到的环境物图像，其中，环境物为移动机器人所处环境中的任何物体；提取环境物图像的点、线和边缘特征，建立环境物图像的帧间数据关联；对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐；对数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿；根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差；通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。计算机程序52可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器51中，并由处理器50执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序52在设备5中的执行过程。例如，计算机程序52可以被分割成图像获取模块401、特征提取模块402、传感器数据处理模块403、预积分模块404、约束残差计算模块405和优化模块406(虚拟装置中的模块)的功能，各模块具体功能如下：图像获取模块401，用于获取图像采集模块采集环境物所得到的环境物图像，其中，环境物为移动机器人所处环境中的任何物体；特征提取模块402，用于提取环境物图像的点、线和边缘特征，建立环境物图像的帧间数据关联；传感器数据处理模块403，用于对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐；预积分模块404，用于对数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿；约束残差计算模块405，用于根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差；优化模块406，用于通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。
设备5可包括但不仅限于处理器50、存储器51。本领域技术人员可以理解，图5仅仅是设备5的示例，并不构成对设备5的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如计算设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器50可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器51可以是设备5的内部存储单元，例如设备5的硬盘或内存。存储器51也可以是设备5的外部存储设备，例如设备5上配备的插接式硬盘，智能存储卡(Smart MediaCard，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器51还可以既包括设备5的内部存储单元也包括外部存储设备。存储器51用于存储计算机程序以及设备所需的其他程序和数据。存储器51还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即，将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述装置中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个装置，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非临时性计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，室内定位方法的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤，即，获取图像采集模块采集环境物所得到的环境物图像，其中，环境物为移动机器人所处环境中的任何物体；提取环境物图像的点、线和边缘特征，建立环境物图像的帧间数据关联；对多个传感器之间获取的移动机器人位姿进行数据对齐以及对环境物的当前帧图像数据与多个传感器获取的移动机器人位姿进行数据对齐；对数据对齐后的移动机器人位姿进行预积分，获取移动机器人的预估位姿；根据帧间数据关联和移动机器人的预估位姿，计算滑动窗口中的约束残差；通过调节状态向量降低滑动窗口中的约束残差，直至滑动窗口中的约束残差最小时输出移动机器人的六自由度姿态数据。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。非临时性计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读内存(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，非临时性计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，非临时性计算机可读介质不包括电载波信号和电信信号。以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。以上所述的具体实施方式，对本申请的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本申请的具体实施方式而已，并不用于限定本申请的保护范围，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (9)
1.一种室内定位方法，其特征在于，所述方法包括：
获取图像采集模块采集环境物所得到的环境物图像，所述环境物为移动机器人所处环境中的任何物体；
提取所述环境物图像的点、线和边缘特征，建立所述环境物图像的帧间数据关联；包括：将所述环境物图像转化为灰度图像，将所述灰度图像栅格化，得到所述灰度图像的若干栅格图像；采用canny边缘特征提取算法，对所述栅格图像进行边缘特征的提取；若所述栅格图像中不存在点特征，则在所述不存在点特征的栅格图像提取到边缘特征时，从所述提取的边缘特征采样梯度最大的点作为所述不存在点特征的栅格图像的点特征；根据所述提取的点特征，通过光流法建立所述点特征的帧间数据关联作为边缘特征的帧间数据关联；
对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐；
对所述数据对齐后的移动机器人位姿进行预积分，获取所述移动机器人的预估位姿；
根据所述帧间数据关联和所述预估位姿，计算滑动窗口中的约束残差，包括：根据所述帧间数据关联，获取所述图像采集模块的重投影误差rc1；根据所述预估位姿，获取所述多个传感器之间获取的移动机器人帧间位姿残差ro；计算将所述rtotal作为所述滑动窗口中的约束残差，所述rp为边缘化留下的先验残差，所述ρ为鲁棒核函数，所述rplane为平面运动约束残差；
通过调节状态向量降低所述滑动窗口中的约束残差，直至所述约束残差最小时输出所述移动机器人的六自由度姿态数据；其中，所述状态向量记为X＝[p，q，bg，pbc，qbc，λ]T，其中，p为移动机器人的位置，q为移动机器人的姿态，bg为陀螺仪角速度偏置，λ为视觉中地图点的逆深度，pbc和qbc为图像采样模块和陀螺仪之间的外参。
2.如权利要求1所述室内定位方法，其特征在于，所述提取所述环境物图像的点、线和边缘特征，建立所述环境物图像的帧间数据关联，包括：
将所述环境物图像转化为灰度图像；
在保存所述灰度图像时，对所述灰度图像进行去畸变处理，得到去畸变灰度图像；
对所述灰度图像分别进行点特征的提取，通过光流法建立点特征的帧间数据关联；
对所述去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联。
3.如权利要求2所述室内定位方法，其特征在于，所述对所述去畸变灰度图像进行线特征提取，通过光流法建立线特征的帧间数据关联，包括：
对所述去畸变灰度图像进行线特征提取，得到所述去畸变灰度图像的初次线特征；
采样所述初次线特征上的若干像素点，将所述采样所得像素点的集合作为所述去畸变灰度图像的线特征；
基于共线约束，通过光流法建立所述线特征的帧间数据关联。
4.如权利要求1所述室内定位方法，其特征在于，所述多个传感器包括至少一个陀螺仪和至少一个轮式里程计，所述对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐，包括：
使用与所述当前帧图像数据前后相邻时间戳陀螺仪获取的移动机器人姿态数据对所述移动机器人姿态数据进行插值，以使插值后所得移动机器人姿态数据与所述环境物的当前帧图像数据对齐；以及
使用与所述当前帧图像数据前后相邻时间戳轮式里程计获取的移动机器人位置数据对所述移动机器人位置数据进行插值，以使插值后所得移动机器人位置数据与所述环境物的当前帧图像数据对齐。
5.如权利要求1所述室内定位方法，其特征在于，所述多个传感器包括至少一个陀螺仪和至少一个轮式里程计，所述对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐，包括：
采用所述陀螺仪获取的移动机器人姿态数据和所述轮式里程计获取的移动机器人位置数据互相插值的方式，以使所述至少一个陀螺仪获取的移动机器人姿态数据与所述至少一个轮式里程计获取的移动机器人位置数据对齐，得到所述多个传感器的移动机器人对齐位姿数据；
使用与所述当前帧图像数据前后相邻时间戳移动机器人对齐位姿数据对所述移动机器人对齐位姿数据进行插值，以使插值后所得移动机器人位姿数据与所述环境物的当前帧图像数据对齐。
6.如权利要求1所述室内定位方法，其特征在于，所述多个传感器包括至少一个陀螺仪和至少一个轮式里程计，所述对所述数据对齐后的移动机器人位姿进行预积分，获取所述移动机器人的预估位姿，包括：
基于所述轮式里程计测量得到的脉冲信号，获取理想状态下在所述图像采样模块采集一帧所述环境物图像的时段内所述轮式里程计的左轮滚过的距离和右轮滚过的距离
计算将所述/>作为ti+1时刻所述移动机器人的位置，所述所述/>为将轮式里程计坐标系转换为世界坐标系的转换矩阵，所述/>为ti时刻所述移动机器人的位置；
获取理想状态下所述陀螺仪在陀螺仪坐标系下的角速度值所述wb为所述陀螺仪在陀螺仪坐标系下的角速度测量值，所述bg为所述陀螺仪的角速度偏置，所述ng为噪声；
计算将所述/>作为ti+1时刻所述移动机器人的姿态，所述/>为ti时刻所述移动机器人的姿态，所述qbo为所述陀螺仪坐标系和所述轮式里程计坐标系之间的外参，所述dt是ti+1时刻与ti时刻之间的时长。
7.一种室内定位装置，其特征在于，所述装置包括：
图像获取模块，用于获取图像采集模块采集环境物所得到的环境物图像，所述环境物为移动机器人所处环境中的任何物体；
特征提取模块，用于提取所述环境物图像的点、线和边缘特征，建立所述环境物图像的帧间数据关联；包括：将所述环境物图像转化为灰度图像，将所述灰度图像栅格化，得到所述灰度图像的若干栅格图像；采用canny边缘特征提取算法，对所述栅格图像进行边缘特征的提取；若所述栅格图像中不存在点特征，则在所述不存在点特征的栅格图像提取到边缘特征时，从所述提取的边缘特征采样梯度最大的点作为所述不存在点特征的栅格图像的点特征；根据所述提取的点特征，通过光流法建立所述点特征的帧间数据关联作为边缘特征的帧间数据关联；
传感器数据处理模块，用于对多个传感器之间获取的移动机器人位姿进行数据对齐以及对所述环境物的当前帧图像数据与所述多个传感器获取的移动机器人位姿进行数据对齐；
预积分模块，用于对所述数据对齐后的移动机器人位姿进行预积分，获取所述移动机器人的预估位姿；
约束残差计算模块，用于根据所述帧间数据关联和所述预估位姿，计算滑动窗口中的约束残差，包括：根据所述帧间数据关联，获取所述图像采集模块的重投影误差rc1；根据所述预估位姿，获取所述多个传感器之间获取的移动机器人帧间位姿残差ro；计算将所述rtotal作为所述滑动窗口中的约束残差，所述rp为边缘化留下的先验残差，所述ρ为鲁棒核函数，所述rplane为平面运动约束残差；
优化模块，用于通过调节状态向量降低所述滑动窗口中的约束残差，直至所述约束残差最小时输出所述移动机器人的六自由度姿态数据；其中，所述状态向量X记为X＝[p，q，bg，pbc，qbc，λ]T，其中，p为移动机器人的位置，q为移动机器人的姿态，bg为陀螺仪角速度偏置，λ为视觉中地图点的逆深度，pbc和qbc为图像采样模块和陀螺仪之间的外参。
8.一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任意一项所述方法的步骤。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任意一项所述方法的步骤。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110084517.3A CN112880687B (zh) | 2021-01-21 | 2021-01-21 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| PCT/CN2022/073082 WO2022156755A1 (zh) | 2021-01-21 | 2022-01-21 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110084517.3A CN112880687B (zh) | 2021-01-21 | 2021-01-21 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112880687A CN112880687A (zh) | 2021-06-01 |
| CN112880687B true CN112880687B (zh) | 2024-05-17 |
Family
ID=76049891
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110084517.3A Active CN112880687B (zh) | 2021-01-21 | 2021-01-21 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112880687B (zh) |
| WO (1) | WO2022156755A1 (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112880687B (zh) * | 2021-01-21 | 2024-05-17 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| CN113587934B (zh) * | 2021-07-30 | 2024-03-19 | 深圳市普渡科技有限公司 | 一种机器人、室内定位方法、装置和可读存储介质 |
| CN113674320B (zh) * | 2021-08-24 | 2024-03-22 | 湖南国科微电子股份有限公司 | 视觉导航特征点获取方法、装置和计算机设备 |
| CN113470121B (zh) * | 2021-09-06 | 2021-12-28 | 深圳市普渡科技有限公司 | 自主移动平台、外参优化方法、装置及存储介质 |
| CN114234959B (zh) * | 2021-12-22 | 2024-02-20 | 深圳市普渡科技有限公司 | 机器人、vslam初始化方法、装置和可读存储介质 |
| CN116222556B (zh) * | 2023-01-13 | 2024-03-26 | 浙江大学 | 一种基于多源传感器融合的室内定位方法及系统 |
| CN116222583B (zh) * | 2023-05-10 | 2023-07-21 | 北京航空航天大学 | 一种基于滑动窗口的协同导航快速计算方法 |
| CN116499455B (zh) * | 2023-06-19 | 2023-11-14 | 煤炭科学研究总院有限公司 | 定位方法及装置 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及系统 |
| CN110060277A (zh) * | 2019-04-30 | 2019-07-26 | 哈尔滨理工大学 | 一种多特征融合的视觉slam方法 |
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN110189390A (zh) * | 2019-04-09 | 2019-08-30 | 南京航空航天大学 | 一种单目视觉slam方法及系统 |
| CN110763251A (zh) * | 2019-10-18 | 2020-02-07 | 华东交通大学 | 视觉惯性里程计优化的方法及系统 |
| CN110807809A (zh) * | 2019-10-25 | 2020-02-18 | 中山大学 | 基于点线特征和深度滤波器的轻量级单目视觉定位方法 |
| CN111220154A (zh) * | 2020-01-22 | 2020-06-02 | 北京百度网讯科技有限公司 | 车辆定位方法、装置、设备和介质 |
| CN111750853A (zh) * | 2020-06-24 | 2020-10-09 | 国汽(北京)智能网联汽车研究院有限公司 | 一种地图建立方法、装置及存储介质 |
| CN111882607A (zh) * | 2020-07-14 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
| CN112115980A (zh) * | 2020-08-25 | 2020-12-22 | 西北工业大学 | 基于光流跟踪和点线特征匹配的双目视觉里程计设计方法 |
| CN112183171A (zh) * | 2019-07-05 | 2021-01-05 | 杭州海康机器人技术有限公司 | 一种基于视觉信标建立信标地图方法、装置 |
| CN112197770A (zh) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | 一种机器人的定位方法及其定位装置 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103959307B (zh) * | 2011-08-31 | 2017-10-24 | Metaio有限公司 | 从灰度图像中检测和描述特征的方法 |
| CN108181897A (zh) * | 2017-12-11 | 2018-06-19 | 华侨大学 | 一种双足机器人自动循迹的方法 |
| CN111354042B (zh) * | 2018-12-24 | 2023-12-01 | 深圳市优必选科技有限公司 | 机器人视觉图像的特征提取方法、装置、机器人及介质 |
| CN110675307B (zh) * | 2019-08-19 | 2023-06-06 | 杭州电子科技大学 | 基于vslam的3d稀疏点云到2d栅格图的实现方法 |
| CN111795686B (zh) * | 2020-06-08 | 2024-02-02 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN111739063B (zh) * | 2020-06-23 | 2023-08-18 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
| CN112880687B (zh) * | 2021-01-21 | 2024-05-17 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
-
2021
- 2021-01-21 CN CN202110084517.3A patent/CN112880687B/zh active Active
-
2022
- 2022-01-21 WO PCT/CN2022/073082 patent/WO2022156755A1/zh active Application Filing
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN108682027A (zh) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | 基于点、线特征融合的vSLAM实现方法及系统 |
| CN110189390A (zh) * | 2019-04-09 | 2019-08-30 | 南京航空航天大学 | 一种单目视觉slam方法及系统 |
| CN110060277A (zh) * | 2019-04-30 | 2019-07-26 | 哈尔滨理工大学 | 一种多特征融合的视觉slam方法 |
| CN112183171A (zh) * | 2019-07-05 | 2021-01-05 | 杭州海康机器人技术有限公司 | 一种基于视觉信标建立信标地图方法、装置 |
| CN110763251A (zh) * | 2019-10-18 | 2020-02-07 | 华东交通大学 | 视觉惯性里程计优化的方法及系统 |
| CN110807809A (zh) * | 2019-10-25 | 2020-02-18 | 中山大学 | 基于点线特征和深度滤波器的轻量级单目视觉定位方法 |
| CN111220154A (zh) * | 2020-01-22 | 2020-06-02 | 北京百度网讯科技有限公司 | 车辆定位方法、装置、设备和介质 |
| CN111750853A (zh) * | 2020-06-24 | 2020-10-09 | 国汽(北京)智能网联汽车研究院有限公司 | 一种地图建立方法、装置及存储介质 |
| CN111882607A (zh) * | 2020-07-14 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
| CN112115980A (zh) * | 2020-08-25 | 2020-12-22 | 西北工业大学 | 基于光流跟踪和点线特征匹配的双目视觉里程计设计方法 |
| CN112197770A (zh) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | 一种机器人的定位方法及其定位装置 |
Non-Patent Citations (5)
| Title |
|---|
| Direct Line Guidance Odometry;Shi-Jie Li等;《2018 IEEE International Conference on Robotics and Automation (ICRA)》;第5137-5143页 * |
| Tightly-Coupled Monocular Visual-Odometric SLAM Using Wheels and a MEMS Gyroscope;Meixiang Quan等;《IEEE Access》;第7卷;第97374-97389页 * |
| VINS-Mono: a robust and versatile monocular visual-inertial state estimator;Qin, Tong等;《IEEE Transactions on Robotics》;20180727;第34卷(第4期);第1004-1020页 * |
| 基于点线特征和边缘特征的单目视觉里程计算法;陈小聪等;《信息与控制》;第49卷(第4期);第396-403页 * |
| 结合轮速传感器的紧耦合单目视觉惯性SLAM;陆泽早;《中国优秀硕士学位论文全文数据库 信息科技辑》(第1期);第I140-501页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2022156755A1 (zh) | 2022-07-28 |
| CN112880687A (zh) | 2021-06-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112880687B (zh) | 一种室内定位方法、装置、设备和计算机可读存储介质 | |
| CN111354042B (zh) | 机器人视觉图像的特征提取方法、装置、机器人及介质 | |
| CN112197770B (zh) | 一种机器人的定位方法及其定位装置 | |
| US8698875B2 (en) | Estimation of panoramic camera orientation relative to a vehicle coordinate frame | |
| JP6100380B2 (ja) | 特に装置に係る視覚ベースの位置決めに使用される画像処理方法 | |
| CN111210477B (zh) | 一种运动目标的定位方法及系统 | |
| CN112686877B (zh) | 基于双目相机的三维房屋损伤模型构建测量方法及系统 | |
| KR101621370B1 (ko) | 도로에서의 차선 검출 방법 및 장치 | |
| CN112556685B (zh) | 导航路线的显示方法、装置和存储介质及电子设备 | |
| US20160210761A1 (en) | 3d reconstruction | |
| CN113587934B (zh) | 一种机器人、室内定位方法、装置和可读存储介质 | |
| KR102525570B1 (ko) | 라이다-카메라 영상 융합을 위한 라이다 데이터 이상치 제거 방법 및 이를 수행하는 컴퓨팅 장치 | |
| CN112652020A (zh) | 一种基于AdaLAM算法的视觉SLAM方法 | |
| CN110673607B (zh) | 动态场景下的特征点提取方法、装置、及终端设备 | |
| CN113610702B (zh) | 一种建图方法、装置、电子设备及存储介质 | |
| CN113450334B (zh) | 一种水上目标检测方法、电子设备及存储介质 | |
| CN103489165A (zh) | 一种面向视频拼接的小数查找表生成方法 | |
| CN110648353A (zh) | 一种基于单目传感器的机器人室内定位方法和装置 | |
| CN109284707A (zh) | 动目标检测方法及装置 | |
| Qin et al. | A coarse elevation map-based registration method for super-resolution of three-line scanner images | |
| CN111780744A (zh) | 移动机器人混合导航方法、设备及存储装置 | |
| EP2953096B1 (en) | Information processing device, information processing method, system and carrier means | |
| CN110660134B (zh) | 三维地图构建方法、三维地图构建装置及终端设备 | |
| WO2022253043A1 (zh) | 人脸深度图像的面部变形补偿方法、成像装置和存储介质 | |
| CN112381939A (zh) | 一种视觉slam方法、装置、机器人及存储介质 |
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 |