CN114993338B

CN114993338B - 基于多段独立地图序列的一致性高效视觉惯性里程计算法

Info

Publication number: CN114993338B
Application number: CN202210303476.7A
Authority: CN
Inventors: 王越; 请求不公布姓名; 熊蓉
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2022-03-24
Filing date: 2022-03-24
Publication date: 2024-03-15
Anticipated expiration: 2042-03-24
Also published as: CN114993338A

Abstract

本发明公开了一种基于多段独立地图序列的一致性高效视觉惯性里程计算法。算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图匹配模块和基于多段地图信息的融合模块，局部里程计用于接收相机和IMU的数据；地图匹配用于获得当前时刻图像特征与各个地图序列之间的特征匹配对；基于多段地图信息通过融合来自于多段地图的特征来更新局部里程计的状态，能够将多段独立的地图信息融入视觉惯性里程计中，缓解里程计的漂移现象，并提升里程计的位姿估计精度，利用“第一次估计雅克比”(FEJ)算法，避免了因为引入地图信息而使系统的能观性遭到破坏的问题，从而使整个视觉惯性里程计系统保持一致性。

Description

基于多段独立地图序列的一致性高效视觉惯性里程计算法

技术领域

本发明涉及机器人定位技术，具体地说，涉及一种基于多段独立地图序列的一致性高效视觉惯性里程计算法。

背景技术

自主移动机器人需要实时获取自己在所处环境中的位姿信息。通常情况下，机器人的位姿信息是通过自身携带的传感器，利用里程计技术获得的。相机和惯性测量单元(IMU)由于其轻量和低成本的特性被广泛应用于移动机器人，并衍生出了视觉惯性里程计(VIO)。

然而，里程计不可避免地会随着机器人的移动距离的增加而产生漂移，从而使里程计估计的机器人本体位姿精度变差。为了缓解里程计漂移的问题，很多算法利用事先建好的地图来给机器人提供额外的信息，进而约束里程的漂移，使里程计的定位精度提升。但是目前绝大多数基于地图的算法都是基于一个全局一致的大地图。很多情况下，尤其是在大场景中，全局一致的地图是很难获取的。构建全局一致的大地图是一个费时费力的事情。这就可以考虑使用多段独立的小地图来辅助机器人定位。

如何一致地融合多段多里的小地图是现有技术需要着重考虑的问题。如果融合多段独立地图序列的算法存在不一致的问题，就会使系统对机器人位姿的估计过于乐观，从而大大降低系统的定位精度。这样一来，引入的额外的地图信息不但不能帮助提升里程计的估计精度，反而可能使系统表现出更差的效果。

发明内容

为了克服现有技术的不足，本发明提出了一种基于多段独立地图序列的一致性高效视觉惯性里程计算法，该算法基于滤波框架。本发明是通过以下技术方案来实现的：

本发明公开了一种基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图匹配模块和基于多段地图信息的融合模块，局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；地图匹配用于检测当前时刻相机所观测到的场景与事先获取的多段独立地图序列之间的相似度，获得当前时刻图像特征与各个地图序列之间的特征匹配对；基于多段地图信息的融合模块用于接收局部里程计的输出量以及特征匹配对，通过融合来自于多段地图的特征来更新局部里程计的状态，同时估计局部里程计的参考系与各个独立地图序列的参考系之间的相对变换。

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

作为进一步地改进，本发明IMU用于为系统提供实时的旋转角速度和线加速度；状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给观测融合状态更新模块，特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给观测融合状态更新模块；观测融合状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用扩展卡尔曼滤波(Extended Kalman Filter， EKF)来更新状态变量与协方差，整个过程与多状态约束卡尔曼滤波(MSCKF) 一致。

作为进一步地改进，本发明状态变量及其对应的协方差需要包含以下变量：

1)系统当前时刻本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度；

2)IMU传感器的关于角速度与线加速度的偏差(bias)；

3)系统过去B个时刻的本体位姿，B为所设定的滑动窗口的大小；

4)局部里程计参考系和多段地图序列的参考系之间的位姿变换其中N代表地图序列的个数，记/>为局部里程计参考系和第i段地图序列的参考系之间的位姿变换，变量需要通过在基于多段地图信息的融合模块中的初始化局部与地图间的相对位姿/>模块加入到状态变量中；

5)地图序列中关键帧的位姿，变量通过基于多段地图信息的融合模块中的将地图关键帧加入状态向量模块加入到状态变量中。

作为进一步地改进，本发明地图匹配模块用于通过当前相机与各个地图序列的信息，获得与当前相机匹配到的各个地图序列的特征点以及能观测到这些特征点的地图关键帧，即特征匹配对与多段地图序列的关键帧，特征匹配对与多段地图序列的关键帧信号传递至基于多段地图信息的融合模块。

作为进一步地改进，本发明基于多段地图信息的融合模块包括以下模块：

初始化局部与各个地图序列参考系之间的相对位姿模块，将地图关键帧加入状态向量模块和利用多段地图信息进行状态更新模块；初始化局部与各个地图序列参考系之间的相对位姿/>模块用于计算局部参考系和第i段地图序列参考系之间的相对位姿并将其加入到系统的状态向量中，获得增广的系统状态变量和协方差；将地图关键帧加入状态向量模块用于将地图关键帧的位姿和协方差加入系统的状态变量和系统的协方差中，从而天然地考虑了地图序列的不确定度，进而保证了系统的一致性，通过该模块我们获得了增广的状态变量和协方差；利用多段地图信息进行状态更新模块在满足1)考虑地图不确定度，2)维护系统的正确能观性，3) 系统的所需的运算量低的同时，将多段独立的地图序列高效、一致地融入局部里程计系统中，从而在提高局部里程计的定位精度的同时，还获取了各个独立地图之间的相对位置关系。

作为进一步地改进，本发明初始化局部与地图参考系之间的相对位姿模块，通过接收到的与第i段地图序列匹配的特征对和地图关键帧，利用PnP(Perspective-n-Point)算法求解地图关键帧和相机当前帧之间的相对位姿，并利用地图关键帧位姿以及当前时刻系统的位姿解算出完成局部参考系和第i段地图序列参考系之间的相对位姿的初始化；地图关键帧加入状态向量模块具体为：当第i段地图序列的地图帧与当前帧有特征匹配对的时候，且/>已经初始化，直接将地图帧加到状态向量模块中，当第i段地图序列的地图帧与当前帧有特征匹配对的时候，且/>还没有初始化，则通过初始化局部与地图参考系之间的相对位姿模块完成/>的初始化，然后将用于初始化的地图关键帧位姿信号传递到地图关键帧加入状态向量模块。

作为进一步地改进，本发明利用多段地图信息进行状态更新模块包括三个部分：

a)基于多段独立地图序列的观测方程；

b)基于Schmidt滤波的状态更新；

c)能观性保持。

作为进一步地改进，本发明a)部分观测方程分为两种情况；1)匹配到的来自第i段地图序列的特征点，只在当前帧和第i段地图关键帧中观测到；2)匹配到的来自第i段地图序列的特征点在当前帧和第i段地图关键帧中观测到的同时，还在第j(j≠i)段地图序列中的关键帧中观测到，针对情况1)，将特征点重投影到第i段地图关键帧，并通过将特征点重投影到当前帧，针对情况2)，将特征点重投影到第i段地图关键帧；通过/>将特征点重投影到当前帧；通过/>和/>将特征点重投影到第j段地图的关键帧；

在计算重投影误差的时候，需要求解观测方程关于地图点的雅克比矩阵，通过计算该雅克比矩阵的左零空间，将该左零空间乘到观测方程上，从而消去地图点特征对应的误差项，并且隐式地考虑了地图特征点的不确定性。

作为进一步地改进，本发明b)部分具体为：

利用Schmidt滤波算法来进行状态更新，用于在考虑地图信息不确定性的同时保持滤波算法的计算效率；对于得到的观测方程按照如下步骤进行Schmidt状态更新：

其中，下标t表示当前时刻，下标t|t-1表示状态传播模块输出的值，为重投影误差，/>为观测方程关于系统状态向量的雅克比，分为两部分，/> 和/>分别是与/>和/>相关的雅可比矩阵，其中/>和/>表示需要实时更新的状态部分和地图关键帧部分，R_t为观测噪声/>的协方差，P_t|t-1是状态传播后的状态协方差，根据/>和/>将其按块划分成：

为最终更新的机器人的状态。/>为地图关键帧部分的状态，该部分保持不变，上标^表示估计值；

c)部分能观性保持具体为：

为了保持正确的能观性，进而保证系统的一致性，提出了“第一次估计雅克比”算法(FEJ)用于基于多段地图信息的融合，“第一次估计雅克比”算法(FEJ)具体为：

·在状态传播计算雅克比矩阵Φ_t+1|t时，利用估计值来计算；

·在计算基于多段地图序列的观测方程的雅克比时，对于任意时刻t，当计算雅克比涉及到局部参考系和第i个地图的参考系之间的相对位姿变换时，始终利用/>初始化时候的值来计算。

本发明的有益效果如下：

1)能够将多段独立的地图信息融入视觉惯性里程计中，缓解里程计的漂移现象，并提升里程计的位姿估计精度。

2)利用Schmidt滤波，在考虑了多段地图信息不确定性的同时，保证了算法的计算效率，使系统的计算量随着地图信息量的增多而仅呈线性增长。

3)利用“第一次估计雅克比”(FEJ)算法，避免了因为引入地图信息而使系统的能观性遭到破坏的问题，从而使整个视觉惯性里程计系统保持一致性。

附图说明

图1是本发明算法的流程示意图；

图2是基于多段地图的观测示意图。

具体实施方式

本发明公开了一种基于多段独立地图序列的一致性高效视觉惯性里程计算法，图1是本发明算法的流程示意图；算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图匹配模块和基于多段地图信息的融合模块，局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；地图匹配用于检测当前时刻相机所观测到的场景与事先获取的多段独立地图序列之间的相似度，获得当前时刻图像特征与各个地图序列之间的特征匹配对；基于多段地图信息的融合模块用于接收局部里程计的输出量以及特征匹配对，通过融合来自于多段地图的特征来更新局部里程计的状态，同时估计局部里程计的参考系与各个独立地图序列的参考系之间的相对变换。

图2是基于多段地图的观测示意图。

1)局部里程计坐标系L，里程计所获得的机器人位姿都是表示在该系下的，最终里程计融合多段独立地图序列后得到的位姿也是在该坐标系下。

2)惯性测量单元(IMU)/机器人本体坐标系I，IMU提供的角速度和线速度都表示在该系下，下角标t表示t时刻。

3)与IMU固连的相机坐标系C。

4)多段独立的地图坐标系G，右上角标用于区分不同的地图序列。

5)在地图坐标系下的关键帧坐标系KF。

一、系统的状态向量

在介绍系统的各个模块之前，先介绍一下系统的状态向量，即系统需要实时维护估计的变量。系统的状态向量由两大部分组成。第一部分分别是机器人本体状态/>B个机器人历史位姿/>B代表一个正整数，表示有多个(B个) 机器人的历史位姿。以及局部里程计坐标系和不同段地图坐标系之间的相对位姿变换假设有N段不同的地图，第二部分/>存储着不同地图的关键帧信息：/>

上述的符号意义分别为：下标_t代表任意时刻t，上标T代表矩阵的转置，是机器人本体坐标系I与局部里程计坐标系L之间的旋转姿态，表示为单位四元数q，其对应的旋转矩阵为/>该旋转矩阵将在L下的三维向量旋转到I下，/>代表I在L下的位置，/>代表机器人在L下的线速度，/>和/>分别代表惯性测量单元(IMU)的角速度偏差和线加速度偏差，表示在第1段地图中的第j个关键帧的位姿(旋转，位置)，同理，/>表示在第N段地图中的第k个关键帧的位姿。

二、局部里程计

图1是本发明算法的流程示意图，t时刻局部里程计的输入为IMU获得的机器人本体的角速度和线加速度/>以及相机获取的图像。

1)状态传播

t时刻的IMU的数据用于状态传播(预测)，用过IMU的运动学模型，可以得到系统在t+1时刻的状态预测：

其中，下标_t+1|t表示通过IMU数据得到的t+1时刻的预测的状态，即状态传播模块的输出的状态变量。f代表IMU的运动学方程。是IMU 的线加速度和角速度的观测噪声。该噪声被构建为零均值的高斯白噪声。通过上式，可以获得对于x_t+1|t时刻的误差状态协方差。即状态传播模块输出的协方差：

其中P_t|t和Q_t|t分别表示t时刻的状态协方差与噪声协方差。Φ_t+1|t和 G_t+1|t分别表示(1)式关于系统状态x_t和噪声的雅克比矩阵。

2)观测融合状态更新

相机的图像用于提供视觉观测，从而进行状态更新。通过特征跟踪模块，可以追踪输入的图像流的特征点，进而在当前图像中得到一系列跟踪到的特征点{f},以及观测到这些特征点的一系列克隆的历史位姿{T_c}。对于每一个被观测到的特征f_i，有如下的观测方程：

其中为f_i在图像中的二维像素坐标，f_i为f_i的三维坐标，/>为图像的观测噪声，h为观测方程，该方程将f_i通过位姿/>投影到图像C_j上。

通过对(3)式在当前估计状态进行一阶泰勒展开，可以得到：

其中代表估计的值，/>代表真值与估计值之间的误差。/>和/>分别为(3)式关于估计的状态和观测的特征的雅克比矩阵。通过计算/>的左零空间N，可以将其消去，得到最终的状态更新方程：

通过式(5)进行EKF状态更新，输出更新后的状态变量和协方差。

三、地图匹配

图1是本发明算法的流程示意图，要利用多段地图序列辅助VIO定位，需要获得当前相机与各个地图序列间的匹配关系，即地图匹配模块。事先构建的地图会包含特征描述子，如BRIEF，ORB，SIFT等。通过这些特征描述子，可以获得与当前相机匹配到的多段地图的特征点(特征匹配对)以及相应的多段地图的关键帧，这些匹配信息会用于后续基于多段地图信息的融合模块中。

四、基于多段地图信息的融合

通过三，可以获取当前图像与多段地图中的关键帧的特征匹配对，如图二中的C_t的白点与¹KF₁，²KF₂的黑点。如果当前图像与地图序列中的关键帧有特征匹配对，就将地图关键帧的位姿加入到状态向量中。

1)局部坐标系与多段地图参考系间相对位姿变换的初始化：如果当前图像与第i个地图序列中的关键帧有特征匹配对，且局部坐标系L与第i个地图序列的参考系Gⁱ之间的相对位姿还没有初始化，那么将通过PnP算法获得地图关键帧与当前图像之间的相对位姿，进而求得地图坐标系与局部坐标系之间的相对位姿/>然后将这个相对位姿/>加入到状态中。

2)这部分为本发明的主要创新部分，由三部分组成，核心问题是在考虑地图不确定度的同时，维护系统的正确能观性，进而使多段独立的地图序列高效、一致地融入VIO系统中。

a)基于多段独立地图序列的观测方程

图2是基于多段地图的观测示意图，假设来自第k个地图序列的第i 个特征为它能被第k个地图序列中的第j个关键帧^kKF_j观测到，并且能被第s(s≠k)个地图序列中的第l个关键帧观测到。举个例子，当 k＝1,i＝1,j＝1,s＝2,l＝2，那么图2就描述了这个情况。有如下三个观测方程：

其中¹z_F,²z_F,³z_F分别表示在当前帧C_t，第k个地图序列中的第j 个关键帧，以及第s个地图序列中的第l个关键帧上的二维像素观测。¹h，²h，³h分别为对应的投影方程。¹n_F，²n_F，³n_F分别为对应的观测噪声。

将(6)-(8)式在估计值处进行一阶泰勒展开，可以得到如下的观测方程：

其中分别为观测方程关于x_A,x_N,/>在其估计值处的雅克比矩阵。

由于进将地图中的关键帧位姿加入到系统的状态向量中，而不将地图中的特征点位置加入到状态向量中，所以需要隐式地将地图点的不确性度考虑进去，从而保证系统的一致性。具体做法为:将(9)-(11) 式通过矩阵按列堆叠操作后合并成一个等式，按照与(5)式类似的操作，通过计算的左零空间来消去地图特征点/>对应的误差项。最终得到如下的紧凑观测方程表达式：

b)Schmidt状态更新

为了在融合多段地图信息的同时，考虑到地图信息的不确定性，并且使系统的计算量保持在较低的水平，该发明利用Schmidt更新算法来更新状态。具体而言，针对(12)式，按照如下步骤进行Schmidt状态更新：

其中，R_t为观测噪声的协方差。P_t|t-1是状态传播后的状态协方差，根据x_A和x_N，可以将其按块划分成：

为最终更新的机器人状态。/>为所有地图关键帧部分的状态，该部分保持不变。

c)能观性保持

由于将多段独立的地图序列信息融合到了VIO系统中。所以整个系统的能观性不同于传统的VIO系统的能观性。通过分析该系统的能观性，该发明提出了保持系统正确能观性，进而保证系统的一致性的算法“第一次估计雅克比”(FEJ)。具体而言：

·在状态传播计算雅克比矩阵Φ_t+1|t时，利用估计值来计算，而不是用/>来计算。

·在计算基于多段地图序列的观测方程的雅克比时，对于任意时刻t，当计算雅克比涉及到局部参考系L和第i个地图的参考系之间的相对位姿变换时，始终利用/>初始化时候的值来计算。

为了验证该发明的有效性，将算法(记为C-MIEB-VIO)与如下方法进行了对比：

a)纯里程计Open-VINS[1]，用于验证通过正确地融合多段独立地图序列，可以提高里程计的定位精度。

b)没有正确维护系统能观的算法，即没有FEJ能观性约束，记为 MIEB-VIO。用于说明正确维护系统能观性的必要性。

c)没有考虑多段地图序列的不确定性，即没有用Schmidt状态更新，记为P-MIEB-VIO。用于说明考虑地图序列不确定性的必要性。

实验结果在室内无人机数据集EuRoC[2](MH01-MH05，V101-V103， V201-V203)上以及室外自动驾驶数据集Kaist[3](Urban38)上进行。实验结果如下表所示：

通过上表可以看出，提出的基于多段独立地图序列的一致性高效视觉惯性里程计(C-MIEB-VIO)的定位误差最小，通过正确地融合多段独立地图序列，使纯里程计(Open-VINS)的定位误差明显降低。此外，可以看出，当不能正确地维护系统的能观性(MIEB-VIO),或者不考虑地地图信息的不确定性(P-MIEB-VIO)，其表现远不如提出的算法。这验证了该发明的有效性。

显然，本发明不限于以上实施例，还可以有许多变形，本领域的普通技术人员能从本发明公开的内容直接导出或联想到的所有变形，均应认为是本发明的保护范围。

[1]P.Geneva,K.Eckenhoff,W.Lee,Y.Yang and G.Huang,“OpenVINS:A ResearchPlatform forVisual-Inertial Estimation,”2020IEEE International Conference onRobotics and Automation(ICRA),Paris,France,2020,pp.4666-4672,doi:10.1109/ICRA40945.2020.9

[2]B.Siciliano et al.,”EuRoC-The Challenge Initiative for EuropeanRobotics,”ISR/Robotik 2014；41st International Symposium on Robotics,Munich,Germany,2014, pp.1-7

[3]J.Jeong,Y.Cho,Y.S.Shin,et al.”Complex urban dataset with multi-level sensors from highly diverse urban environments”,The InternationalJournal of Robotics Research,2019,38(6):02783649198。

Claims

1.一种基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的算法通过以下系统来实现，所述的系统包括三个模块：局部里程计模块，地图匹配模块和基于多段地图信息的融合模块，所述的局部里程计模块用于接收相机和IMU的数据，通过对相机提取的特征进行跟踪，以及用IMU数据辅助状态传播，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图匹配模块用于检测当前时刻相机所观测到的场景与事先获取的多段独立地图序列之间的相似度，获得当前时刻图像特征与各个地图序列之间的特征匹配对；所述的基于多段地图信息的融合模块用于接收局部里程计的输出量以及特征匹配对，通过融合来自于多段地图的特征来更新局部里程计的状态，同时估计局部里程计的参考系与各个独立地图序列的参考系之间的相对变换。

2.根据权利要求1所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的观测融合状态更新模块。

3.根据权利要求2所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的IMU用于为系统提供实时的旋转角速度和线加速度；所述的状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给观测融合状态更新模块，所述的特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给观测融合状态更新模块；所述的观测融合状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用扩展卡尔曼滤波(Extended Kalman Filter，EKF)来更新状态变量与协方差，整个过程与多状态约束卡尔曼滤波(MSCKF)一致。

4.根据权利要求3所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的状态变量及其对应的协方差需要包含以下变量：

2)IMU传感器的关于角速度与线加速度的偏差(bias)；

局部里程计参考系L和多段地图序列的参考系Gⁱ之间的位姿变换其中N代表地图序列的个数，t为当前时刻的时间戳，上标^T代表矩阵的转置，/>为多段地图序列的参考系Gⁱ与局部里程计参考系L之间的旋转姿态，为局部里程计参考系L在多段地图序列的参考系Gⁱ之下的位置；记为局部里程计参考系和第i段地图序列的参考系之间的位姿变换，所述的变量需要通过在基于多段地图信息的融合模块中的初始化局部与地图间的相对位姿/>模块加入到状态变量中；

4)地图序列中关键帧的位姿，所述的变量通过基于多段地图信息的融合模块中的将地图关键帧加入状态向量模块加入到状态变量中。

5.根据权利要求1或2或3或4所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的地图匹配模块用于通过当前相机与各个地图序列的信息，获得与当前相机匹配到的各个地图序列的特征点以及能观测到这些特征点的地图关键帧，即特征匹配对与多段地图序列的关键帧，所述的特征匹配对与多段地图序列的关键帧信号传递至基于多段地图信息的融合模块。

6.根据权利要求5所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的基于多段地图信息的融合模块包括以下模块：

初始化局部与各个地图序列参考系之间的相对位姿模块，将地图关键帧加入状态向量模块和利用多段地图信息进行状态更新模块；所述的初始化局部与各个地图序列参考系之间的相对位姿/>模块用于计算局部参考系和第i段地图序列参考系之间的相对位姿并将其加入到系统的状态向量中，获得增广的系统状态变量和协方差；将地图关键帧加入状态向量模块用于将地图关键帧的位姿和协方差加入系统的状态变量和系统的协方差中，从而天然地考虑了地图序列的不确定度，进而保证了系统的一致性，通过该模块我们获得了增广的状态变量和协方差；所述的利用多段地图信息进行状态更新模块在满足1)考虑地图不确定度，2)维护系统的正确能观性，3)系统的所需的运算量低的同时，将多段独立的地图序列高效、一致地融入局部里程计系统中，从而在提高局部里程计的定位精度的同时，还获取了各个独立地图之间的相对位置关系。

7.根据权利要求6所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的初始化局部与地图参考系之间的相对位姿模块，通过接收到的与第i段地图序列匹配的特征对和地图关键帧，利用PnP算法求解地图关键帧和相机当前帧之间的相对位姿，并利用地图关键帧位姿以及当前时刻系统的位姿解算出完成局部参考系和第i段地图序列参考系之间的相对位姿的初始化；所述的将地图关键帧加入状态向量模块具体为：当第i段地图序列的地图帧与当前帧有特征匹配对的时候，且/>已经初始化，直接将地图帧加到状态向量模块中，当第i段地图序列的地图帧与当前帧有特征匹配对的时候，且/>还没有初始化，则通过初始化局部与地图参考系之间的相对位姿模块完成的初始化，然后将用于初始化的地图关键帧位姿信号传递到地图关键帧加入状态向量模块。

8.根据权利要求6所述的基于多段独立地图序列的一致性高效视觉惯性里程计算法，其特征在于，所述的利用多段地图信息进行状态更新模块包括三个部分：

a)基于多段独立地图序列的观测方程；

b)基于Schmidt滤波的状态更新；

c)能观性保持；

所述的a)部分观测方程分为两种情况；1)匹配到的来自第i段地图序列的特征点，只在当前帧和第i段地图关键帧中观测到；2)匹配到的来自第i段地图序列的特征点在当前帧和第i段地图关键帧中观测到的同时，还在第j(j≠i)段地图序列中的关键帧中观测到，针对情况1)，将特征点重投影到第i段地图关键帧，并通过将特征点重投影到当前帧，针对情况2)，将特征点重投影到第i段地图关键帧；通过/>将特征点重投影到当前帧；通过和/>将特征点重投影到第j段地图的关键帧；

在计算重投影误差的时候，需要求解观测方程关于地图点的雅克比矩阵，通过计算该雅克比矩阵的左零空间，将该左零空间乘到观测方程上，从而消去地图点特征对应的误差项，并且隐式地考虑了地图特征点的不确定性；

所述的b)部分具体为：

其中，下标t表示当前时刻，下标t|t-1表示状态传播模块输出的值，为重投影误差，为观测方程关于系统状态向量的雅克比，分为两部分，

和/>分别是与/>和/>相关的雅可比矩阵，其中/>和/>表示需要实时更新的状态部分和地图关键帧部分，R_t为观测噪声/>的协方差，P_t|t-1是状态传播后的状态协方差，根据/>和/>将其按块划分成：

为最终更新的机器人的状态,/>为地图关键帧部分的状态，该部分保持不变，上标^表示估计值；

所述的c)部分能观性保持具体为：

为了保持正确的能观性，进而保证系统的一致性，提出了“第一次估计雅克比”算法(FEJ)用于基于多段地图信息的融合，所述的“第一次估计雅克比”算法(FEJ)具体为：

■在状态传播计算雅克比矩阵Φ_t+1|t时，利用估计值来计算；

■在计算基于多段地图序列的观测方程的雅克比时，对于任意时刻t，当计算雅克比涉及到局部参考系和第i个地图的参考系之间的相对位姿变换时，始终利用/>初始化时候的值来计算。