CN110160522A - 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，该方法主要包括了系统初始化、惯性传感单元的状态传递、图像特征点的跟踪、惯性传感单元误差项的构建、系统状态量的边缘化、视觉量测模型误差项的构建以及可观测性矩阵的约束等模块。本发明利用了平方根信息滤波方法将传统滤波法的高纬矩阵求逆步骤替换为计算速度更快的QR分解法，同时引入了线特征来增加估计轨迹的平滑性从而构建了全新的视觉惯导里程计框架。

Description

一种基于稀疏特征法的视觉惯导里程计的位姿估计方法

技术领域

本发明涉及四旋翼飞行器状态估计领域，尤其涉及一种基于稀疏特征法的视觉惯导里程计的位姿估计方法。

背景技术

四旋翼飞行器具有体积小、机动性好以及可垂直起降等优点，能轻易地在山区等复杂地形中垂直起飞执行任务，而且较高的机动性能使其快速规划出复杂的轨迹来躲避密集的避障物，完成固定翼飞行器无法完成的动作，所以在航拍、测绘、物流、安防以及农业等领域具有广泛的应用。一般来说，四旋翼飞行器的算法模块主要包括控制、规划、位姿估计和决策等等，其中准确的位姿信息是所有算法模块的基础。当下大部分四旋翼飞行器在多种环境下执行任务都需要飞手的参与，其中一个难点就在于无法依靠自身的传感器来快速准确地估计出当前时刻的位置和姿态信息。因此，位姿估计是当前飞行器研究领域的热点之一。

对于四旋翼飞行器来说，高机动性带来快速响应的同时也会使得系统变得不稳定，所以对算法的实时性要求很高。另外，四旋翼飞行器有限的承载能力往往使其无法携带较重的传感器以及大型高速计算设备。这导致了很多诸如激光雷达这样的高精度传感器无法使用，而且计算复杂度较高的算法也无法满足实时性的要求。当下，利用视觉里程计对于四旋翼飞行器的位姿估计来说是一种比较好的解决方案。但在实际运用中，视觉里程计无法避免视觉传感器带来的鲁棒性不强的缺点。因此，更合理的方式是融合其他传感器来弥补视觉传感器存在的缺陷从而提高系统的鲁棒性。众所周知，IMU是一种对短时间内的快速运动估计比较准确的传感器，它与视觉传感器在特性上存在非常好的互补，而且其运动积分信息还可以给单目视觉信息提供真实尺度。所以这种融合IMU信息的视觉里程计在机器人领域受到了极大的关注，称为视觉惯导里程计(Visual Inertial odometry,VIO)。

综上所述，四旋翼飞行器由于载重和续航能力有限，只能搭载诸如单目摄像头、超声波、IMU、GPS等小重量和低功耗的传感器，所以VIO非常适用于四旋翼飞行器的状态估计。然而，当下主流的VIO无法消除系统的累积误差，没有长时间运行的能力，对环境的变化忍耐力较差。因此，近年来VIO在理论和工程上取得了很大的进展，但仍会遇到在应用层面的瓶颈，例如如何平衡传感器数量与计算复杂度之间的矛盾，如何使VIO能够保持长时间高帧率稳定地运行。这些问题的探究和解决对VIO在四旋翼飞行器中的应用以及推广到其他轻量级的移动平台(手机设备等)具有现实的意义。

发明内容

本发明的目的在于针对四旋翼飞行器无法搭载大型计算设备的问题，提出了一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，该方法利用平方根信息滤波方法将传统滤波法的高纬矩阵求逆步骤替换为计算速度更快的QR分解法，同时引入了线特征来增加估计轨迹的平滑性从而构建了新的视觉惯导里程计框架。

本发明所提供的技术方案为：一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，

包括如下步骤：

(1)当第二帧图像到来时，对第二帧图像和第一帧图像之间所有惯性传感单元输出的加速度和角速度进行积分得到系统状态的初始值，并利用重力方向来定义初始坐标系；

(2)当新的图像到来时，计算当前时刻惯性传感单元的状态量，并根据状态量产生状态传递的误差项，将误差项和上一时刻系统的目标函数，从而得到当前时刻初始目标函数；同时，提取当前时刻图像中的视觉特征，然后将地图点重投影至当前图像实现特征点的匹配，并产生新的地图点；

(3)将步骤(2)计算的状态量加入到状态向量中，并将旧的状态量边缘化，产生新的状态向量；

(4)根据新的状态向量以及利用相机针孔模型表示步骤(2)中的视觉特征与地图点之间的映射关系，对针孔模型进行线性化构建量测方程，地图点雅可比矩阵加可观测性矩阵的约束；利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程；

(5)根据步骤(4)得到的映射量测方程，对视觉特征构建量测误差项，根据量测误差项更新步骤(2)获得的初始目标函数；

(6)对更新后的初始目标函数进行求解，利用求得的误差校正项来更新系统的状态向量。

进一步的，所述步骤(2)中初始目标函数的求解过程如下：

利用连续时间下的惯性传感单元的运动模型进行离散化，然后根据误差状态向量的定义对离散化运动方程的公式进行变换可得初始目标函数，利用QR分解将初始目标函数中的信息矩阵转换为上三角矩阵。

进一步的，所述步骤(2)中提取当前时刻图像中的视觉特征，然后将地图点重投影至当前图像实现特征点的匹配，并产生新的地图点，具体过程如下：

利用SSE加速的ORB特征检测器提取当前图像中ORB特征描述子，然后将地图点重投影至当前图像帧实现特征点的匹配，并利用两帧图像之间的匹配的特征点，得到新的地图点。

进一步的，所述步骤(2)中的视觉特征包括已生成的SLAM特征、新生成的SLAM特征和局部特征。

进一步的，所述步骤(3)中边缘化的旧状态量包括未被当前图像观测到SLAM特征、滑动窗口中存在的时间最长的相机位姿和上一时刻IMU的状态量。

进一步的，所述步骤(5)具体如下：

(5.1)已生成的SLAM特征对应的误差项：

利用当前帧的位姿和最小重投影误差来构造特征点对应的误差项，利用Plücker坐标系和正交表示来简化线特征的描述，利用线特征的最小重投影误差来构造线特征对应的误差项，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.2)新生成的SLAM特征对应的误差项：

利用观测过该特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项，利用来构造线特征对应的误差项，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.3)局部特征对应的误差项：

利用观测过局部特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项；利用构造的的误差项更新步骤(2)获得的初始目标函数，利用QR分解将误差项中的信息矩阵转换为上三角矩阵。

进一步的，所述步骤(2)中，根据状态量产生状态传递的误差项C_u为：

其中Φ表示状态转移矩阵，I表示单位矩阵，表示上一时刻的系统状态误差向量，表示当前时刻预测的系统状态误差向量，表示当前时刻估计的系统状态向量，f表示上一时刻估计的系统状态向量的结果。

进一步的，所述步骤(4)中，对针孔模型进行线性化构建量测方程C_z，具体如下：

其中H_x表示相机位姿相关的雅可比矩阵，表示相机位姿的误差，H_f表示地图点相关的雅可比矩阵，表示地图点位置的误差，表示视觉特征观测的误差；

所述步骤(4)中，地图点雅可比矩阵加可观测性矩阵M的约束，具体如下：

M＝[H₁…H₁Φ^n-1]^T (3)

其中H₁表示系统的雅可比矩阵，Φ表示系统的状态转移矩阵；

从可观测性矩阵的性质分析，视觉惯导里程计四个不可观测的方向正好被可观测性矩阵的零空间N₁张成，那么将雅可比矩阵H₁和状态转移矩阵Φ代入到式(3)中，然后求解可观测性矩阵M的零空间，如下：

其中R表示旋转矩阵，I表示单位矩阵，v表示速度，g表示重力向量，p₁表示相机在世界坐标系的位置，p₂表示地图点在世界坐标系的位置；

在线性化得到量测方程后，对应的雅可比矩阵应该要满足H₁N₁＝0，从而可以得到如下的关系式：

上式是一个Au＝0的形式，然后构造一个带约束的最小二乘最优问题，如下：

min||A^*-A||,st.Au＝0 (6)

其中A^*表示式(5)中雅可比矩阵的最优值，A表示当前雅可比矩阵的值，u表示可观测性矩阵的零空间N₁；

通过拉格朗日乘子法直接对式(6)进行求解，可得最优的雅可比矩阵如下：

A^*＝A-Au(u^Tu)^-1u^T (7)。

进一步的，所述步骤(4)中，利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程，具体如下：

对式(3)两边同乘以由H_f列空间和左零空间构成的正交矩阵Q＝[S U]，展开以后可以得到特征对应的量测误差项C_z，如下：

其中S表示列空间，U表示左零空间。

进一步的，所述步骤(6)中，对更新后的初始目标函数进行求解，具体如下：

对更新后的初始目标函数进行求解，

其中

为了求得的使得最小，令从而有

其中表示系统误差项的信息矩阵，r表示观测误差；的结构是三上角的形式；C_k-1表示上一时刻系统产生的误差项，表示已生成的SLAM特征更新时产生的误差项，表示新的SLAM特征产生的误差项，表示局部特征点产生的误差项；

采用回代方法迭代地求解系统的误差状态向量。

同现有技术相比，本发明的有益效果体现在：

(1)本发明利用平方根信息滤波方法将传统滤波法的高纬矩阵求逆步骤替换为计算速度更快的QR分解法，使得系统的计算速度更快；利用了信息矩阵的平方根来表示系统误差的协方差，降低了不同状态之间的条件数，提高了系统的稳定性。

(2)视觉前端采用处理器底层指令集对视觉特征的检测和匹配进行了加速，降低了特征处理的计算复杂度；采用了点、线特征来约束多个相机位姿，提高了估计轨迹的连续性。

(3)采用可观测性约束的方法，通过可观测矩阵的约束使部分状态量恢复不可观测，避免引入错误的信息使系统的估计精度下降。

附图说明

图1是本发明的总体框架示意图；

图2是本发明的slam特征约束相机位姿示意图；

图3本发明的局部特征约束相机位姿示意图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有的实施方式。相反，它们仅是与如所附中权利要求书中所详述的，本发明的一些方面相一致的装置的例子。本说明书的各个实施例均采用递进的方式描述。

如图1所示，本发明提供一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，包括如下步骤：

(1)当第二帧图像到来时，对第二帧图像和第一帧图像之间所有惯性传感单元输出的加速度和角速度进行积分得到系统状态的初始值，并利用重力方向来定义初始坐标系；

(2)当新的图像到来时，利用4阶龙格库塔积分方式来计算当前时刻惯性传感单元的状态量，但积分方式不限于此，并根据状态量来构建状态传递的误差项，将误差项和上一时刻系统的目标函数，从而得到当前时刻初始目标函数；同时，提取当前时刻图像中的视觉特征(包括已生成的SLAM特征、新生成的SLAM特征和局部特征)，然后将地图点重投影至当前图像实现特征点的匹配，利用三角化的方式将匹配的特征点对转换为新的地图点；

其中，根据状态量产生状态传递的误差项C_u为：

其中Φ表示状态转移矩阵，I表示单位矩阵，表示上一时刻的系统状态误差向量，表示当前时刻预测的系统状态误差向量，表示当前时刻估计的系统状态向量，f表示上一时刻估计的系统状态向量的结果。

其中，初始目标函数的求解过程如下：

利用微分方程的特性将连续时间下的惯性传感单元的运动模型进行离散化，然后根据误差状态向量的定义对离散化运动方程的公式进行变换可得初始目标函数，利用QR分解将初始目标函数中的信息矩阵转换为上三角矩阵。

其中，提取当前时刻图像中的视觉特征，然后将地图点重投影至当前图像实现特征点的匹配，利用三角化的方式将匹配的特征点对转换为新的地图点，具体如下：利用SSE加速的ORB特征检测器提取当前图像中ORB特征描述子，但不限于此，然后将地图点重投影至当前图像帧实现特征点的匹配，并利用两帧图像之间的匹配的特征点，得到新的地图点。

(3)将步骤(2)计算的状态量加入到状态向量中，同时为了保持状态向量的维度，将旧的状态量边缘化，产生新的状态向量；所述步骤(3)中边缘化的旧状态量包括未被当前图像观测到SLAM特征、滑动窗口中存在的时间最长的相机位姿和上一时刻IMU的状态量。

(4)根据新的状态向量以及利用相机针孔模型表示步骤(2)中的视觉特征与地图点之间的映射关系，采用一阶泰勒展开的方式对针孔模型进行线性化构建量测方程，然后对地图点雅可比矩阵施加可观测性矩阵的约束；利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程；

其中，对针孔模型进行线性化构建量测方程C_z，具体如下：

其中H_x表示相机位姿相关的雅可比矩阵，表示相机位姿的误差，H_f表示地图点相关的雅可比矩阵，表示地图点位置的误差，表示视觉特征观测的误差；

所述步骤(4)中，地图点雅可比矩阵加可观测性矩阵M的约束，具体如下：

M＝[H₁…H₁Φ^n-1]^T (3)

其中H₁表示系统的雅可比矩阵，Φ表示系统的状态转移矩阵；

从可观测性矩阵的性质分析，视觉惯导里程计四个不可观测的方向正好被可观测性矩阵的零空间N₁张成，那么将雅可比矩阵H₁和状态转移矩阵Φ代入到式(3)中，然后求解可观测性矩阵M的零空间，如下：

其中R表示旋转矩阵，I表示单位矩阵，v表示速度，g表示重力向量，p₁表示相机在世界坐标系的位置，p₂表示地图点在世界坐标系的位置。

在线性化得到量测方程后，对应的雅可比矩阵应该要满足H₁N₁＝0，从而可以得到如下的关系式：

上式是一个Au＝0的形式，然后构造一个带约束的最小二乘最优问题，如下：

min||A^*-A||,st.Au＝0 (6)

其中A^*表示式(5)中雅可比矩阵的最优值，A表示当前雅可比矩阵的值，u表示可观测性矩阵的零空间N₁。

通过拉格朗日乘子法直接对式(6)进行求解，可得最优的雅可比矩阵如下：

A^*＝A-Au(u^Tu)^-1u^T (7)

进一步，所述步骤(4)中，利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程，具体如下：

对式(3)两边同乘以由H_f列空间和左零空间构成的正交矩阵Q＝[S U]，展开以后可以得到特征对应的量测误差项C_z，如下：

其中S表示列空间，U表示左零空间。

(5)根据步骤(4)得到的映射量测方程，对视觉特征构建量测误差项，根据量测误差项更新步骤(2)获得的初始目标函数；具体过程如下：

(5.1)已生成的SLAM特征对应的误差项：

利用当前帧的位姿和最小重投影误差来构造特征点对应的误差项，利用Plücker坐标系和正交表示来简化线特征的描述，利用线特征的最小重投影误差来构造线特征对应的误差项，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.2)新生成的SLAM特征对应的误差项：

利用观测过该特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项，利用来构造线特征对应的误差项，其中线特征的描述采用Plücker坐标系和正交表示，但不限于此，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.3)局部特征对应的误差项：

利用观测过局部特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项；利用构造的的误差项更新步骤(2)获得的初始目标函数，利用QR分解将误差项中的信息矩阵转换为上三角矩阵。

(6)对更新后的初始目标函数进行求解，利用求得的误差校正项来更新系统的状态向量。

其中，对更新后的初始目标函数进行求解，具体如下：

对更新后的初始目标函数进行求解，

其中

为了求得的使得最小，令从而有

其中表示系统误差项的信息矩阵，r表示观测误差；的结构是三上角的形式；C_k-1表示上一时刻系统产生的误差项，表示已生成的SLAM特征更新时产生的误差项，表示新的SLAM特征产生的误差项，表示局部特征点产生的误差项；

采用回代方法迭代地求解系统的误差状态向量。

如图2所示为视觉惯导里程计的传感器模块，由微型电脑1、惯性传感器2和相机3组成。其中微型电脑1采用的是英特尔公司的NUC系列，但不限于此。惯性传感单元3为工业级ADIS系列的产品，但不限于此。相机4为全局曝光的bluefox2摄像头，但不限于此。

如图3所示为SLAM特征约束多个相机位姿的示意图。所述的SLAM特征将所有相机的位姿信息都包含在观测方程中，每个SLAM特征包含了所有相机位姿的信息。所述的SLAM特征有两种类型，分别为点特征和线特征。对于线特征来说，可采用LSD算法来提取，但不限于此。线特征的描述可以采用Plücker坐标系和正交表示，但不限于此。

上述具体实施方案用来解释说明本发明，仅为本发明的优选实施例而已，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改、等同替换、改进等，都落入本发明的保护范围。

Claims (10)

1.一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，包括如下步骤：

(1)当第二帧图像到来时，对第二帧图像和第一帧图像之间所有惯性传感单元输出的加速度和角速度进行积分得到系统状态的初始值，并利用重力方向来定义初始坐标系；

(2)当新的图像到来时，计算当前时刻惯性传感单元的状态量，并根据状态量产生状态传递的误差项，将误差项和上一时刻系统的目标函数，从而得到当前时刻初始目标函数；同时，提取当前时刻图像中的视觉特征，然后将地图点重投影至当前图像实现特征点的匹配，并产生新的地图点；

(3)将步骤(2)计算的状态量加入到状态向量中，并将旧的状态量边缘化，产生新的状态向量；

(4)根据新的状态向量以及利用相机针孔模型表示步骤(2)中的视觉特征与地图点之间的映射关系，对针孔模型进行线性化构建量测方程，地图点雅可比矩阵加可观测性矩阵的约束；利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程；

(5)根据步骤(4)得到的映射量测方程，对视觉特征构建量测误差项，根据量测误差项更新步骤(2)获得的初始目标函数；

(6)对更新后的初始目标函数进行求解，利用求得的误差校正项来更新系统的状态向量。

2.根据权利要求1所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(2)中初始目标函数的求解过程如下：

利用连续时间下的惯性传感单元的运动模型进行离散化，然后根据误差状态向量的定义对离散化运动方程的公式进行变换可得初始目标函数，利用QR分解将初始目标函数中的信息矩阵转换为上三角矩阵。

3.根据权利要求2所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(2)中提取当前时刻图像中的视觉特征，然后将地图点重投影至当前图像实现特征点的匹配，并产生新的地图点，具体过程如下：

利用SSE加速的ORB特征检测器提取当前图像中ORB特征描述子，然后将地图点重投影至当前图像帧实现特征点的匹配，并利用两帧图像之间的匹配的特征点，得到新的地图点。

4.根据权利要求3所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(2)中的视觉特征包括已生成的SLAM特征、新生成的SLAM特征和局部特征。

5.根据权利要求4所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(3)中边缘化的旧状态量包括未被当前图像观测到SLAM特征、滑动窗口中存在的时间最长的相机位姿和上一时刻IMU的状态量。

6.根据权利要求5所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(5)具体如下：

(5.1)已生成的SLAM特征对应的误差项：

利用当前帧的位姿和最小重投影误差来构造特征点对应的误差项，利用Plücker坐标系和正交表示来简化线特征的描述，利用线特征的最小重投影误差来构造线特征对应的误差项，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.2)新生成的SLAM特征对应的误差项：

利用观测过该特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项，利用来构造线特征对应的误差项，利用点和线的误差项更新步骤(2)获得的初始目标函数，将更新后的初始目标函数中的信息矩阵转换为上三角矩阵；

(5.3)局部特征对应的误差项：

利用观测过局部特征的多个相机对应的位姿和最小重投影误差来构造特征点对应的误差项；利用构造的的误差项更新步骤(2)获得的初始目标函数，利用QR分解将误差项中的信息矩阵转换为上三角矩阵。

7.根据权利要求6所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(2)中，根据状态量产生状态传递的误差项C_u为：

其中Φ表示状态转移矩阵，I表示单位矩阵，表示上一时刻的系统状态误差向量，表示当前时刻预测的系统状态误差向量，表示当前时刻估计的系统状态向量，f表示上一时刻估计的系统状态向量的结果。

8.根据权利要求7所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(4)中，对针孔模型进行线性化构建量测方程C_z，具体如下：

其中H_x表示相机位姿相关的雅可比矩阵，表示相机位姿的误差，H_f表示地图点相关的雅可比矩阵，表示地图点位置的误差，表示视觉特征观测的误差；

所述步骤(4)中，地图点雅可比矩阵加可观测性矩阵M的约束，具体如下：

M＝[H₁…H₁Φ^n-1]^T (3)

其中H₁表示系统的雅可比矩阵，Φ表示系统的状态转移矩阵；

从可观测性矩阵的性质分析，视觉惯导里程计四个不可观测的方向正好被可观测性矩阵的零空间N₁张成，那么将雅可比矩阵H₁和状态转移矩阵Φ代入到式(3)中，然后求解可观测性矩阵M的零空间，如下：

其中R表示旋转矩阵，I表示单位矩阵，v表示速度，g表示重力向量，p₁表示相机在世界坐标系的位置，p₂表示地图点在世界坐标系的位置；

在线性化得到量测方程后，对应的雅可比矩阵应该要满足H₁N₁＝0，从而可以得到如下的关系式：

上式是一个Au＝0的形式，然后构造一个带约束的最小二乘最优问题，如下：

min||A^*-A||,st.Au＝0 (6)

其中A^*表示式(5)中雅可比矩阵的最优值，A表示当前雅可比矩阵的值，u表示可观测性矩阵的零空间N₁；

通过拉格朗日乘子法直接对式(6)进行求解，可得最优的雅可比矩阵如下：

A^*＝A-Au(u^Tu)^-1u^T (7)。

9.根据权利要求8所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(4)中，利用正交矩阵把量测方程映射到地图点雅可比矩阵的列空间和左零空间，得到映射量测方程，具体如下：

对式(3)两边同乘以由H_f列空间和左零空间构成的正交矩阵Q＝[S U]，展开以后可以得到特征对应的量测误差项C_z，如下：

其中S表示列空间，U表示左零空间。

10.根据权利要求9所述的一种基于稀疏特征法的视觉惯导里程计的位姿估计方法，其特征在于，所述步骤(6)中，对更新后的初始目标函数进行求解，具体如下：

对更新后的初始目标函数进行求解，

其中

为了求得的使得最小，令从而有

其中表示系统误差项的信息矩阵，r表示观测误差；的结构是三上角的形式；C_k-1表示上一时刻系统产生的误差项，表示已生成的SLAM特征更新时产生的误差项，表示新的SLAM特征产生的误差项，表示局部特征点产生的误差项；

采用回代方法迭代地求解系统的误差状态向量。