位姿融合方法、系统、终端、介质以及移动机器人与流程

本申请涉及一种导航定位领域，特别是涉及一种位姿融合方法、系统、终端、介质以及移动机器人。

背景技术：

随着计算机技术的迅速发展，机器人研究的深入以及人们对机器人需求的扩大，能自主导航与智能移动的机器人成为研究的热点和重点。

对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中，机器人不能利用全局定位系统进行定位，而且事先获取机器人工作环境的地图也比较有难度。这时机器人需要在自身位置不确定的条件下，在完全未知环境中创建地图,同时利用地图进行自主定位和导航。而slam(同步定位与地图构建)技术，即是被认为目前实现真正全自主移动机器人的核心和关键技术。

随着激光雷达以及机器人视觉等技术的发展，多传感器融合融合技术已经成为地面移动机器人导航与定位的发展趋势。但是通过激光雷达slam得出的位姿tl与通过视觉得出的位姿tv由于环境和传感器特性，常常会有偏离。二者的输出频率也有所不同，激光雷达slam的运算速度更快，理论上能达到的帧率更高，但常常被激光雷达传感器的转速所限制；摄像头的帧率较高，但视觉算法的计算量更高，主要受到计算平台的限制。两种位姿的输出也是非同步的。进而使利用以上两种方式得到的环境地图出现了不一致的情况，进而令所述移动机器人的定位及导航工作效率降低。

申请内容

鉴于以上所述现有技术的缺点，本申请的目的在于提供一种位姿融合方法、系统、终端、介质以及移动机器人，用于解决通过激光雷达slam得出的位姿tl与通过视觉得出的位姿t由于环境和传感器特性，常常会有偏离。二者的输出频率也有所不同，激光雷达slam的运算速度更快，理论上能达到的帧率更高，但常常被激光雷达传感器的转速所限制；摄像头的帧率较高，但视觉算法的计算量更高，主要受到计算平台的限制。两种位姿的输出也是非同步的。进而使利用以上两种方式得到的环境地图出现了不一致的情况，进而令所述移动机器人的定位及导航工作效率降低的问题。

为实现上述目的及其他相关目的，本申请提供一种位姿融合方法，包括：将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，获得用于环境地图构建的融合位姿信息。

于本申请的一实施例中，所述方法包括：根据所述第一位姿信息与所述第二位姿信息预测当前状态的先验值和先验方差矩阵；根据所述第一位姿信息与所述第二位姿信息预测当前状态的后验值和后验方差矩阵；根据所述先验值、先验方差矩阵、后验值以及后验方差矩阵获得融合位姿信息。

于本申请的一实施例中，所述方法还包括：根据所述融合位姿信息获得定位信息，以用于构建环境地图。

于本申请的一实施例中，所述激光雷达获得所述第一位姿信息的方式包括：获得障碍物点信息以及imu数据；基于slam算法获得第一位姿信息。

于本申请的一实施例中，所述视觉摄像头获得所述第二位姿信息的方式包括：

获得拍摄的连续图像；提取角点及计算描述子并与上一帧图像进行匹配；根据匹配结果计算获得第二位姿信息。。

为实现上述目的及其他相关目的，本申请提供一种位姿融合系统，包括：处理模块，用于将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，得到用于获得更准确的定位的融合位姿信息。

于本申请的一实施例中，所述处理模块包括：先验单元，用于根据所述第一位姿信息与所述第二位姿信息预测当前状态的先验值和先验方差矩阵；后验单元，用于根据所述第一位姿信息与所述第二位姿信息预测当前状态的后验值和后验方差矩阵；融合位姿获得单元，连接所述先验单元且连接于所述后验单元，用于根据所述先验值、先验方差矩阵、后验值以及后验方差矩阵获得融合位姿信息。

为实现上述目的及其他相关目的，本申请提供一种位姿融合终端，包括：存储器，用于存储计算机程序；处理器，运行所述计算机程序，以执行所述位姿融合方法。

为实现上述目的及其他相关目的，本申请提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被运行时实现所述位姿融合方法。

为实现上述目的及其他相关目的，本申请提供一种移动机器人，包括：一或多个所述位姿融合终端。

如上所述，本申请的位姿融合方法、系统、终端、介质以及移动机器人，具有以下有益效果：本申请为了将由激光雷达以及机器人视觉分别得到的位姿融合起来，使用了基于ekf的算法融合框架得到融合后的位姿，使得获得的环境地图更加准确，进而令所述移动机器人的定位及导航工作效率升高。

附图说明

图1显示为本申请一实施例中位姿融合方法的流程示意图。

图2显示为本申请一实施例中位姿融合方法的流程示意图。

图3显示为本申请一实施例中位姿融合系统的结构示意图。

图4显示为本申请一实施例中处理模块的结构示意图。

图5显示为本申请一实施例中位姿融合终端的结构示意图。

具体实施方式

以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，在下述描述中，参考附图，附图描述了本申请的若干实施例。应当理解，还可使用其他实施例，并且可以在不背离本申请的精神和范围的情况下进行机械组成、结构、电气以及操作上的改变。下面的详细描述不应该被认为是限制性的，并且本申请的实施例的范围仅由公布的专利的权利要求书所限定。这里使用的术语仅是为了描述特定实施例，而并非旨在限制本申请。空间相关的术语，例如“上”、“下”、“左”、“右”、“下面”、“下方”、““下部”、“上方”、“上部”等，可在文中使用以便于说明图中所示的一个元件或特征与另一元件或特征的关系。

再者，如同在本文中所使用的，单数形式“一”、“一个”和“该”旨在也包括复数形式，除非上下文中有相反的指示。应当进一步理解，术语“包含”、“包括”表明存在所述的特征、操作、元件、组件、项目、种类、和/或组，但不排除一个或多个其他特征、操作、元件、组件、项目、种类、和/或组的存在、出现或添加。此处使用的术语“或”和“和/或”被解释为包括性的，或意味着任一个或任何组合。因此，“a、b或c”或者“a、b和/或c”意味着“以下任一个：a；b；c；a和b；a和c；b和c；a、b和c”。仅当元件、功能或操作的组合在某些方式下内在地互相排斥时，才会出现该定义的例外。

本申请提供一种位姿融合方法，解决了通过激光雷达slam得出的位姿tl与通过视觉得出的位姿tv由于环境和传感器特性，常常会有偏离。二者的输出频率也有所不同，激光雷达slam的运算速度更快，理论上能达到的帧率更高，但常常被激光雷达传感器的转速所限制；摄像头的帧率较高，但视觉算法的计算量更高，主要受到计算平台的限制。两种位姿的输出也是非同步的。进而使利用以上两种方式得到的环境地图出现了不一致的情况，进而令所述移动机器人的定位及导航工作效率降低的问题。本申请为了将由激光雷达以及机器人视觉分别得到的位姿融合起来，使用了基于ekf的算法融合框架得到融合后的位姿，使得获得的环境地图更加准确，进而令所述移动机器人的定位及导航工作效率升高。

所述方法包括：

将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，获得用于环境地图构建的融合位姿信息。

下面以附图1为参考，针对本申请得实施例进行详细说明，以便本申请所述技术领域的技术人员能够容易地实施。本申请可以以多种不同形态体现，并不限于此处说明的实施例。

如图1所示，展示一实施例中位姿融合方法的流程示意图，即经过以下步骤；

步骤s11：将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，获得用于环境地图构建的融合位姿信息。

可选的，如图2所示为位姿融合方法在本实施例中的方法流程示意图，可以实现图1所述位姿融合方法的功能，所述位姿融合的方法包括：

步骤s21：根据所述第一位姿信息与所述第二位姿信息预测当前状态的先验值和先验方差矩阵。

可选的，将机器人各时刻的位姿记为x1，...，xk，其中k是离散时间下标。则系统运动和观测方程可表达为

其中，f为运动方程，g为观测方程，yk为激光雷达或视觉输出的第一位姿信息或第二位姿信息，uk为控制输入，wk为输入噪声，nk为观测噪声。一般我们将各个变量分布假设为高斯分布，并将系统在工作点附近线性化，得到

其中，a，c为状态转移矩阵，qk，rk为噪声的协方差矩阵

步骤s22：根据所述第一位姿信息与所述第二位姿信息预测当前状态的后验值和后验方差矩阵；

可选的，用分别表示x的后验概率和先验概率，我们需要求的即最大后验概率(map)根据贝叶斯法则可化简为：

该式的递推解可由下面的公式得到：

其中pk为xk的协方差矩阵。

由此可以得到先验值、先验值方差矩阵、后验值以及后验方差矩阵获得融合位姿信息

步骤s23：根据所述先验值、先验方差矩阵、后验值以及后验方差矩阵获得融合位姿信息。

可选的，所述位姿融合方法利用卡尔曼滤波器获得融合位姿信息。

可选的，根据所述融合位姿信息获得定位信息，进而构建环境地图。

可选的，所述激光雷达获得所述第一位姿信息的方式包括：获得障碍物点信息以及imu数据；基于slam算法获得第一位姿信息。

举例来说，使用3d激光雷达或是2d激光雷达来获得所述第一位姿信息；通过激光雷达获得的障碍物点信息与imu数据，算法在建立地图的同时，计算出一个激光雷达位姿即第一位姿信息；激光雷达slam分为两个线程。在前端线程，激光数据点通过两帧之间imu测量值计算出相对移动的初值，在对局部地图进行匹配之后，得到修正过的当前位姿。在后端线程，每过一段时间就更新地图信息，并对地图进行优化，提升地图的精度。

可选的，所述视觉摄像头获得所述第一位姿信息的方式包括：获得拍摄的多帧障碍物的图像；将不同帧的图像进行对比并计算获得第二位姿信息。

举例来说，视觉slam使用单目或者双目系统，分别对应一个摄像头或两个摄像头。通过对比前后两帧图像，计算出视觉位姿即第二位姿信息。其中视觉slam分为三个线程。前端线程计算图像的特征点，并与前一帧图像进行匹配。对匹配上的特征点进行三角化，从而获得3d的点云。后端线程对机器人位姿与3d点云一同进行优化，推测机器人的最新位姿，并调整局部的位姿和点云。回环线程检查当前场景是否与地图中的历史场景一致，在触发回环之后进行全局地图的修正。

与上述实施例原理相似的是，本申请提供一种位姿融合

处理模块，用于将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，得到用于获得更准确的定位的融合位姿信息。

以下结合附图提供具体实施例：

如图3所示展示本申请实施例中的一种位姿融合系统的结构示意图。

所述系统包括：

所述处理模块31，用于将由激光雷达获得的第一位姿信息与由视觉摄像头获得的第二位姿信息进行位姿融合，得到用于获得更准确的定位的融合位姿信息。

可选的，如图4显示所述实施例中的一种处理模块40，其中处理模块可以实现如图3所述的所述处理模块31，所述处理模块40包括：

先验单元41，用于根据所述第一位姿信息与所述第二位姿信息预测当前状态的先验值和先验方差矩阵；

后验单元42，用于根据所述第一位姿信息与所述第二位姿信息预测当前状态的后验值和后验方差矩阵；

融合位姿获得单元43，连接所述先验单元且连接于所述后验单元，用于根据所述先验值、先验方差矩阵、后验值以及后验方差矩阵获得融合位姿信息。

可选的，所述先验单元41以及所述后验单元42根据所述第一位姿信息与所述第二位姿信息预测当前状态的先验值和先验方差矩阵；根据所述第一位姿信息与所述第二位姿信息预测当前状态的后验值和后验方差矩阵的方式包括：将机器人各时刻的位姿记为x1，...，xk，其中k是离散时间下标。则系统运动和观测方程可表达为

其中，f为运动方程，g为观测方程，yk为激光雷达或视觉输出的第一位姿信息或第二位姿信息，uk为控制输入，wk为输入噪声，nk为观测噪声。一般我们将各个变量分布假设为高斯分布，并将系统在工作点附近线性化，得到

其中，a，c为状态转移矩阵，qk，rk为噪声的协方差矩阵

用分别表示x的后验概率和先验概率，我们需要求的即最大后验概率(map)根据贝叶斯法则可化简为：

该式的递推解可由下面的公式得到：

其中pk为xk的协方差矩阵。

由此可以得到先验值、先验值方差矩阵、后验值以及后验方差矩阵，再根据所述融合位姿获得单元43获得融合位姿信息。

可选的，所述位姿融合系统还包括卡尔曼滤波器，用于获得融合位姿信息。

可选的，所述处理模块31还根据所述融合位姿信息获得定位信息，进而构建环境地图。

可选的，所述激光雷达获得所述第一位姿信息的方式包括：获得障碍物点信息以及imu数据；基于slam算法获得第一位姿信息。

举例来说，使用3d激光雷达或是2d激光雷达来获得所述第一位姿信息；通过激光雷达获得的障碍物点信息与imu数据，算法在建立地图的同时，计算出一个激光雷达位姿即第一位姿信息；激光雷达slam分为两个线程。在前端线程，激光数据点通过两帧之间imu测量值计算出相对移动的初值，在对局部地图进行匹配之后，得到修正过的当前位姿。在后端线程，每过一段时间就更新地图信息，并对地图进行优化，提升地图的精度。

可选的，所述视觉摄像头获得所述第一位姿信息的方式包括：获得拍摄的多帧障碍物的图像；将不同帧的图像进行对比并计算获得第二位姿信息。

举例来说，视觉slam使用单目或者双目系统，分别对应一个摄像头或两个摄像头。通过对比前后两帧图像，计算出视觉位姿即第二位姿信息。其中视觉slam分为三个线程。前端线程计算图像的特征点，并与前一帧图像进行匹配。对匹配上的特征点进行三角化，从而获得3d的点云。后端线程对机器人位姿与3d点云一同进行优化，推测机器人的最新位姿，并调整局部的位姿和点云。回环线程检查当前场景是否与地图中的历史场景一致，在触发回环之后进行全局地图的修正。

如图5所示，展示本申请实施例中的位姿融合终端50的结构示意图。

所述电子装置50包括：存储器51及处理器52所述存储器51用于存储计算机程序；所述处理器52运行计算机程序实现如图1所述的位姿融合方法。

可选的，所述存储器51的数量均可以是一或多个，所述处理器52的数量均可以是一或多个，所而图5中均以一个为例。

可选的，所述电子装置50中的处理器52会按照如图1述的步骤，将一个或多个以应用程序的进程对应的指令加载到存储器51中，并由处理器52来运行存储在存储器51中的应用程序，从而实现如图1所述位姿融合方法中的各种功能。

可选的，所述存储器51，可能包括但不限于高速随机存取存储器、非易失性存储器。例如一个或多个磁盘存储设备、闪存设备或其他非易失性固态存储设备；所述处理器51，可能包括但不限于中央处理器(centralprocessingunit，简称cpu)、网络处理器(networkprocessor，简称np)等；还可以是数字信号处理器(digitalsignalprocessing，简称dsp)、专用集成电路(applicationspecificintegratedcircuit，简称asic)、现场可编程门阵列(field－programmablegatearray，简称fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

可选的，所述处理器52可以是通用处理器，包括中央处理器(centralprocessingunit，简称cpu)、网络处理器(networkprocessor，简称np)等；还可以是数字信号处理器(digitalsignalprocessing，简称dsp)、专用集成电路(applicationspecificintegratedcircuit，简称asic)、现场可编程门阵列(field－programmablegatearray，简称fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

本申请还提供计算机可读存储介质，存储有计算机程序，所述计算机程序运行时实现如图1所示的位姿融合方法。所述计算机可读存储介质可包括，但不限于，软盘、光盘、cd-rom(只读光盘存储器)、磁光盘、rom(只读存储器)、ram(随机存取存储器)、eprom(可擦除可编程只读存储器)、eeprom(电可擦除可编程只读存储器)、磁卡或光卡、闪存、或适于存储机器可执行指令的其他类型的介质/机器可读介质。所述计算机可读存储介质可以是未接入计算机设备的产品，也可以是已接入计算机设备使用的部件。

综上所述，本申请位姿融合方法、系统、终端、介质以及移动机器人，解决了解决了通过激光雷达slam得出的位姿t_l与通过视觉得出的位姿t_v由于环境和传感器特性，常常会有偏离。二者的输出频率也有所不同，激光雷达slam的运算速度更快，理论上能达到的帧率更高，但常常被激光雷达传感器的转速所限制；摄像头的帧率较高，但视觉算法的计算量更高，主要受到计算平台的限制。两种位姿的输出也是非同步的。进而使利用以上两种方式得到的环境地图出现了不一致的情况，进而令所述移动机器人的定位及导航工作效率降低的问题。本申请为了将由激光雷达以及机器人视觉分别得到的位姿融合起来，使用了基于ekf的算法融合框架得到融合后的位姿，使得获得的环境地图更加准确，进而令所述移动机器人的定位及导航工作效率升高。所以，本申请有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本申请的原理及其功效，而非用于限制本申请。任何熟悉此技术的人士皆可在不违背本申请的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本申请所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本申请的权利要求所涵盖。