CN109900265A - 一种camera/mems辅助北斗的机器人定位算法 - Google Patents

Abstract

本发明属于导航定位技术领域，公开了一种camera/mems辅助北斗的机器人定位算法，包括：北斗卫星系统、惯性传感器、视觉传感器的时空同步；北斗卫星系统、惯性传感器、视觉传感器融合定位的数据预处理；北斗卫星系统、惯性传感器、视觉传感器融合的精密定位；其中，视觉传感器、惯性传感器搭载于机器人上。本发明解决了现有技术中移动机器人室内外导航定位精度较低的问题，能够为机器人提供厘米到分米级精度的导航定位服务。

Description

一种camera/mems辅助北斗的机器人定位算法

技术领域

本发明涉及导航定位技术领域，尤其涉及一种camera/mems辅助北斗的机器人定位算法。

背景技术

中国北斗卫星导航系统(BeiDouNavigation Satellite System，BDS)是中国自行研制的全球卫星导航系统。是继美国全球定位系统(GPS)、俄罗斯格洛纳斯卫星导航系统(GLONASS)、欧洲伽利略卫星导航系统(Galileo satellite navigation system)之后第四个成熟的卫星导航系统。2018年12月27日，北斗系统服务范围由区域扩展为全球，北斗系统正式迈入全球时代。在目前国内大部分机器人导航系统采用GPS的情况下，本算法采用BDS提供精密定位技术。相较于GPS(Global Positioning System)而言，BDS是主动式双向测距二维导航，由地面中心控制系统解算，提供给用户三维定位数据，且具有经济实用、定位精确、信息保密等优点，并且支持短报文通信功能，这点GPS并不具备。

随着近十几年来微机电系统(MEMS)工艺的进步，MEMS器件的性能得到了迅速的提高，其体积小、成本低、外围电路简单等优点为移动机器人，尤其是微小型移动机器人导航定位系统的设计提供了新的思路。特别是随着微电子技术和计算机软件技术向着纵深和交叉方向发展，采用DSP、ARM、CPLD等微处理器并移植操作系统内核的软硬件协同架构，使得系统体积微小型化，功耗大大降低，设计和维护升级更加便捷，这已成为机器人领域嵌入式系统设计的主流。

基于视觉等传感器的SLAM定位技术是近年来发展起来的一种新型定位方法，它通过安装在移动载体上的摄像机拍摄当前环境的图像信息，利用图像识别和匹配等技术跟踪环境中的特征，再经由立体几何与三维重建等技术恢复像机的位姿。根据系统中安装的相机个数，视觉SLAM可以分为单目系统、双目系统和与多目系统。在单目系统中相机姿态和相对位置变化可直接由单一摄像头获得，双目和多目视觉系统则通过多个摄像头对同一空间模板的识别与跟踪，可重建周围环境的三维空间信息并计算出相机的6自由度姿态变化。受益于成像技术、深度学习、计算机视觉的发展，基于视觉传感器的定位技术成为近年来的研究热点。视觉传感器灵敏度很高，加工成本较低，更为重要的是除定位外，其图像观测信息本身也是智能机器人进行环境感知和与外界交换的重要途径。特别地，视觉作为一种自主的测量方式，在卫星导航受干扰、遮挡而无法工作的情况下仍能有效发挥作用，相对惯性导航具有良好的误差累积抑制能力，在导弹武器、火星探测机器人、无人驾驶汽车、小型无人机等领域已经得到有效的应用，并具有进一步小型化、低成本化能力，发展空间十分广阔。因此，通过视觉传感器定位，实现机器人的智能行驶，是一项非常具有挑战性的技术，同时，也会是导航领域的未来发展的趋势，具有很高研究价值。

发明内容

本申请实施例通过提供一种camera/mems辅助北斗的机器人定位算法，解决了现有技术中移动机器人室内外导航定位精度较低的问题。

本申请实施例提供一种camera/mems辅助北斗的机器人定位算法，包括：

北斗卫星系统、惯性传感器、视觉传感器的时空同步；

北斗卫星系统、惯性传感器、视觉传感器融合定位的数据预处理；

北斗卫星系统、惯性传感器、视觉传感器融合的精密定位；

其中，所述视觉传感器、所述惯性传感器搭载于机器人上。

优选的，所述北斗卫星系统、惯性传感器、视觉传感器的时空同步包括：空间同步和时间同步；

所述空间同步包括对惯性传感器、视觉传感器、北斗卫星系统的接收机这三种传感器的相对位置进行标定，并在北斗卫星系统、惯性传感器、视觉传感器三者融合时加以改正；

所述时间同步包括设定统一的时间基准，同步触发传感器，并对不同频率的传感器数据进行对齐。

优选的，所述空间同步在进行北斗卫星系统、惯性传感器、视觉传感器三者融合时，事先对三者之间的杆臂和相对姿态进行标定，在使用测量数据时，使用合适的参考系加以描述，使用杆臂改正模型及不同传感器的基准。

优选的，所述时间同步在进行三者融合时，使用内插的方法，将惯性传感器机械编排的结算结果对齐到北斗卫星系统和视觉传感器输出的时间整点上。

优选的，所述北斗卫星系统、惯性传感器、视觉传感器融合定位的数据预处理中，经由北斗卫星系统、惯性传感器、视觉传感器分别同时获取当前时刻载体的位置、速度与姿态信息，并将北斗卫星系统、惯性传感器、视觉传感器三者获得的信息进行两两组合，对单一传感器进行数据的检核与粗差探测。

优选的，所述北斗卫星系统、惯性传感器、视觉传感器融合的精密定位包括：

构建视觉传感器与惯性传感器的紧融合模型；

构建北斗卫星系统与视觉传感器、惯性传感器的松融合模型。

优选的，构建所述紧融合模型中，北斗卫星系统的卫星观测数较少或观测条件较差时，采用视觉惯导SLAM技术实现对移动机器人的定位定姿。

优选的，构建所述的松融合模型中，在北斗卫星系统的观测条件较好的情况下，通过北斗卫星系统的全局定位结果对视觉惯导融合定位结果进行修正。

优选的，所述北斗卫星系统、惯性传感器、视觉传感器融合的精密定位还包括：

通过惯性传感器的三轴加速度计和陀螺仪的输出结果在三维空间的积分预测载体的运动情况，对融合定位滤波器的状态向量和滤波方差进行时间更新；

将提取到的视觉特征点数据和北斗卫星系统获取的位置信息作为融合定位滤波器的量测更新机制，更新系统状态向量和滤波方差矩阵。

优选的，所述camera/mems辅助北斗的机器人定位算法还包括：

利用视觉传感器与惯性传感器融合定位结果输出的载体位置信息构建虚拟观测值，虚拟观测值参与北斗卫星系统的模糊度求解，辅助北斗卫星系统模糊度固定；

将视觉传感器与惯性传感器融合定位结果输出的载体位置信息作为约束，缩小北斗卫星系统的模糊度的搜索空间。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

在本申请实施例中，基于移动机器人平台，利用北斗卫星导航系统和机器人平台搭载的视觉传感器、惯性传感器等实现多源融合导航，为机器人提供厘米到分米级精度的导航定位服务。本发明建立融合北斗/视觉/IMU多源导航定位模型，突破移动机器人实时高精度的室内外无缝导航定位这一技术瓶颈。

附图说明

为了更清楚地说明本实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一个实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种camera/mems辅助北斗的机器人定位算法的框架示意图；

图2为视觉惯导组合系统的框架示意图；

图3为本发明提供的一种camera/mems辅助北斗卫星系统的机器人智能导航定位算法中空间同步的示意图。

具体实施方式

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

本实施例提供了一种camera/mems辅助北斗的机器人定位算法，如图1所示，主要包括以下步骤：

步骤1，预处理GNSS数据，推导和建立多频多系统的PPP与RTK融合解算函数模型。通过对观测数据的分析，比较在不同参考星条件下的定位精度，获得最佳的参考星选择策略。

其中，随机模型拟采用赫尔默特方差分量估计进行随机模型精化，以获取最优结果。

步骤2，根据步骤1所得模型，利用卡尔曼滤波实时求解，研究快速确定模糊度的方法，如利用估计得到的相位小数偏差来固定模糊度。针对多频多系统，选取最优组合观测值，进行模糊度搜索，以最大程度上减少模糊度固定的时间。

其中，确定模糊度，在固定策略上可采用Geo-Free与Geo-Based相结合的方式。对高维模糊度的快速解算，从降相关、快速搜索两方面开展，或尝试采用部分模糊度固定的方法解决。

步骤3，对GNSS、惯性传感器(Inertial measurement unit，IMU)、视觉传感器等多源传感器进行时空同步，确定不同传感器之间的外参关系，融合时用于数据修正。时间同步中设定统一的时间基准，同步触发传感器，对不同频率的传感器数据进行对齐。

其中，确定时间同步的方案，需要对相机的内参和外参进行标定，应分别设计实验场标定以及自标定两种标定方式，尝试以实验场标定为真值的方式，评估自标定方式的可靠性，并对两种标定方案的标定精度进行评估。

步骤4，采用预积分的方式对IMU数据进行处理，初始化模块采用运动恢复结构(structure from motion，SFM)获得初始位姿，并与IMU预积分数据对准，粗略的重构出尺度、重力、速度等参数。

步骤5，根据步骤4中IMU提供的位置与姿态等信息，辅助视觉导航进行图像匹配、位姿预报，异常特征点剔除以及精度优化，并建立两者基于非线性优化的紧融合方案。

其中，建立基于非线性优化的紧融合方案，采用滑动窗口的策略和参数边缘化的估计模型。

步骤6，根据步骤2和步骤5提供的数据信息，通过松组合融合定位，搭建通用的智能移动机器人平台，建立基于模仿学习的机器人路径规划策略，为机器人提供室内外无缝衔接的厘米到分米级精度的导航定位。

步骤7，对机器人进行测试，通过大量的数据处理分析对算法进行验证、改进和完善，进一步修正相应的模型与算法，提高导航定位的精度、计算速度。

如图2所示，下面对本发明涉及的主要部分做进一步的说明。

(1)视觉与IMU融合

步骤一，观测数据的预处理。主要有图像特征检测与跟踪以及IMU数据的预积分，当有新的图像进入时，系统采用KLT稀疏光流法进行跟踪，并提取100-300个角点的信息。其中，光流跟踪用来快速定位，角点用于构建图像，同时预处理部分还进行关键帧的选取。IMU预积分的作用除了计算IMU数据的观测值及其协方差矩阵外，还为视觉惯导的联合初始化提供初值，为后端优化提供IMU约束关系。

步骤二，测量初始化，通过将IMU预集成并与仅限视图的结构松散地对齐为后续基于非线性优化的VIO引导提供了所有必要的值。利用五点算法实现两帧间的相对旋转和缩放平移。然后，任意设置尺度，对这两帧中观察到的所有特征进行三角剖分。基于这些三角化特征，采用透视n点(PnP)方法估计窗口中所有其他帧的位姿。最后，应用全局全包调整，使所有特征观测的总重投影误差最小。

将第一个camera frame(·)c0设置为SFM的参照系。所有框架构成 和功能位置对(·)c0表示。给定相机与IMU之间的外部参数可以将姿态从摄像机帧转换为body(IMU)帧。

步骤三，局部视惯性捆绑与再定位包含两个主要功能，第一个是基于滑动窗口的BA优化模型，第二个是重定位功能。在VINS中，将滑动窗口内的状态向量整合到一个状态向量中，如下：

其中，第一个式子表示滑动窗口内的整个状态向量，n表示帧数，λ是特征点深度值的逆，m表示滑动窗口中特征点的总数，整个式子的维度是15×n+6+m。第二个式子中，Xk表示在第K帧捕获的IMU状态，主要包括位姿、速度、旋转以及加速度计和陀螺仪的零偏。第三个式子是相机的外参。由以上参数可以看出，Xk只与IMU项有关，λ只与相机有关，下面建立视觉+惯导的BA优化模型:

其中，分别是IMU和视觉测量的残差部分，B表示IMU测量值集合，C是特征点的集合，{r_p，H_p}是先验信息，这些特征点要求在当前窗口被观测至少两次，是先验信息，这个非线性问题可以用CeresSolver求解。

重定位功能主要是在检测到闭环后，对系统进行一次优化，并对误差进行矫正，重定位功能离不开闭环检测模块，闭环检测模块检测到当前场景与之前到过场景的相似度很高时，就会确认形成闭环，这时重定位就会启动。

步骤四，全局图优化，只针对全局范围。在重新定位之后，算法还会进行全局位姿图优化步骤，使过去和现在的位姿在全局协调一致，达到优化载体姿态的目的。

步骤五，回环检测，检测到回环可以显著提高整体定位精度。本发明的滑动窗口和边界化方案限制了计算的复杂性，但也为系统引入了累积漂移。为了消除漂移，提出了一种紧密耦合的、与单眼VIO无缝集成的relo-calization模块。重新定位过程从一个循环检测模块开始，该模块识别已经访问过的位置。然后，在循环闭包候选对象和当前帧之间建立特性级的连接。将这些特征对应紧密集成到单眼VIO模块中，以最小的计算量实现无漂移状态估计。多个特征的多个观测值直接用于重定位，具有更高的精度和更好的状态估计平滑性。

(2)GNSS、IMU、视觉等多源传感器的时空同步

多传感器的利用可以有效提高导航定位的精度和鲁棒性，并且降低成本。在利用多传感器数据进行导航定位时，需要注意不同传感器之间的时空同步问题，加以标定或改正。时空同步是多传感器数据有效融合的前提条件，主要包括空间同步和时间同步，具体如图3所示。

在GNSS、IMU、视觉三者融合中，GNSS求解的位置以GNSS天线中心为基准，在地心地固系下描述；IMU解算的位置和速度以IMU中心为基准，其测量数据在通常以IMU或载体坐标系下描述；相机观测到的地图点坐标以相机中心为基准，其测量值在相机坐标系下描述。因此，进行三者融合时，需要事先对不同传感器之间的杆臂和相对姿态进行标定；在使用测量数据时，要使用合适的参考系(三者融合的统一坐标系)加以描述，使用杆臂改正模型改正不同传感器之间的相对位置偏移。

时间同步需要解决不同传感器输出时刻不统一和输出频率不同的问题。考虑到不同传感器之间的时间基准不同，需要设定统一的时间基准，使用硬件同步或是软件同步的方法，同步触发传感器输出测量值，误差应控制在数毫秒以内。另外需要注意，在使用多目相机时，需要在硬件上实现各个相机的同步曝光，以提供可靠的多目视觉测量数据。在GNSS、IMU、视觉三者融合中，IMU典型的输出频率为200Hz，GNSS典型的输出频率为1Hz，相机的帧数为20Hz左右。即使使用统一的时间基准，传感器的输出时刻往往不对齐。在进行三者融合时，需要使用内插的方法，将IMU机械编排的结算结果对齐到GNSS和相机输出的时间整点上。

(3)传感器融合定位的数据预处理

在传感器融合定位中数据预处理主要包括视觉观测值的粗差剔除、特征点深度的粗差剔除、伪距观测值的粗差剔除、载波相位观测值的周跳探测、接收机钟跳的探测与修复等内容。由于双目相机受所处环境光线、纹理信息的影响，其跟踪的像素点可能存在着错误跟踪、匹配的情况。而GNSS观测值受通视条件、多路径效应的影响，其动态解算位置可能会较大程度偏离真值。为了避免这些粗差对系统产生影响，提高系统的稳定性和连续性，本项目提出了多传感器融合定位的数据处理新方法。

在移动机器人运动期间，若GNSS的信号通视良好，卫星观测数较多则可以利用GNSS精密定位结果求解除出机器人平台的在该段时间内的位置、速度变化。惯性传感器可以在不借助外界条件的情况下，以固定采样间隔的获取其自身加速度、角速度的连续观测数据，在短时间内通过对其积分的同样能够大致预测出机器人的位置、速度、姿态变化。而视觉传感器通过图像识别、匹配等方法可以对周围环境中特征点的连续跟踪，同样可以获取其自身位置、姿态的变化。在同一时刻，经由三种不同的传感器可以同时获取当前时刻载体的位置、速度与姿态信息，进而可以将其两两组合，从而对剩余传感器进行数据的检核与粗差探测。例如，将IMU与视觉相融合，可以进行周跳探测与修复，剔除弱通视条件或严重多路径效应下PPP的粗差解。将IMU与GNSS融合可以对图像特征点的跟踪与位姿恢复进行检验，将误差较大的图像帧剔除。

(4)GNSS、IMU、视觉等多源传感器融合的精密定位

在多源传感器融合定位中，GNSS卫星观测数较少或观测条件较差时，采用视觉惯导SLAM技术实现对智能移动机器人的定位定姿。其中，对相机采集的图像进行特征提取与匹配是视觉导航的第一步，由关键点和描述子组成。为由零偏估计所带来对IMU观测值的反复积分产生，采用预积分的方式对IMU数据进行处理。充分利用IMU提供的位置与姿态信息，辅助视觉导航进行图像匹配、位姿预报，异常特征点剔除以及精度优化。该视觉与IMU紧融合模型在(1)视觉与IMU融合部分已进行了详细描述。

而在GNSS观测条件较好的情况下，则通过GNSS高精度的全局定位结果对视觉惯导融合定位结果进行修正，消除累计误差。对于多源传感器融合定位的滤波器设计上，在对各传感器观测数据预处理的基础上，用惯性传感器观测量预测载体的运动情况，对滤波器的状态向量和滤波方差进行时间更新。同时，利用视觉特征追踪数据和GNSS提供的位置信息同时作为滤波器的量测更新机制，更新系统状态向量和滤波方差矩阵。

(5)基于传感器融合的GNSS快速精密定位及模糊度固定理论与方法

GNSS接收机发生信号失锁时，此时利用传统的LAMBDA搜索方法往往需要较长的时间才能获得可靠的整周模糊度。通过单一GNSS接受机往往很难实现模糊度的快速固定，而多源传感器的融合能够带来的丰富信息，通过其它传感器融合后的定位结果可以构建虚拟观测值，参与模糊度的求解，辅助GNSS模糊度固定。同时其他传感器的位置信息可以作为约束，缩小模糊度的搜索空间，提高搜索效率，攻克PPP初始化时间长、可靠性偏低的难题。

多源传感器融合后，视觉与IMU的融合定位结果可以持续输出较为精确的载体的位置坐标。上述两种传感器组合输出的位置信息可以构建虚拟观测值，参与模糊度的求解，辅助GNSS模糊度固定。同时也可以将其作为约束，缩小模糊度的搜索空间，显著缩短固定整周模糊度所需要的时间，进而提升传感器融合后GNSS精密定位的精度与可靠性，实现快速精密定位。

综上，本申请实施例提供一种camera/mems辅助北斗的机器人定位算法，构建多频多系统观测数据的预处理算法与随机模型、对PPP与RTK进行融合解算，固定整周模糊度；同时建立融合BDS/视觉/IMU的导航定位参数估计的组合模型；通过松组合融合定位后，搭建通用的智能移动机器人平台，建立基于模仿学习的机器人路径规划策略，为机器人提供室内外无缝衔接的厘米到分米级精度的导航定位。

本发明以卫星精密定位为核心进行室外定位，以视觉/IMU导航为主提供室内路径规划与导航，实现室内外无缝导航定位，通过智能多模态移动机器人平台研制，采用模仿学习的技术实现机器人的路径规划算法。

本发明实施例提供的一种camera/mems辅助北斗的机器人定位算法至少包括如下技术效果：

1)本发明建立PPP与RTK融合解算模型，融合了两者模糊度固定快速、作业方式灵活等显著优势，同时回避了通讯负担较重、定位效率较低等问题，实现系统间自由灵活的无缝切换。

2)在复杂环境下，单一的导航技术容易出现错误，导致定位失败。本发明充分挖掘北斗/视觉/IMU三者的导航定位特性，完成三者的时空基准统一。并针对实时与高精度两大特性，建立三者融合的导航定位参数估计的组合模型，实现一套完善的精度评价体系。

3)充分利用我国北斗卫星导航系统资源，研究以BDS为中心的精密定位技术，提供分米/厘米级的定位精度，增强自主装卸移动机器人对位置环境的适应性，发展实时性好的导航与定位技术。架起北斗导航与新一代移动机器人领域的一座桥梁，推进我国北斗与视觉领域的融合定位及其在各个领域的应用。

最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种camera/mems辅助北斗的机器人定位算法，其特征在于，包括：

北斗卫星系统、惯性传感器、视觉传感器的时空同步；

北斗卫星系统、惯性传感器、视觉传感器融合定位的数据预处理；

北斗卫星系统、惯性传感器、视觉传感器融合的精密定位；

其中，所述视觉传感器、所述惯性传感器搭载于机器人上。

2.根据权利要求1所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述北斗卫星系统、惯性传感器、视觉传感器的时空同步包括：空间同步和时间同步；

所述空间同步包括对惯性传感器、视觉传感器、北斗卫星系统的接收机这三种传感器的相对位置进行标定，并在北斗卫星系统、惯性传感器、视觉传感器三者融合时加以改正；

所述时间同步包括设定统一的时间基准，同步触发传感器，并对不同频率的传感器数据进行对齐。

3.根据权利要求2所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述空间同步在进行北斗卫星系统、惯性传感器、视觉传感器三者融合时，事先对三者之间的杆臂和相对姿态进行标定，在使用测量数据时，使用合适的参考系加以描述，使用杆臂改正模型及不同传感器的基准。

4.根据权利要求2所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述时间同步在进行三者融合时，使用内插的方法，将惯性传感器机械编排的结算结果对齐到北斗卫星系统和视觉传感器输出的时间整点上。

5.根据权利要求1所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述北斗卫星系统、惯性传感器、视觉传感器融合定位的数据预处理中，经由北斗卫星系统、惯性传感器、视觉传感器分别同时获取当前时刻载体的位置、速度与姿态信息，并将北斗卫星系统、惯性传感器、视觉传感器三者获得的信息进行两两组合，对单一传感器进行数据的检核与粗差探测。

6.根据权利要求1所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述北斗卫星系统、惯性传感器、视觉传感器融合的精密定位包括：

构建视觉传感器与惯性传感器的紧融合模型；

构建北斗卫星系统与视觉传感器、惯性传感器的松融合模型。

7.根据权利要求6所述的camera/mems辅助北斗的机器人定位算法，其特征在于，构建所述紧融合模型中，北斗卫星系统的卫星观测数较少或观测条件较差时，采用视觉惯导SLAM技术实现对移动机器人的定位定姿。

8.根据权利要求6所述的camera/mems辅助北斗的机器人定位算法，其特征在于，构建所述的松融合模型中，在北斗卫星系统的观测条件较好的情况下，通过北斗卫星系统的全局定位结果对视觉惯导融合定位结果进行修正。

9.根据权利要求6所述的camera/mems辅助北斗的机器人定位算法，其特征在于，所述北斗卫星系统、惯性传感器、视觉传感器融合的精密定位还包括：

通过惯性传感器的三轴加速度计和陀螺仪的输出结果在三维空间的积分预测载体的运动情况，对融合定位滤波器的状态向量和滤波方差进行时间更新；

将提取到的视觉特征点数据和北斗卫星系统获取的位置信息作为融合定位滤波器的量测更新机制，更新系统状态向量和滤波方差矩阵。

10.根据权利要求1所述的camera/mems辅助北斗的机器人定位算法，其特征在于，还包括：

利用视觉传感器与惯性传感器融合定位结果输出的载体位置信息构建虚拟观测值，虚拟观测值参与北斗卫星系统的模糊度求解，辅助北斗卫星系统模糊度固定；

将视觉传感器与惯性传感器融合定位结果输出的载体位置信息作为约束，缩小北斗卫星系统的模糊度的搜索空间。