[发明专利]一种面向视觉惯性定位的基于地图的一致高效滤波算法

说明书

本发明公开了一种面向视觉惯性定位的基于地图的一致高效滤波算法，通过以下系统来实现，系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，局部里程计用于接收相机和IMU的数据；地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对。本发明将维护的里程计相关变量以及局部里程计参考系和事先建好的地图参考系之间的相对变换表示在该李群上，进而得到了一种新的不变卡尔曼滤波算法，保证了算法在不考虑地图不确定性时的能维护系统的正确能观性。

技术领域

本发明涉及机器人定位技术，具体地说，涉及一种面向视觉惯性定位的基于地图的一致高效滤波算法。

背景技术

定位技术是智能机器人的一个必不可少的基础模块。近年来，相机和惯性测量单元(IMU)被广泛应用于定位技术中，催生出了一大批优秀的视觉惯性里程计 (VIO)算法。但是VIO的定位精度会随着算法运行时间的推移而不可避免地产生漂移。这就需要通过引入事先建好的地图来消除累积误差。在定位算法融合事先建好的地图信息时，系统需要保证一致、有效地融合这些地图信息，才能有效地使用地图信息来消除里程计的累积误差。

目前定位算法的框架分为两大类，一类是初始化局部里程计参考系与事先建好的地图参考系之间的相对变换后，将地图参考系下进行实时定位；另一类是同时维护局部的里程计以及局部里程计参考系和事先建好的地图参考系之间的相对变换，从而间接得到在地图参考系下的位置。第一类算法的局限在于对于视觉惯性定位而言，事先建好的地图必须是在惯性系下的，这限制了定位算法的应用。对于第二类算法，虽然没有了第一类算法的限制，但是这类框架存在的潜在问题——可能会破坏了系统的能观性，使原本不可观的信息变得可观，从而引入了虚假的观测信息，降低了定位精度甚至会使算法发散。

此外，绝大多数算法出于计算效率或者算法框架的限制，没有考虑地图的不确定性，认为地图是绝对准确的。这就会使得定位算法过度信赖不准确的地图，使定位效果大打折扣。即便有一些算法在上述框架中考虑了地图的不确定性，但是他们破坏了系统的能观性，使原本不可观的信息变得可观，从而引入了虚假的观测信息，降低了定位精度甚至会使算法发散。

如何在考虑地图不确定性的同时，保证系统的正确能观性，从而提升基于地图定位算法的精度和可靠性是需要解决的问题。与此同时，还需要保证系统的计算量保持在较低的水平。

发明内容

为了克服现有技术的不足，本发明提出了一种基于滤波的定位算法，基于预先建好的视觉地图的视觉惯性定位技术。所提出的视觉惯性定位技术是基于一种一致高效的滤波算法实现的。本发明是通过以下技术方案来实现的：

本发明公开了一种面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考系和地图参考系的相对变换(增广变量)，进而计算得到机器人在地图参考系下的状态。

作为进一步地改进，本发明所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的基于局部特征观测的MSOC-S-IKF状态更新模块。