CN113587934B - 一种机器人、室内定位方法、装置和可读存储介质 - Google Patents
一种机器人、室内定位方法、装置和可读存储介质 Download PDFInfo
- Publication number
- CN113587934B CN113587934B CN202110875770.0A CN202110875770A CN113587934B CN 113587934 B CN113587934 B CN 113587934B CN 202110875770 A CN202110875770 A CN 202110875770A CN 113587934 B CN113587934 B CN 113587934B
- Authority
- CN
- China
- Prior art keywords
- data
- pose data
- frequency
- robot
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 239000011159 matrix material Substances 0.000 claims abstract description 68
- 238000006243 chemical reaction Methods 0.000 claims abstract description 58
- 230000000007 visual effect Effects 0.000 claims description 106
- 238000005259 measurement Methods 0.000 claims description 63
- 238000004364 calculation method Methods 0.000 claims description 21
- 238000004422 calculation algorithm Methods 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 18
- 238000005457 optimization Methods 0.000 claims description 18
- 230000009466 transformation Effects 0.000 claims description 16
- 230000004927 fusion Effects 0.000 claims description 13
- 238000012545 processing Methods 0.000 claims description 12
- 230000003287 optical effect Effects 0.000 claims description 10
- 230000010354 integration Effects 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 101100161752 Mus musculus Acot11 gene Proteins 0.000 claims 2
- 230000004807 localization Effects 0.000 claims 1
- 230000006870 function Effects 0.000 description 8
- 230000008859 change Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 238000005070 sampling Methods 0.000 description 5
- 230000008878 coupling Effects 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000012937 correction Methods 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 241000135164 Timea Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation 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
-
- 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/3807—Creation or updating of map data characterised by the type of data
- G01C21/383—Indoor data
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
Abstract
一种室内定位方法，包括：获取传感器数据；获取机器人在世界坐标系的低频位姿数据；获取基于所述传感器数据所计算得到的所述机器人在机器人坐标系下的高频位姿数据；利用时间戳将低频位姿数据与高频位姿数据对齐；以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算机器人坐标系到世界坐标系的转换矩阵；根据转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。本申请的技术方案使得不再单纯依赖于高频位姿数据，即在高频传感器的精度较低时，依然能够获得精度比较高的室内定位数据。
Description
技术领域
本发明涉及机器人领域，特别涉及一种机器人、室内定位方法、装置和可读存储介质。
背景技术
当机器人工作在狭窄拥挤的环境和/或高速运行模式下，低频传感器(即输出数据的频率较低)输出的位姿数据使得机器人无法得到及时的位姿反馈，从而导致后续机器人控制指令无法及时发送，造成路径规划错误、撞到障碍物、偏离导航路径、定位错误和定位丢失等一系列问题，无法执行正常的工作任务。
针对上述问题，目前的解决方案是利用高频传感器输出的数据(即输出数据的频率较高)对低频传感器输出的数据进行补偿，例如，利用高频的轮式里程计输出的数据和惯性导航单元输出的数据对多传感器融合后的低频位姿输出进行航迹推理，预测机器人位姿，从而达到高频位姿输出的结果。然而，这种方案极其依赖高频传感器的精度，若高频传感器的精度越低，则最后输出的位姿精度相应越低。
发明内容
本申请提供一种机器人、室内定位方法、装置和可读存储介质，以在标识有限或观测约束有限的场景下，能够通过激光和视觉合作标识建立稳定的地图。
一方面，本申请提供了一种机器人，所述机器人包括：
存储器和处理器；
所述存储器存储有可执行程序代码；
与所述存储器耦合的所述处理器，调用所述存储器中存储的可执行程序代码，执行如下室内定位方法，所述方法包括：
获取所述机器人的在世界坐标系下的低频位姿数据；
获取基于所述传感器数据所计算得到的所述机器人在机器人坐标系下的高频位姿数据利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；
根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
另一方面，本申请提供了一种室内定位装置，所述装置包括：
获取模块，用于获取所述机器人在世界坐标系下的低频位姿数据；
融合模块，用于获取所述机器人基于传感器数据所计算得到所述机器人在机器人坐标系下的高频位姿数据；
同步模块，用于利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
计算模块，用于以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；
转换模块，用于根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
第三方面，本申请提供了一种室内定位方法，所述方法包括：
获取所述机器人在世界坐标系下的低频位姿数据；
获取所述机器人基于传感器数据所计算得到所述机器人在机器人坐标系下的高频位姿数据；
利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；
根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
第四方面，本申请提供了一种可读存储介质，其上存储有计算机程序，所述计算机程序用于被处理器执行时实现室内定位方法，所述室内定位方法为上述机器人实现所述室内定位方法。
从上述本申请提供的技术方案可知，在得到时间戳对齐的低频位姿数据与高频位姿数据后，可以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，得到世界坐标系到机器人坐标系的转换矩阵，再根据该转换矩阵将当前高频位姿数据转换为在世界坐标系下的定位位姿数据。由于可以通过图优化求解算法得到机器人坐标系到世界坐标系的转换矩阵，再将高频位姿数据转换为在世界坐标系下的定位位姿数据，因此，不再单纯依赖于高频位姿数据，即在高频传感器的精度较低时，依然能够获得精度比较高的室内定位数据。且进一步的，本申请通过高频位姿数据从低频位姿数据计算出由高频位姿数据到世界坐标系下的定位位姿数据，从而可以持续性对高频位姿数据进行计算，并快速获取对应的定位位姿数据，在增强定位位姿数据输出时的准确度的同时，也增强了整个输出的快速性，从而便于机器人在复杂环境中高速运行的快速定位。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的机器人的结构示意图；
图2是本申请实施例提供的室内定位方法的流程图；
图3是本申请实施例提供的将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐的示意图；
图4是本申请实施例提供的室内定位装置的结构示意图；
图5是本申请实施例提供的设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本说明书中，诸如第一和第二这样的形容词仅可以用于将一个元素或动作与另一元素或动作进行区分，而不必要求或暗示任何实际的这种关系或顺序。在环境允许的情况下，参照元素或部件或步骤(等)不应解释为局限于仅元素、部件、或步骤中的一个，而可以是元素、部件、或步骤中的一个或多个等。
在本说明书中，为了便于描述，附图中所示的各个部分的尺寸并不是按照实际的比例关系绘制的。
请参阅附图1，本申请一实施例提供的机器人的结构示意图。为了便于说明，仅示出了与本申请实施例相关的部分。该机器人可包括：
存储器10和处理器20以及传感器，处理器20为机器人的运算和控制核心，是信息处理、程序运行的最终执行单元。存储器10例如硬盘驱动存储器，非易失性存储器(例如闪存或用于形成固态驱动器的其它电子可编程限制删除的存储器等)，易失性存储器(例如静态或动态随机存取存储器等)等，本申请实施例不作限制。
存储器10中存储有可执行程序代码；与存储器10耦合的处理器20调用存储器10中存储的所述可执行程序代码，执行如下室内定位方法：获取机器人的低频位姿数据；获取机器人基于传感器数据所计算得到机器人的高频位姿数据；利用时间戳将低频位姿数据与高频位姿数据对齐；以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算高频位姿数据到低频位姿数据的转换矩阵；根据转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
参阅图2，是本申请实施例提供的一种室内定位方法，主要包括步骤S201至步骤S205，说明如下：
步骤S200：通过传感器获取传感器数据；
在本实施例中，传感器具体可以是搭载在机器人上的激光雷达、视觉相机、IMU(惯性测量单元)、里程计、GPS中的一个或多个。对应的，传感器数据可以包括激光雷达数据、视觉数据、惯性导航数据、里程计数据以及GPS数据中的一个或多个。
步骤S201：获取机器人在世界坐标系下的低频位姿数据。
在本申请实施例中，机器人的低频位姿数据可来源于全球定位系统(GlobalPositioning System，GPS)或者一些传感器数据，即获取机器人的低频位姿数据可以是：直接以机器人内置的GPS获取的定位数据作为机器人的低频位姿数据，或者，将传感器数据与预设的世界地图进行匹配，以获取机器人在机器人坐标系下的低频位姿数据。
在可选实施例中，低频位姿数据具体可以是在世界坐标系下的位姿数据。
步骤S202：获取基于传感器数据所计算得到的机器人在机器人坐标系下的高频位姿数据。
在具体场景中，高频位姿数据具体可以是类似于SLAM方案中即时定位的方式所得到，即基于上一帧所得到的位姿数据，随后基于高频传感器数据从而获得当前帧的位姿数据。
在可选实施例中，高频位姿数据具体可以是在机器人坐标系下的位姿数据。
例如，当确定好上一帧的位姿数据后，通过如激光雷达、深度相机、IMU以及里程计中的一个采集的传感器数据或多个采集的传感器数据的融合数据，可以计算出机器人在当前帧相对上一帧的位置变化数据，进而基于该位置变化和上一帧的位姿数据可以确定当前帧的位姿数据，其具体计算原理属于SLAM的常见技术，这里不再做赘述，由于每一帧位姿数据均是基于上一帧的位姿数据和位置变化数据计算得到，因此整个计算过程很快，且如IMU、里程计等传感器的采集频率也很快，即所得到的位姿数据相对而言是高频位姿数据。但是由于这样的计算，由于传感器的误差等原因，位置变化数据存在一定的不精准性，因此每次计算的高频位姿数据精确度相对较低且会产生累积误差。
在具体场景中，低频位姿数据是基于传感器数据和预设的世界地图进行匹配得到的，如室外可以利用GPS进行获得，如室内的话，可以预先建立激光世界地图(激光世界坐标系)或者视觉世界地图(视觉世界坐标系)，通过激光传感器获得激光数据，可以将该激光数据和激光世界地图进行匹配，从而可以获取机器人在世界坐标系的位姿，由于整个匹配过程相对而言，计算量较大导致计算速度较慢的原因影响，因此低频位姿数据虽然相对精度高不少，但是频率会比较慢，因此不适用高流、高速运行的场景。类似的，采用视觉传感器采用视觉数据和视觉世界坐标系匹配，也会有类似的问题。
本实施例中所述的低频位姿数据和高频位姿数据是相对的，即低频位姿数据的获取频率相对而言，比高频位姿数据要低一些，且位姿数据是指机器人的位置(坐标)和姿态(朝向角)的数据。
可选地，上述只是简单阐述了本申请中低频位姿数据和高频位姿数据的区别，但是不仅仅限于上述所述的低频位姿数据和高频位姿数据。其低频位姿数据和高频位姿数据的来源也可以是其他，这里不做限定。
以下，本申请以一种低频位姿数据和一种高频位姿数据进行具体介绍。本申请实施例提及的视觉相机和IMU均属于传感器，且均可以搭载在机器人之上，将两个部件获取的数据融合，可以得到在机器人坐标系下的高频位姿数据。上述实施例中，视觉相机可以是单目相机、双目相机或者深度相机，本申请对视觉相机不做限定。作为本申请一个实施例，获取机器人基于传感器数据所计算得到机器人的高频位姿数据可通过如下步骤S2021至步骤S2025实现：
步骤S2021：基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据。
步骤S2022：将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐。
对于不同设备，即使硬件同步以后也存在采样频率不一致的差异，因此，在融合视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据时必然存在设备获取的数据在时间戳上不同步的问题。如图3所示，假设视觉相机在ti时刻获取的一帧视觉数据为Dti，理想状态下是惯性测量单元也能够在ti时刻获取移动机器人位置数据，然而，由于视觉相机的采样频率与惯性测量单元的采样频率不一致等原因，惯性测量单元只能在t′li时刻获取到惯性导航数据D′li，即视觉相机和惯性测量单元两者获取的数据没有对齐。对于上述情形，需要采用数据对齐的方案。
在本申请一个实施例中，将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐可以是：使用与参考帧视觉数据前后相邻时间戳视觉相机获取的视觉数据对视觉数据进行插值，以使插值后所得视觉数据与参考帧视觉数据对齐；以及使用与参考帧视觉数据前后相邻时间戳惯性测量单元获取的惯性导航数据对惯性导航数据进行插值，以使插值后所得惯性导航数据与参考帧视觉数据对齐。仍然以图3为例，使用与参考帧视觉数据D′xi前后相邻时间戳视觉相机获取的视觉数据，即，使用ti-1时刻视觉相机获取的视觉数据D′ti-1与ti时刻视觉相机获取的视觉数据Dti对视觉相机获取的视觉数据进行插值，得到t′xi时刻的图像插值数据从图3可以看出，经过上述插值操作，图像插值数据/>与参考帧视觉数据D′xi已经对齐。同样地，使用与参考帧视觉数据D′xi前后相邻时间戳惯性测量单元获取的惯性导航数据，即，使用t′li时刻惯性导航数据D′li与t′li+1时刻惯性导航数据D′li+1对惯性导航数据进行插值，得到t′xi时刻的惯性导航插值数据/>从图3可以看出，经过上述插值操作，惯性导航插值数据/>与参考帧视觉数据D′xi已经对齐，最终的结果是视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐。
步骤S2023：从对齐后视觉数据提取特征点并跟踪特征点，得到相邻两帧图像的视觉重投影误差。
视觉数据是视觉相机对机器人当前所在环境进行图像采集获取的数据，环境中的路标可以作为视觉数据的特征点，然后对这些特征点进行跟踪以实现特征点的匹配，在本申请一个实施例中，从对齐后视觉数据提取特征点并跟踪特征点，得到相邻两帧图像的视觉重投影误差可以是：从对齐后视觉数据中选取两帧目标图像，并提取两帧目标图像的特征点，然后，通过LK稀疏光流算法对两帧目标图像的特征点进行跟踪，以获取两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差。上述实施例中，LK稀疏光流算法是典型的光流跟踪算法，其基于同一目标在不同帧间运动时亮度不会发生改变和时间的变化不会引起目标位置的剧烈变化这两个假设，相比于稠密光流算法，LK稀疏光流算法并不对图像的每个像素点进行逐点计算，而是只需要对指定的一组点进行跟踪(这组点最好具有某种明显的特性，例如Harris角点等)，因而，计算开销比稠密光流算法小得多。
步骤S2024：对对齐后惯性导航数据进行预积分，得到相邻两个时刻之间的惯性残差。
具体地，步骤S2024的实现可以是对两帧对齐后惯性导航数据对应的三轴加速度和三轴角速度信息进行预积分处理，以获取两帧对齐后惯性导航数据的第二相对位姿，并获取相应的惯性残差。
步骤S2025：根据视觉重投影误差和惯性残差，计算得到在机器人坐标系下的高频位姿数据。
步骤S2025的实现可以是根据视觉重投影误差和惯性残差对第一相对位姿和第二相对位姿进行优化，以获取在机器人坐标系下的高频位姿数据，即，采用基于非线性优化的方式优化第一相对位姿和第二相对位姿，以输出与惯性测量单元频率相同的位姿。具体地，可以采用基于滑动窗图优化的方式，即，在一个滑动窗内，联合最小化目标函数，以实现对第一相对位姿和第二相对位姿进行优化，其中，目标函数包括视觉重投影误差、惯性残差和边缘化残差。
作为本申请另一实施例，获取机器人基于传感器数据所计算得到机器人的高频位姿数据可通过如下步骤S’2021至步骤S’2025实现：
步骤S’2021：基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据。
步骤S’2022：将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐。
步骤S’2022的实现方案与前述实施例的S2022的实现方案基本相同，相关术语、特征等的阐释可参考前述实施例中相关部分的描述，此处不做赘述。
步骤S’2023：确定对齐后惯性导航数据中惯性测量单元在第一时刻对应的标准位姿数据和惯性测量单元在第二时刻对应的待修正位姿数据，其中，第一时刻和第二时刻分别为相邻两个时刻的在先时刻和在后时刻，可选地，其中第一时刻也可以对应上一帧，第二时刻也可以对应当前帧。
作为一种测量物体三轴加速度和三轴角速度信息等参量的设备，惯性测量单元可以按照设定的频率采集机器人的位姿数据，并且，惯性测量单元在当前时刻采集的位姿数据可以反映惯性测量单元在当前时刻相对于上一时刻的位姿变化，因此，可以对惯性测量单元不断采集的位姿数据进行积分，得到惯性测量单元在当前时刻对应的待修正位姿数据，通过对该待修正位姿数据进行修正后，可以得到惯性测量单元对应的标准位姿数据，该标准位姿数据可以用于表示机器人在预先构建的世界坐标系中的位姿数据。本申请实施例提供的确定惯性测量单元在第一时刻对应的标准位姿数据与确定惯性测量单元在第二时刻对应的标准位姿数据的方式相同，因此，确定惯性测量单元在第一时刻对应的标准位姿数据的过程可以参照本申请介绍的确定惯性测量单元在第二时刻对应的标准位姿数据的过程。
步骤S’2024：基于惯性测量单元在第一时刻对应的标准位姿数据以及在第二时刻对应的待修正位姿数据，确定视觉相机在第二时刻相对第一时刻的第二相对位姿数据。
由于在同一时刻，惯性测量单元和视觉相机之间具有一定的位姿关系，该位姿关系可以通过将视觉相机和惯性测量单元置于同一坐标系下进行确定，或者可以在机器人移动之前，对安装于机器人上的定位传感器进行标定，可以获取到视觉相机和惯性测量单元之间的位姿关系。基于该位姿关系，可以通过惯性测量单元在第一时刻对应的标准位姿数据和在第二时刻对应的待修正位姿数据，预测视觉相机在第二时刻相对第一时刻的第二相对位姿数据。
步骤S’2025：基于第一相对位姿数据和第二相对位姿数据，对惯性测量单元在第二时刻对应的待修正位姿数据进行修正，得到惯性测量单元在第二时刻对应的标准位姿数据，作为机器人的高频位姿数据。
如前所述，第一相对位姿数据是根据视觉相机在相邻时刻采集的视觉数据确定的，第二相对位姿数据是根据惯性测量单元在第一时刻对应的标准位姿数据和第二时刻对应的待修正位姿数据预测得到的，因此，第一相对位姿数据和第二相对位姿数据之间的残差可以用于对惯性测量单元在第二时刻对应的待修正位姿数据进行修正，例如，可以引入卡尔曼滤波算法进行修正，得到惯性测量单元在第二时刻对应的标准位姿数据，该标准位姿数据可以作为机器人的高频位姿数据。
需要说明的是，上述实施例中的视觉相机和惯性测量单元作为一个系统，在开启时进行初始化。若未成功初始化，则将视觉相机获取的视觉数据与惯性导航单元获取的惯性导航数据进行预定帧数(例如，十帧)的松耦合对齐，联合系统的初始化，输出首个机器人位姿数据。其中，上述系统初始化成功的判断标准可以是是这十帧数据之间具有一定程度的平移和旋转，同时各帧数据之间，其视觉数据的连续追踪3帧以上的特征点需要超过20个，并且最后初始化完成后的良好视觉特征点超过30个(此处，所谓良好视觉特征点是指能够成为地图上点的特征点)，通过位姿关系投影到这些良好视觉特征点关联的图像上时，与它追踪时的二维像素点的距离相差不超过预设阈值。
步骤S203：利用时间戳将低频位姿数据与经步骤S202得到的高频位姿数据对齐。
如前所述，机器人的低频位姿数据可以是来源于GPS等低频率定位数据获取系统或其他低频传感器获取到的机器人的位姿信息，至于利用时间戳将低频位姿数据与经步骤S202计算得到的高频位姿数据对齐，其具体方案类似于前述实施例中将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐的技术方案，此处不做赘述。
步骤S204：以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算机器人坐标系到世界坐标系的转换矩阵。
具体地，步骤S204的实现可以是：以时间戳对齐的低频位姿数据和经步骤S202计算得到的高频位姿数据为观测数据、以转换矩阵为优化目标，并构建机器人的绝对位置误差，建立非线性最小二乘问题；以图优化求解算法求解该非线性最小二乘问题，将该非线性最小二乘问题的最优解作为机器人的机器人坐标系到世界坐标系下的转换矩阵。此处，图优化求解算法可以是Levenberg-Marquardt算法或高斯-牛顿法，等等，本申请对此不做限定。
在具体实施例中，计算高频位姿数据所在的机器人坐标系到低频位姿数据所在的世界坐标系的转换矩阵，即两个坐标系之间的转换矩阵。也即可以认为是高频位姿数据和低频位姿数据之间坐标的转换矩阵。
可选地，由于高频位姿数据位于机器人坐标系中，而低频位姿数据则位于世界坐标系下，利用转换矩阵处理高频位姿数据，从而直接将高频位姿数据转换成世界坐标系下，从而获得定位位姿数据(世界坐标系)。
需要说明的是，上述实施例中机器人的机器人坐标系到世界坐标系下的转换矩阵并非一成不变，而是随着低频传感器采集数据的时刻(或低频位姿数据生成时刻)和高频传感器采集数据的时刻(或高频位姿数据生成时刻)而变化，这种变化是基于这样一个事实，即：由于高频传感器与低频传感器存在采样频率差即两者采样频率不同步，例如，在10秒之内，若低频传感器在第1秒采集了第一次低频位姿数据，在第8秒采集了第二次低频位姿数据，则在第1秒到第8秒之间高频传感器采集的所有高频位姿数据均采用低频传感器在第1秒采集的第一次低频位姿数据计算得到的转换矩阵进行计算，而第8秒后的高频位姿数据则采用低频传感器在第8秒采集的第二次低频位姿数据计算得到的转换矩阵进行计算……，后续时间的高频位姿数据同前述实例，从而可以使得每个输出的高频位姿数据都进行了优化，转换矩阵一直更新。
步骤S205：根据转换矩阵，将机器人的高频位姿数据转换为在世界坐标系下的定位位姿数据。
此处，假设定位位姿数据用T3表示，假设机器人的高频位姿数据使用T1表示，转换矩阵使用T2表示，则根据转换矩阵，将机器人的高频位姿数据T1转换为在世界坐标系下的定位位姿数据，实际是对T2、T1和T2 -1做矩阵乘法运算，即T3＝T2*T1*T2 -1，所得T2*T1*T2 -1即为世界坐标系下的位姿数据可以是：分别以转换矩阵T2和转换矩阵的逆矩阵T2 -1对以矩阵T1表示的当前高频位姿数据进行左乘和右乘，将矩阵相乘所得T2*T1*T2 -1作为机器人在世界坐标系下的定位位姿数据。
从上述附图2示例的室内定位方法可知，在得到时间戳对齐的低频位姿数据与高频位姿数据后，可以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，得到高频位姿数据到低频位姿数据的转换矩阵，再根据该转换矩阵将当前高频位姿数据转换为在世界坐标系下的定位位姿数据。由于可以通过图优化求解算法得到机器人坐标系到世界坐标系的转换矩阵，再将高频位姿数据转换为在世界坐标系下的定位位姿数据，因此，不再单纯依赖于高频位姿数据，即在高频传感器的精度较低时，依然能够获得精度比较高的室内定位数据。且进一步的，本申请通过高频位姿数据从低频位姿数据计算出由高频位姿数据到世界坐标系下的定位位姿数据，从而可以持续性对高频位姿数据进行计算，并快速获取对应的定位位姿数据，在增强定位位姿数据输出时的准确度的同时，也增强了整个输出的快速性，从而便于机器人在复杂环境中高速运行的快速定位。
请参阅图4，是本申请实施例提供的一种室内定位装置，该装置可以是机器人的中央处理单元或者其中的功能模块，该装置可以包括获取模块401、融合模块402、同步模块403、计算模块404和转换模块405，详述如下：
获取模块401，用于获取传感器数据和机器人在世界坐标系下的低频位姿数据；
融合模块402，用于获取基于所述传感器数据所计算得到的机器人在机器人坐标下的高频位姿数据；
同步模块403，用于利用时间戳将低频位姿数据与高频位姿数据对齐；
计算模块404，用于以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算机器人坐标系到世界坐标系的转换矩阵；
转换模块405，用于根据转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
在本申请一个实施例中，上述附图4示例的融合模块402可以包括数据获取单元、数据对齐单元、误差计算单元、残差计算单元和位姿计算单元，其中：
数据获取单元，用于基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据；
数据对齐单元，用于将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐；
误差计算单元，用于从对齐后视觉数据提取特征点并跟踪该特征点，得到相邻两帧图像的视觉重投影误差；
残差计算单元，用于对对齐后惯性导航数据进行预积分，得到相邻两个时刻之间的惯性残差；
位姿计算单元，用于根据视觉重投影误差和惯性残差，计算得到在机器人坐标系下的高频位姿数据。
可选地，在本申请另一实施例中，上述误差计算单元包括特征点提取单元和跟踪单元，残差计算单元包括预积分处理单元，位姿计算单元包括第一优化单元，其中：
特征点提取单元，用于从对齐后视觉数据中选取两帧目标图像，并提取两帧目标图像的特征点；
跟踪单元，用于通过LK稀疏光流算法对两帧目标图像的特征点进行跟踪，以获取两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；
预积分处理单元，用于对两帧对齐后惯性导航数据对应的三轴加速度和三轴角速度信息进行预积分处理，以获取两帧对齐后惯性导航数据的第二相对位姿，并获取相应的惯性残差；
第一优化单元，用于根据视觉重投影误差和惯性残差对第一相对位姿和第二相对位姿进行优化，以获取在机器人坐标系下的高频位姿数据。
可选地，在本申请另一实施例中，附图4示例的融合模块402可以包括数据获取单元、数据对齐单元、第一确定单元、第二确定单元和修正单元，其中：
数据获取单元，用于基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据；
数据对齐单元，用于将视觉相机获取的视觉数据和惯性测量单元获取的惯性导航数据对齐；
第一确定单元，用于确定对齐后惯性导航数据中惯性测量单元在第一时刻对应的标准位姿数据和惯性测量单元在第二时刻对应的待修正位姿数据，其中，第一时刻和第二时刻分别为相邻两个时刻的在先时刻和在后时刻；
第二确定单元，用于基于惯性测量单元在第一时刻对应的标准位姿数据以及在第二时刻对应的待修正位姿数据，确定视觉相机在第二时刻相对第一时刻的第二相对位姿数据；
修正单元，用于基于第一相对位姿数据和第二相对位姿数据，对惯性测量单元在第二时刻对应的待修正位姿数据进行修正，得到惯性测量单元在第二时刻对应的标准位姿数据，作为在机器人坐标系下的高频位姿数据。
在本申请一个实施例中，上述附图4示例的计算模块404可以包括问题构建单元和问题求解单元，其中：
问题构建单元，用于以时间戳对齐的低频位姿数据和高频位姿数据为观测数据、以转换矩阵为优化目标，构建非线性最小二乘问题；
问题求解单元，用于以图优化求解算法求解非线性最小二乘问题，将非线性最小二乘问题的最优解作为机器人坐标系到世界坐标系的转换矩阵。
可选地，在本申请另一实施例中，附图4示例的转换模块405具体用于通过下述公式计算所述定位位姿数据：
T3＝T2*T1*T2 -1；
具体地，分别以所述转换矩阵T2和所述转换矩阵的逆矩阵T2 -1对以矩阵T1表示的当前高频位姿数据进行左乘和右乘，将矩阵相乘所得T2*T1*T2 -1作为所述机器人在所述世界坐标系下的定位位姿数据T3。
可选地，在本申请另一实施例中，附图4示例的获取模块401具体用于直接以机器人内置的GPS获取的定位数据作为机器人在世界坐标系下的低频位姿数据，或者，将传感器数据与世界坐标系进行匹配，以获取机器人在世界坐标系下的低频位姿数据。
从附图4示例的装置可知，在得到时间戳对齐的低频位姿数据与高频位姿数据后，可以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，得到机器人坐标系到世界坐标系的转换矩阵，再根据该转换矩阵将当前高频位姿数据转换为在世界坐标系下的定位位姿数据。由于可以通过图优化求解算法得到机器人坐标系到世界坐标系的转换矩阵，再将高频位姿数据转换为在世界坐标系下的定位位姿数据，因此，不再单纯依赖于高频位姿数据，即在高频传感器的精度较低时，依然能够获得精度比较高的室内定位数据。且进一步的，本申请通过高频位姿数据从低频位姿数据计算出由高频位姿数据到世界坐标系下的定位位姿数据，从而可以持续性对高频位姿数据进行计算，并快速获取对应的定位位姿数据，在增强定位位姿数据输出时的准确度的同时，也增强了整个输出的快速性，从而便于机器人在复杂环境中高速运行的快速定位。
图5是本申请一实施例提供的设备的结构示意图。如图5所示，该实施例的设备5可以是一种机器人或者其中的模块，主要包括：处理器50、存储器51以及存储在存储器51中并可在处理器50上运行的计算机程序52，例如室内定位方法的程序。处理器50执行计算机程序52时实现上述室内定位方法实施例中的步骤，例如图2所示的步骤S201至S205。或者，处理器50执行计算机程序52时实现上述各装置实施例中各模块/单元的功能，例如图4所示获取模块401、融合模块402、同步模块403、计算模块404和转换模块405的功能。
示例性地，室内定位方法的计算机程序52主要包括：获取传感器数据，获取机器人在机器人坐标系下的低频位姿数据；获取基于所述传感器数据所计算得到的所述机器人在机器人坐标系下的高频位姿数据；；利用时间戳将低频位姿数据与高频位姿数据对齐；以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算机器人坐标系到世界坐标系下的转换矩阵；根据转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。计算机程序52可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器51中，并由处理器50执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序52在设备5中的执行过程。例如，计算机程序52可以被分割成获取模块401、融合模块402、同步模块403、计算模块404和转换模块405(虚拟装置中的模块)的功能，各模块具体功能如下：获取模块401，用于获取机器人的低频位姿数据；融合模块402，用于获取机器人基于传感器数据所计算得到机器人的高频位姿数据；同步模块403，用于利用时间戳将低频位姿数据与高频位姿数据对齐；计算模块404，用于以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，计算高频位姿数据到低频位姿数据的转换矩阵；转换模块405，用于根据转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
设备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 (14)
1.一种机器人，其特征在于，所述机器人包括：
存储器、处理器和传感器；
所述存储器存储有可执行程序代码；
与所述存储器耦合的所述处理器，调用所述存储器中存储的可执行程序代码，执行如下室内定位方法，所述方法包括：
通过所述传感器获取传感器数据；
获取所述机器人的在世界坐标系下的低频位姿数据；
获取基于所述传感器数据所计算得到的所述机器人在机器人坐标系下的高频位姿数据；
利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；包括：以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，以所述转换矩阵为优化目标，并构建所述机器人的绝对位置误差，建立非线性最小二乘问题；以图优化求解算法求解所述非线性最小二乘问题，将所述非线性最小二乘问题的最优解作为所述高频位姿数据到所述低频位姿数据的转换矩阵；
根据所述转换矩阵，将高频位姿数据转换为在所述世界坐标系下的定位位姿数据。
2.如权利要求1所述的机器人，其特征在于，所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述获取所述机器人的低频位姿数据的步骤包括：
以所述机器人内置的GPS获取的定位数据作为所述低频位姿数据，或者，将所述传感器数据与世界地图进行匹配，以获取所述低频位姿数据。
3.如权利要求1所述的机器人，其特征在于，所述传感器包括视觉相机和惯性测量单元，所述传感器数据包括视觉数据和惯性导航数据，所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述获取所述机器人基于传感器数据所计算得到所述机器人的高频位姿数据的步骤包括：
基于所述视觉相机获取所述视觉数据以及基于所述惯性测量单元获取所述惯性导航数据；
将所述视觉数据和所述惯性导航数据对齐；
从对齐后视觉数据提取特征点并跟踪所述特征点，以得到相邻两帧图像的视觉重投影误差；
对对齐后惯性导航数据进行预积分，以得到相邻两个时刻之间的惯性残差；
根据所述视觉重投影误差和惯性残差，计算得到在机器人坐标系下的高频位姿数据。
4.如权利要求3所述的机器人，其特征在于，所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述从对齐后视觉数据提取特征点并跟踪所述特征点，以得到相邻两帧图像的重投影误差数据的步骤包括：从所述对齐后视觉数据中选取两帧目标图像，并提取所述两帧目标图像的特征点；通过LK稀疏光流算法对所述两帧目标图像的特征点进行跟踪，以获取所述两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；
所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述对对齐后惯性导航数据进行预积分，以得到相邻两个时刻之间的惯性残差的步骤包括：对两帧对齐后惯性导航数据对应的三轴加速度和三轴角速度信息进行预积分处理，以获取所述两帧对齐后惯性导航数据的第二相对位姿，并获取相应的惯性残差；
所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述根据所述视觉重投影误差和惯性残差，计算得到在机器人坐标系下的高频位姿数据的步骤包括：根据所述视觉重投影误差和所述惯性残差对所述第一相对位姿和所述第二相对位姿进行优化，以获取在机器人坐标系下的高频位姿数据。
5.如权利要求1所述的机器人，其特征在于，所述传感器包括视觉相机和惯性测量单元，所述传感器数据包括视觉数据和惯性导航数据，所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述获取所述机器人基于传感器数据所计算得到所述机器人的高频位姿数据的步骤包括：
基于所述视觉相机获取所述视觉数据以及基于所述惯性测量单元获取所述惯性导航数据；
将所述视觉数据和所述惯性导航数据对齐；
确定对齐后惯性导航数据中惯性测量单元在第一时刻对应的标准位姿数据和所述惯性测量单元在第二时刻对应的待修正位姿数据，所述第一时刻和第二时刻分别为相邻两个时刻的在先时刻和在后时刻；
基于所述惯性测量单元在所述第一时刻对应的标准位姿数据以及在所述第二时刻对应的待修正位姿数据，确定所述视觉相机在所述第二时刻相对所述第一时刻的第二相对位姿数据；
基于第一相对位姿数据和所述第二相对位姿数据，对所述惯性测量单元在所述第二时刻对应的待修正位姿数据进行修正，得到所述惯性测量单元在所述第二时刻对应的标准位姿数据，作为所述机器人的高频位姿数据；所述第一相对位姿数据根据视觉相机在相邻时刻采集的视觉数据确定。
6.如权利要求1所述的机器人，其特征在于，所述处理器调用所述存储器中存储的可执行程序代码，执行的室内定位方法中所述根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据的步骤包括：
以下述公式计算所述定位位姿数据：
其中，所述T2为所述转换矩阵，所述为所述转换矩阵的逆矩阵，所述T1为所述高频位姿数据，所述T3为所述机器人在世界坐标系下的定位位姿数据。
7.一种室内定位装置，其特征在于，所述装置包括：
获取模块，用于获取传感器数据和机器人在世界坐标系下的低频位姿数据；
融合模块，用于获取所述机器人基于传感器数据所计算得到所述机器人在机器人坐标系下的高频位姿数据；
同步模块，用于利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
计算模块，用于以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；包括：问题构建单元，用于以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，所述转换矩阵为优化目标，并构建所述机器人的绝对位置误差，建非线性最小二乘问题；问题求解单元，用于以图优化求解算法求解所述非线性最小二乘问题，将所述非线性最小二乘问题的最优解作为所述高频位姿数据到所述低频位姿数据的转换矩阵；
转换模块，用于根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
8.如权利要求7所述室内定位装置，其特征在于，所述融合模块包括：
数据获取单元，用于基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据；
数据对齐单元，用于将所述视觉数据和所述惯性导航数据对齐；
误差计算单元，用于从对齐后视觉数据提取特征点并跟踪所述特征点，得到相邻两帧图像的视觉重投影误差；
残差计算单元，用于对对齐后惯性导航数据进行预积分，以得到相邻两个时刻之间的惯性残差；
位姿计算单元，用于根据所述视觉重投影误差和惯性残差，以计算得到在机器人坐标系下的高频位姿数据。
9.如权利要求8所述室内定位装置，其特征在于，所述误差计算单元包括特征点提取单元和跟踪单元，所述残差计算单元包括预积分处理单元，所述位姿计算单元包括第一优化单元；
所述特征点提取单元，用于从所述对齐后视觉数据中选取两帧目标图像，并提取所述两帧目标图像的特征点；
所述跟踪单元，用于通过LK稀疏光流算法对所述两帧目标图像的特征点进行跟踪，以获取所述两帧目标图像的第一相对位姿，并获取相应的视觉重投影误差；
所述预积分处理单元，用于对两帧对齐后惯性导航数据对应的三轴加速度和三轴角速度信息进行预积分处理，以获取所述两帧对齐后惯性导航数据的第二相对位姿，并获取相应的惯性残差；
所述第一优化单元，用于根据所述视觉重投影误差和所述惯性残差对所述第一相对位姿和所述第二相对位姿进行优化，以获取在机器人坐标系下的高频位姿数据。
10.如权利要求7所述室内定位装置，其特征在于，所述融合模块包括：
数据获取单元，用于基于视觉相机获取视觉数据以及基于惯性测量单元获取惯性导航数据；
数据对齐单元，用于将所述视觉数据和所述惯性导航数据对齐；
第一确定单元，用于确定对齐后惯性导航数据中惯性测量单元在第一时刻对应的标准位姿数据和所述惯性测量单元在第二时刻对应的待修正位姿数据，所述第一时刻和第二时刻分别为相邻两个时刻的在先时刻和在后时刻；
第二确定单元，用于基于所述惯性测量单元在所述第一时刻对应的标准位姿数据以及在所述第二时刻对应的待修正位姿数据，确定所述视觉相机在所述第二时刻相对所述第一时刻的第二相对位姿数据；
修正单元，用于基于第一相对位姿数据和所述第二相对位姿数据，对所述惯性测量单元在所述第二时刻对应的待修正位姿数据进行修正，得到所述惯性测量单元在所述第二时刻对应的标准位姿数据，作为所述在机器人坐标系下的高频位姿数据；所述第一相对位姿数据根据视觉相机在相邻时刻采集的视觉数据确定。
11.如权利要求7所述室内定位装置，其特征在于，所述转换模块具体用于通过下述公式计算所述定位位姿数据：
其中，所述T2为所述转换矩阵，所述为所述转换矩阵的逆矩阵，所述T1为所述高频位姿数据，所述T3为所述机器人在世界坐标系下的定位位姿数据。
12.如权利要求7所述室内定位装置，其特征在于，所述获取模块具体用于直接以所述机器人内置的GPS获取的定位数据作为所述机器人的低频位姿数据，或者，将所述传感器数据与世界地图进行匹配，以获取所述机器人的低频位姿数据。
13.一种室内定位方法，其特征在于，所述方法包括：
获取传感器数据；
获取机器人在世界坐标系下的低频位姿数据；
获取所述机器人基于传感器数据所计算得到所述机器人在机器人坐标系下的高频位姿数据；
利用时间戳将所述低频位姿数据与所述高频位姿数据对齐；
以时间戳对齐的低频位姿数据和所述高频位姿数据为观测数据，计算所述机器人坐标系到所述世界坐标系下的转换矩阵；包括：以时间戳对齐的低频位姿数据和高频位姿数据为观测数据，以所述转换矩阵为优化目标，并构建所述机器人的绝对位置误差，建立非线性最小二乘问题；以图优化求解算法求解所述非线性最小二乘问题，将所述非线性最小二乘问题的最优解作为所述高频位姿数据到所述低频位姿数据的转换矩阵；
根据所述转换矩阵，将高频位姿数据转换为在世界坐标系下的定位位姿数据。
14.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序用于被处理器执行时实现室内定位方法，所述室内定位方法为权利要求1至6中的任一项所述机器人实现所述室内定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110875770.0A CN113587934B (zh) | 2021-07-30 | 2021-07-30 | 一种机器人、室内定位方法、装置和可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110875770.0A CN113587934B (zh) | 2021-07-30 | 2021-07-30 | 一种机器人、室内定位方法、装置和可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113587934A CN113587934A (zh) | 2021-11-02 |
| CN113587934B true CN113587934B (zh) | 2024-03-19 |
Family
ID=78253133
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110875770.0A Active CN113587934B (zh) | 2021-07-30 | 2021-07-30 | 一种机器人、室内定位方法、装置和可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113587934B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114167858B (zh) * | 2021-11-12 | 2022-11-11 | 广州文远知行科技有限公司 | 车辆的导航控制方法、装置、导航控制器和车辆控制系统 |
| CN114136316B (zh) * | 2021-12-01 | 2024-08-02 | 珠海一微半导体股份有限公司 | 基于点云特征点的惯导误差消除方法、芯片及机器人 |
| CN115342806B (zh) * | 2022-07-14 | 2024-09-10 | 歌尔股份有限公司 | 头戴显示设备的定位方法、装置、头戴显示设备及介质 |
| CN117434571B (zh) * | 2023-12-21 | 2024-03-15 | 绘见科技(深圳)有限公司 | 基于单天线确定设备绝对位姿的方法、mr设备及介质 |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109766758A (zh) * | 2018-12-12 | 2019-05-17 | 北京计算机技术及应用研究所 | 一种基于orb特征的视觉slam方法 |
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110047108A (zh) * | 2019-03-07 | 2019-07-23 | 中国科学院深圳先进技术研究院 | 无人机位姿确定方法、装置、计算机设备及存储介质 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN110706279A (zh) * | 2019-09-27 | 2020-01-17 | 清华大学 | 基于全局地图与多传感器信息融合的全程位姿估计方法 |
| CN111928847A (zh) * | 2020-09-22 | 2020-11-13 | 蘑菇车联信息科技有限公司 | 惯性测量单元位姿数据优化方法、装置及电子设备 |
| CN112230242A (zh) * | 2020-09-30 | 2021-01-15 | 深兰人工智能(深圳)有限公司 | 位姿估计系统和方法 |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| CN112781586A (zh) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | 一种位姿数据的确定方法、装置、电子设备及车辆 |
| CN112880687A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
-
2021
- 2021-07-30 CN CN202110875770.0A patent/CN113587934B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109766758A (zh) * | 2018-12-12 | 2019-05-17 | 北京计算机技术及应用研究所 | 一种基于orb特征的视觉slam方法 |
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110047108A (zh) * | 2019-03-07 | 2019-07-23 | 中国科学院深圳先进技术研究院 | 无人机位姿确定方法、装置、计算机设备及存储介质 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| CN110706279A (zh) * | 2019-09-27 | 2020-01-17 | 清华大学 | 基于全局地图与多传感器信息融合的全程位姿估计方法 |
| CN111928847A (zh) * | 2020-09-22 | 2020-11-13 | 蘑菇车联信息科技有限公司 | 惯性测量单元位姿数据优化方法、装置及电子设备 |
| CN112230242A (zh) * | 2020-09-30 | 2021-01-15 | 深兰人工智能(深圳)有限公司 | 位姿估计系统和方法 |
| CN112781586A (zh) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | 一种位姿数据的确定方法、装置、电子设备及车辆 |
| CN112880687A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors;Tong Qin等;《arXiv数据库》;第1-7页 * |
| 多模融合的室内定位算法研究;刘运航;《中国优秀硕士学位论文全文数据库 信息科技辑》(第5期);第I138-1249页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113587934A (zh) | 2021-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113587934B (zh) | 一种机器人、室内定位方法、装置和可读存储介质 | |
| US8698875B2 (en) | Estimation of panoramic camera orientation relative to a vehicle coordinate frame | |
| CN107888828B (zh) | 空间定位方法及装置、电子设备、以及存储介质 | |
| CN109506642B (zh) | 一种机器人多相机视觉惯性实时定位方法及装置 | |
| JP5992184B2 (ja) | 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム | |
| EP2959315B1 (en) | Generation of 3d models of an environment | |
| CN110873883B (zh) | 融合激光雷达和imu的定位方法、介质、终端和装置 | |
| Dong et al. | Lighting-invariant visual odometry using lidar intensity imagery and pose interpolation | |
| CN112880687B (zh) | 一种室内定位方法、装置、设备和计算机可读存储介质 | |
| CN110411457B (zh) | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 | |
| CN109631911B (zh) | 一种基于深度学习目标识别算法的卫星姿态转动信息确定方法 | |
| CN113066127B (zh) | 一种在线标定设备参数的视觉惯性里程计方法和系统 | |
| CN111665512A (zh) | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 | |
| CN114279434B (zh) | 一种建图方法、装置、电子设备和存储介质 | |
| CN113240813A (zh) | 三维点云信息确定方法及装置 | |
| CN112580683B (zh) | 一种基于互相关的多传感器数据时间对齐系统及其方法 | |
| CN112907746A (zh) | 电子地图的生成方法、装置、电子设备及存储介质 | |
| CN111353453A (zh) | 用于车辆的障碍物检测方法和装置 | |
| CN113580134A (zh) | 视觉定位方法、设备、机器人、存储介质及程序产品 | |
| CN115063480A (zh) | 位姿确定方法、装置、电子设备和可读存储介质 | |
| CN113252066B (zh) | 里程计设备参数的标定方法及装置、存储介质、电子装置 | |
| He et al. | Three-point-based solution for automated motion parameter estimation of a multi-camera indoor mapping system with planar motion constraint | |
| CN110930455B (zh) | 定位方法、装置、终端设备及存储介质 | |
| CN116246029A (zh) | 数据同步方法、装置、终端设备及计算机可读存储介质 | |
| CN116912310B (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 |