CN103424114A - 一种视觉导航/惯性导航的全组合方法 - Google Patents

Abstract

本发明涉及一种视觉导航/惯性导航的全组合方法，该方法包括以下步骤：1)视觉导航解算：以共线方程为基础列立观测方程，采用最小二乘原理平差求取载***置和姿态参数，并计算参数之间的方差-协方差阵；2)惯性导航计算：在当地水平坐标系中做导航计算，得到每个时刻载体的位置、速度和姿态参数，同时计算参数之间的方差-协方差阵；3)视觉***修正惯导***：以卡尔曼滤波为手段，估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正，从而得到惯导***所有参数的最优估值；4)惯导***修正视觉***：采用序贯平差处理，修正视觉***所有参数。与现有技术相比，本发明具有理论严密、性能稳定、高效等优点。

Description

一种视觉导航/惯性导航的全组合方法

技术领域

本发明涉及一种视觉导航***和惯导***的组合方式，尤其是涉及一种视觉导航/惯性导航的全组合方法。

背景技术

视觉导航，又称为视觉测程法(Visual Odometry，VO)，基本原理是：载体前进过程中，利用立体相机获取周围场景的影像，依靠特征点在左右影像间的匹配，以及前后帧影像间的追踪，以共线方程为基础列立观测方程，根据最小二乘原理平差求取相机的空间位置和姿态。视觉导航是一种高精度的自主导航方式，但是作为一种航迹推算方式，其导航误差随时间积累，而且算法的稳定性一直是个难题，有时候影像视场中的背景单一或过于相似，或者载体车的动态性过大，例如转弯角度过大，使得相邻影像之间的重叠度太小，这些都会导致视觉导航解算失败，因此要借助其它手段来增强算法的稳定性。

惯导***(Inertial Navigation System，INS)以牛顿力学定律为工作原理，利用安装在载体上的惯性测量元件(Inertial Measurement Unit，IMU)来敏感载体的角速度和加速度信息，通过积分运算得到载体的位置、速度和姿态等导航参数。惯性导航具有完全的自主性和抗干扰性，短时间内具有高精度，但是由于积分原理，其误差随时间累积，因此长时间工作的精度较差。为了减少误差累积，要引入外部观测值来对惯导***进行修正。

因此，视觉导航与惯性导航作为两种自主导航方式，具有很好的互补性。两者的组合可以解决无GPS环境下的导航问题，对于室内导航、机器人路径规划、无人机自主着陆，自动行驶车的导航、地下交通导航定位、矿井作业安全等应用需求具有重要意义。

目前，视觉导航与惯导***的组合方式一般有三类：(1)结合型，一般依靠视觉***提供载体的位置信息，惯导***提供载体的姿态信息；(2)修正型，这种组合方式是以一种导航***去修正另一种导航***；(3)融合型，一般采用卡尔曼滤波来实现两者的数据融合。以上三类组合方式中，结合型属于低层次的组合，常用于精度要求不高的场合，而在高精度的场合中通常采用修正型或融合型。

在修正型组合方式中，一般是利用惯导***来修正视觉***，实现过程一般是这样的：(1)相机获取当前时刻的影像，(2)对当前时刻的左右影像进行特征点提取和匹配，并与前一时刻的影像进行匹配，然后通过后方交会计算相机的外方位元素(位置和姿态)，(3)利用IMU的输出数据计算载体的俯仰角和翻滚角，以及航向角增量，(4)利用载体的俯仰角和翻滚角以及航向角增量，通过卡尔曼滤波对相机位置和姿态进行修正，(5)利用修正后的相机位置和姿态，通过前方交会求特征点的物方坐标，(6)重复以上步骤。

融合型组合方式的实现过程一般是这样的：(1)相机获取当前时刻的影像，(2)对当前时刻的左右影像进行特征点提取和匹配，并与前一时刻的影像进行匹配，然后通过后方交会计算相机的外方位元素(位置和姿态)，(3)利用相机的位置和姿态作为量测值，采用卡尔曼滤波估计IMU的位置和姿态，(4)将滤波后得到的IMU位置和姿态传递给相机，然后通过前方交会求特征点的物方坐标，(5)重复以上步骤。

从以上可以看出，修正型的组合方式中，仅仅利用IMU的输出数据提供姿态，而没提供位置信息，因此这种方式没能最大程度上的利用惯导***所提供的导航信息。融合型的组合方式能较好的利用视觉***和惯导***的信息，使得两者都可以得到修正，但是这种方式没有考虑到影像特征点物方坐标的先验信息，组合效果不能达到最佳。实际上在视觉导航中，特征点在不同影像之间的传递是关键，当前影像中提取的特征点，大部分在上一帧影像中已经通过最小二乘平差获得了其物方坐标，且坐标精度也已知，因此必须对精度信息加以考虑。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种理论严密、稳定、高效的视觉导航/惯性导航的全组合方法。

本发明的目的可以通过以下技术方案来实现：

一种视觉导航/惯性导航的全组合方法，该方法包括以下步骤：

1)视觉导航解算：相机在载体运动过程中获取影像，以共线方程为基础列立观测方程，采用最小二乘原理平差求取载***置和姿态参数X_vo，并计算参数之间的方差-协方差阵P(X_vo)；

2)惯性导航计算：在当地水平坐标系中做导航计算，得到每个时刻载体的位置、速度和姿态参数X_ins，同时根据惯导***的误差方程，按协方差传播律，计算参数之间的方差-协方差阵P(X_ins)；

3)视觉***修正惯导***：以卡尔曼滤波为手段，取惯导***的误差方程作为状态方程，利用视觉导航解算得到的载***置、姿态，与惯导***计算得到的载***置、姿态分别求差，作为卡尔曼滤波的量测，估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正，从而得到惯导***所有参数的最优估值，记为

4)惯导***修正视觉***：从惯导***的参数中分离出IMU的位置和姿态，考虑相机与IMU之间的几何关系，将IMU的位置和姿态转化成相机的位置和姿态信息，建立观测方程，采用序贯平差处理，得到视觉***所有参数的最优估值，记为

所述的视觉导航解算具体步骤包括：

11)相机外方位元素的初始化：在视觉导航起始位置的周围布设控制点，通过后方交会求取相机的外方位元素；

12)载体不停的运动，利用相机获取当前时刻的影像，进行特征点提取和匹配；

13)以共线方程为基础，对左右影像匹配成功的所有特征点都建立观测方程：

$\left\{\begin{array}{c}v=B_1{\hat{x}}_1+B_2{\hat{x}}_2+A_1{\hat{y}}_1+A_2{\hat{y}}_2-l\\ v\left(y_1\right)={\hat{y}}_1-l\left(y_1\right)\\ C({\hat{x}}_1,{\hat{x}}_2)-W_x=0\end{array}\right.$

上式中，指左相机的外方位元素改正数，指右相机的外方位元素改正数，

指已知点的物方坐标改正数，

指待定点的物方坐标改正数，y₁指已知点的物方坐标，v指观测值残差，l为常量，

为左右相机之间的约束关系，W_x为约束方程中的常量；

14)根据最小二乘原理求解上式，求得左右相机的外方位元素，以及所有特征点的物方坐标。

所述的特征点匹配包括两个方面：一是将当前时刻的左右影像进行特征点匹配，二是将当前时刻的左影像与上一时刻的左影像进行特征点匹配；所述的特征点包括已知点和待定点：已知点是指在上一帧影像中已经出现过的特征点，待定点则是指当前影像中新增加的特征点。

所述的惯性导航计算步骤具体包括：

(1)姿态更新：利用陀螺输出的角速度观测值，采用四元数法作姿态更新，然后将更新后的四元数转化成姿态矩阵，记为

k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

(2)姿态提取：根据姿态矩阵

中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

(3)比力分解：利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，如下所示

$f^L=R_b^L(k+1)\·f^b$

(4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度a，如下所示

$a=f^L-({2\mathrm{\Ω}}_{\mathrm{ie}}+{\mathrm{\Ω}}_{\mathrm{eL}}^L)v^L+g^L$

其中，

为地球自转角速度三维分量构成的反对称阵，

为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

(5)积分求取速度：利用加速度进行积分得到速度增量，进而得到当前时刻的速度

如下所示

$v_{k+1}^L=v_l^L+a\·\mathrm{\Δt}$

(6)积分求取位置：利用速度进行积分得到位置增量，进而得到当前时刻的位置

如下所示

$r_{k+1}^L=r_k^L+\frac{1}{2}(v_k^L+v_{k+1}^L)\·\mathrm{\Δt}$

所述的视觉***修正惯导***步骤具体包括：

31)取惯导***的误差方程作为卡尔曼滤波器的状态方程，形式如下

$\stackrel{\·}{X}=F\·X+G\·\mathrm{\ω}$

其中，X为误差状态向量，包括位置误差δr^L、速度误差δv^L、平台失准角δε^L、陀螺漂移d和加速度计零偏b，

X＝(δr^L，δv^L，δε^L，d，b)

32)将惯性导航计算得到的位置与视觉导航解算的位置之差，以及惯性导航计算得到的姿态与视觉导航解算的姿态之差作为量测值，量测方程如下

$\left(\begin{array}{c}{Z}_r\\ Z_a\end{array}\right)=\left(\begin{array}{c}{H}_r\\ H_a\end{array}\right)\·X+\left(\begin{array}{c}{V}_r\\ V_a\end{array}\right)$

Z_r，Z_a分别表示位置误差量测和姿态误差量测，H_r，H_a分别表示位置误差量测矩阵和姿态误差量测矩阵，V_r，V_a分别表示位置误差量测和姿态误差量测的残差；

位置误差Z_r按如下方式计算

Z_r＝r_ins-r_vo

上式中，r_ins指惯性导航计算得到的坐标，r_vo表示视觉导航解算得到的坐标；位置误差量测矩阵H_r的形式如下

$H_r=\left(\begin{array}{cc}\underset{3\×3}{I}& \underset{12\×3}{0}\end{array}\right)$

式中，I为单位阵，0为零矩阵，姿态误差Z_a的计算方式如下

Z_a＝a_ins-a_vo

其中a_ins表示惯性导航计算得到的姿态，a_vo表示视觉导航解算得到的姿态；考虑相机与IMU之间的旋转矩阵，将视觉导航解算得到的相机姿态归算到载体的姿态：

a_vo＝(p′，r′，y′)^T

式中p′，r′，y′表示由相机姿态归算得到的载体俯仰角、翻滚角和航向角；姿态误差量测矩阵H_a的形式如下

$H_a=\left(\begin{array}{ccc}\underset{6\×3}{0}& \underset{3\×3}{A}& \underset{6\×3}{0}\end{array}\right)$

A阵的形式如下

$A=\left(\begin{array}{ccc}& & \\ \cos y& \sin y& 0\\ -\frac{\sin y}{\cos p}& \frac{\cos y}{\cos p}& 0\\ \frac{\sin y\sin p}{\cos p}& -\frac{\cos y\sin p}{\cos p}& 1\end{array}\right)$

式中，p，r，y分别表示由惯性导航计算得到的载体俯仰角、翻滚角和航向角；

33)通过滤波估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正。

所述的惯导***修正视觉***步骤具体包括：

41)从导航参数中分离出IMU的位置和姿态，根据相机与IMU之间的几何关系，将IMU的位置和姿态转化成相机位置和姿态的观测值；

42)采用序贯平差处理，完成惯导***对视觉***的修正。

所述的步骤42)具体包括：

惯导***修正视觉***的函数模型写成以下形式：

$\left\{\begin{array}{c}{X}_{\mathrm{vo}}=\left(\begin{array}{cccc}I& I& I& I\end{array}\right)\left(\begin{array}{c}{\hat{x}}_1\\ {\hat{x}}_2\\ {\hat{y}}_1\\ {\hat{y}}_2\end{array}\right)-l_{X_{\mathrm{vo}}}\\ \left(\begin{array}{c}{r}_{\mathrm{ins}}\\ a_{\mathrm{ins}}\end{array}\right)=\left(\begin{array}{cccc}I& 0& 0& 0\end{array}\right)\left(\begin{array}{c}{\hat{x}}_1\\ {\hat{x}}_2\\ {\hat{y}}_1\\ {\hat{y}}_2\end{array}\right)-l_{X_{\mathrm{ins}}}\end{array}\right.$

其中，分别为两个方程中对应的常量。

将上式整理成观测方程的形式，如下所示：

$\left\{\begin{array}{c}v\left(x_{\mathrm{vo}}\right)={\hat{x}}_{\mathrm{vo}}-l\left(x_{\mathrm{vo}}\right)\\ v(r_{\mathrm{ins}},a_{\mathrm{ins}})=B{\hat{x}}_{\mathrm{vo}}-l(r_{\mathrm{ins}},a_{\mathrm{ins}})\end{array}\right.$

上式中，矩阵B的形式如下

B＝(I 0 0 0)

观测值的随机模型，即方差-协方差阵如下：

$\left\{\begin{array}{c}D\left(X_{\mathrm{vo}}\right)=P\left(V_{\mathrm{vo}}\right)\\ D(r_{\mathrm{ins}},a_{\mathrm{ins}})=P(r_{\mathrm{ins}},a_{\mathrm{ins}})\end{array}\right.$

上式中，P(r_ins，a_ins)为由P(X_ins)中分离出的与位置、姿态相关的子阵，根据上述的函数模型和随机模型，可以导出视觉***的修正公式，如下所示

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{vo}}=x_{\mathrm{vo}}+D\left(X_{\mathrm{vo}}\right)B^T{(D(r_{\mathrm{ins}},a_{\mathrm{ins}})+\mathrm{BD}\left(X_{\mathrm{vo}}\right)B^T)}^{-1}(l(r_{\mathrm{ins}},a_{\mathrm{ins}})-{\mathrm{Bx}}_{\mathrm{vo}})\\ D\left({\hat{x}}_{\mathrm{vo}}\right)=D\left(X_{\mathrm{vo}}\right)-D\left(X_{\mathrm{vo}}\right)B^T{(D(r_{\mathrm{ins}},a_{\mathrm{ins}})+\mathrm{BD}\left(X_{\mathrm{vo}}\right)B^T)}^{-1}\mathrm{BD}\left(X_{\mathrm{vo}}\right)\end{array}\right.$

上式中，x_vo是指由视觉导航解算得到的视觉参数，

经过修正后的视觉参数，

是经修正后的方差-协方差阵。

与现有技术相比，本发明具有以下几个优点：

(1)视觉***修正惯导***。通过卡尔曼滤波不仅可以估计出惯导***的导航参数误差(位置误差、速度误差、姿态误差)，还可估计出器件误差(陀螺漂移、加速度计零偏)，并对导航参数误差进行补偿，将器件误差反馈到下一个周期的导航计算中，这样可以使惯导***保持长时间的高精度。在视觉导航解算失败的情况下，组合***则完全依靠惯性导航来提供载体的导航参数，由于之前对各种误差进行了估计和校正，因此在短时间内，不会因为视觉导航解算的失败而导致整个***的精度明显下降。

(2)惯导***修正视觉***。利用惯性导航计算得到的位置和姿态，通过序贯平差处理，不仅可以改善相机的外方位元素，影像特征点的物方坐标也得到修正。经过序贯平差修正后的影像特征点的物方坐标以及精度信息(物方坐标之间的方差-协方差阵)将传递到下一帧影像的导航解算中，采用最小二乘配置进行处理，这种方法顾及了影像特征点的物方坐标先验信息，可以使视觉***保持长时间的高精度。

附图说明

图1为本发明的步骤流程示意图；

图2为视觉***修正惯导***的原理图；

图3为惯导***修正视觉***原理图；

图4为不同导航方法计算的载体运行轨迹；

图5为卡尔曼滤波过程中的位置新息；

图6为卡尔曼滤波过程中的姿态新息。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种视觉导航/惯性导航的全组合方法，该方法包括以下步骤：

1)视觉导航解算

步骤如下：

(1)相机外方位元素的初始化，一般在视觉导航起始位置的周围布设一些控制点，通过后方交会求取相机的外方位元素。

(2)载体不停的运动，利用立体相机获取当前时刻的影像，进行特征点提取和匹配，特征点匹配包括两个方面：一是将当前时刻的左右影像进行特征点匹配，二是将当前时刻的左影像与上一时刻的左影像进行特征点匹配。

(3)以共线方程为基础，对左右影像匹配成功的所有特征点都建立观测方程。这里的特征点分为两类：已知点和待定点。已知点是指在上一帧影像中已经出现过的特征点，因此其物方坐标已知，且相应的精度也已知，待定点则是指当前影像中新增加的特征点，其物方坐标没有任何先验信息。本发明中将已知点的先验信息用最小二乘配置原理来对待，因此观测方程可以写成以下形式：

$\left\{\begin{array}{c}v=B_1{\hat{x}}_1+B_2{\hat{x}}_2+A_1{\hat{y}}_1+A_2{\hat{y}}_2-l\\ v\left(y_1\right)={\hat{y}}_1-l\left(y_1\right)\\ C({\hat{x}}_1,{\hat{x}}_2)-W_x=0\end{array}\right.---\left(1\right)$

上式中共包含三个方程。第一个方程中的

指左相机的外方位元素改正数，

指右相机的外方位元素改正数，

指已知点的物方坐标改正数，

指待定点的物方坐标改正数，y₁指已知点的物方坐标，v指观测值残差，l指常量。第二个方程是根据已知点的先验信息建立起来的虚拟观测方程。第三个方程是左右相机之间的约束方程，这个方程是通过实验前对相机与IMU之间的几何关系进行检校而建立起来的。

(4)根据最小二乘原理求解式(1)，实际上，式(1)是带约束的间接平差模型，因此可以通过平差求得左右相机的外方位元素，以及所有特征点的物方坐标。

2)惯性导航计算

选取当地水平坐标系(L系)作为导航系，L系的三轴指向为“东、北、天”，载体系(b系)的三轴指向为“右、前、上”。导航计算的步骤如下：

(1)姿态更新。利用陀螺输出的角速度观测值，采用四元数法作姿态更新，然后将更新后的四元数转化成姿态矩阵，记为

(2)姿态提取。根据姿态矩阵

中元素的值，计算姿态的姿态，包括航向角、俯仰角、翻滚角。

(3)比力分解。利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，如下所示

$f^L=R_b^L(k+1)\·f^b---\left(2\right)$

(4)有害加速度的补偿。对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度，如下所示

$a=f^L-({2\mathrm{\Ω}}_{\mathrm{ie}}+{\mathrm{\Ω}}_{\mathrm{eL}}^L)v^L+g^L---\left(3\right)$

(5)积分求取速度。利用加速度进行积分得到速度增量，进而得到当前时刻的速度，如下所示

$v_{k+1}^L=v_l^L+a\·\mathrm{\Δt}---\left(4\right)$

(6)积分求取位置。利用速度进行积分得到位置增量，进而得到当前时刻的位置，如下所示

$r_{k+1}^L=r_k^L+\frac{1}{2}(v_k^L+v_{k+1}^L)\·\mathrm{\Δt}---\left(5\right)$

3)视觉***修正惯导***

视觉***修正惯导***通过卡尔曼滤波来实现，取惯导***的误差方程作为状态方程，图2显示了其原理和过程，从图中可以看出，***的量测值是惯性导航计算得到的位置与视觉导航解算的位置之差，以及惯性导航计算得到的姿态与视觉导航解算的姿态之差。

取15阶的误差状态向量，包括位置误差、速度误差、平台失准角、陀螺漂移、加速度计零偏。如下所示

X＝(δr^L，δv^L，δε^L，d，b)    (6)

状态方程取惯导***的误差方程，形式如下

$\stackrel{\·}{X}=F\·X+G\·\mathrm{\ω}---\left(7\right)$

***的外部量测信息来自视觉导航所获取的相机位置和姿态。因此量测方程可以写出如下形式

$\left(\begin{array}{c}{Z}_r\\ Z_a\end{array}\right)=\left(\begin{array}{c}{H}_r\\ H_a\end{array}\right)\·X+\left(\begin{array}{c}{V}_r\\ V_a\end{array}\right)---\left(8\right)$

上式中Z_r，Z_a分别表示位置误差量测和姿态误差量测。位置误差Z_r按如下方式计算

Z_r＝r_ins-r_vo    (9)

上式中，r_ins指INS导航计算得到的坐标，r_vo表示视觉导航解算得到的坐标。由于视觉导航所确定的坐标是相机投影中心的坐标，而INS导航计算得到的是IMU几何中心的坐标，因此需要对r_vo进行转化，扣除相机与IMU之间的三维偏置，从而将相机投影中心的坐标归算到IMU几何中心。

位置误差量测矩阵H_r的形式如下

$H_r=\left(\begin{array}{cc}\underset{3\×3}{I}& \underset{12\×3}{0}\end{array}\right)---\left(10\right)$

式中，I为单位阵，0为零矩阵。姿态误差Z_a的计算方式如下

Z_a＝a_ins-a_vo    (11)

其中a_ins表示INS导航计算得到的姿态，a_vo表示视觉导航解算得到的姿态。这里要注意的是，INS导航计算得到的姿态是载体的姿态，而视觉导航解算得到的是相机的姿态，确切的说是指像空间坐标系到世界坐标系的三个旋转欧拉角，因此要考虑相机与IMU之间的旋转矩阵，将相机的姿态归算到载体的姿态。

a_vo＝(p′，r′，y′)^T    (12)

式中p′，r′，y′表示由相机姿态归算得到的载体俯仰角、翻滚角和航向角。姿态误差量测矩阵H_a的形式如下

$H_a=\left(\begin{array}{ccc}\underset{6\×3}{0}& \underset{3\×3}{A}& \underset{6\×3}{0}\end{array}\right)---\left(13\right)$

A阵的形式如下

$A=\left(\begin{array}{ccc}& & \\ \cos y& \sin y& 0\\ -\frac{\sin y}{\cos p}& \frac{\cos y}{\cos p}& 0\\ \frac{\sin y\sin p}{\cos p}& -\frac{\cos y\sin p}{\cos p}& 1\end{array}\right)---\left(14\right)$

式中，p，r，y分别表示由惯性导航计算得到的载体俯仰角、翻滚角和航向角。至此，卡尔曼滤波的函数模型已经确立，通过滤波可以估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正，从而使惯导***保持长时间的高精度。

4)惯导***修正视觉***

惯导***修正视觉***采用序贯平差来实现，图3显示了其原理和过程。从图中可以看出，通过惯性导航计算，可以得到每个时刻的导航参数，同时根据惯导***的误差方程，按协方差传播律，可以计算出参数之间的方差-协方差阵。然后从导航参数中分离出位置和姿态，根据相机与IMU之间的几何关系，转化成相机位置和姿态的观测值，采用序贯平差处理，实现视觉***导航参数的优化。

序贯平差一般用于数据分期的情况，也就是说在不同的时刻对***进行了观测，而且利用先前的观测值对***进行过平差处理，获得了参数估值，以及参数的精度信息(方差-协方差阵)。序贯平差要解决的问题就是如何利用先前获得的参数估值和精度信息，与新增加的观测值一起处理，得到更加优化的参数估值。设总的观测方程为

$v=B\hat{x}-l---\left(15\right)$

将观测数据分为两组，则观测方程可以写成以下形式

$\left\{\begin{array}{c}{v}_1=B_1\hat{x}-l_1\\ v_2=B_2\hat{x}-l_2\end{array}\right.---\left(16\right)$

假设上式第一个方程中的观测值个数足够，按最小二乘原理求解第一个方程，得

$\left.\begin{array}{c}{\hat{x}}_1={\left(B_1^T{P}_1{B}_1\right)}^{-1}{B}_1^T{P}_1{l}_1\\ Q_{{\hat{x}}_1}={\left(B_1^T{P}_1{B}_1\right)}^{-1}\end{array}\right\}---\left(17\right)$

上式中的P₁表示第一个方程中观测值的权阵。同时求解两个方程，得：

$\left\{\begin{array}{c}\hat{x}={(B_1^T{P}_1{B}_1+B_2^T{P}_2{B}_2)}^{-1}(B_1^T{P}_1{l}_1+B_2^T{P}_2{l}_2)\\ Q_{\hat{x}}={(B_1^T{P}_1{B}_1+B_2^T{P}_2{B}_2)}^{-1}\end{array}\right.---\left(18\right)$

上式中的P₂表示第二个方程中观测值的权阵。根据矩阵反演公式，可以导出序贯平差的计算公式，如下所示

$\left\{\begin{array}{c}\hat{x}={\hat{x}}_1+Q_{{\hat{x}}_1}{B}_2^T{(P_2^{-1}+B_2{Q}_{{\hat{x}}_1}{B}_2^T)}^{-1}(l_2-B_2{\hat{x}}_1)\\ Q_{\hat{x}}=Q_{{\hat{x}}_1}-Q_{{\hat{x}}_1}{B}_2^T{(P_2^{-1}+B_2{Q}_{{\hat{x}}_1}{B}_2^T)}^{-1}{B}_2{Q}_{{\hat{x}}_1}\end{array}\right.---\left(19\right)$

现在介绍采用序贯平差实现惯导***对视觉***的修正。视觉导航解算完成以后，可以得到相机参数的估值X_vo，通过精度评定，可以得到参数之间的方差-协方差阵P(X_vo)。惯导***通过导航计算，可以获得相应时刻的导航参数X_ins，同时根据惯导***的误差方程，按协方差传播律，计算参数之间的方差-协方差阵，记为P(X_ins)。从惯导***的导航参数X_ins中分离出位置r_ins和姿态a_ins，从方差-协方差阵P(X_ins)中分离出位置与姿态相关的子阵P(r_ins，a_ins)，考虑相机与IMU之间的几何关系，转化为对视觉参数的观测值，于是惯导***修正视觉***的函数模型可以写成以下形式：

$\left\{\begin{array}{c}{X}_{\mathrm{vo}}=\left(\begin{array}{cccc}I& I& I& I\end{array}\right)\left(\begin{array}{c}{\hat{x}}_1\\ {\hat{x}}_2\\ {\hat{y}}_1\\ {\hat{y}}_2\end{array}\right)-l_{X_{\mathrm{vo}}}\\ \left(\begin{array}{c}{r}_{\mathrm{ins}}\\ a_{\mathrm{ins}}\end{array}\right)=\left(\begin{array}{cccc}I& 0& 0& 0\end{array}\right)\left(\begin{array}{c}{\hat{x}}_1\\ {\hat{x}}_2\\ {\hat{y}}_1\\ {\hat{y}}_2\end{array}\right)-l_{X_{\mathrm{ins}}}\end{array}\right.---\left(20\right)$

将上式整理成观测方程的形式，如下所示：

$\left\{\begin{array}{c}v\left(x_{\mathrm{vo}}\right)={\hat{x}}_{\mathrm{vo}}-l\left(x_{\mathrm{vo}}\right)\\ v(r_{\mathrm{ins}},a_{\mathrm{ins}})=B{\hat{x}}_{\mathrm{vo}}-l(r_{\mathrm{ins}},a_{\mathrm{ins}})\end{array}\right.---\left(21\right)$

上式中，矩阵B的形式如下

B＝(I 0 0 0)    (22)

观测值的随机模型(方差-协方差阵)如下：

$\left\{\begin{array}{c}D\left(X_{\mathrm{vo}}\right)=P\left(V_{\mathrm{vo}}\right)\\ D(r_{\mathrm{ins}},a_{\mathrm{ins}})=P(r_{\mathrm{ins}},a_{\mathrm{ins}})\end{array}\right.---\left(23\right)$

根据上面的函数模型和随机模型，可以导出视觉***的修正公式，如下所示

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{vo}}=x_{\mathrm{vo}}+D\left(X_{\mathrm{vo}}\right)B^T{(D(r_{\mathrm{ins}},a_{\mathrm{ins}})+\mathrm{BD}\left(X_{\mathrm{vo}}\right)B^T)}^{-1}(l(r_{\mathrm{ins}},a_{\mathrm{ins}})-{\mathrm{Bx}}_{\mathrm{vo}})\\ D\left({\hat{x}}_{\mathrm{vo}}\right)=D\left(X_{\mathrm{vo}}\right)-D\left(X_{\mathrm{vo}}\right)B^T{(D(r_{\mathrm{ins}},a_{\mathrm{ins}})+\mathrm{BD}\left(X_{\mathrm{vo}}\right)B^T)}^{-1}\mathrm{BD}\left(X_{\mathrm{vo}}\right)\end{array}\right.0---\left(24\right)$

上式中，x_vo是指由视觉导航解算得到的视觉参数，

经过修正后的视觉参数，

是经修正后的方差-协方差阵。采用上式计算，可以修正视觉***的所有参数，包括左右相机的位置、姿态，以及所有特征点的物方坐标。

如果纯粹从算法上讲，第1步与第2步其实是并列运行的，第3步与第4步也是并列运行的。即首先视觉***与惯导***各自进行导航解算，求得相应的参数，然后两个子***相互辅助，使得每个子***都得到修正，获得最优估值。第3步获得的IMU最优估值与第4步中获得的相机最优估值

都是对整个***的最优估计，或者说是对载体运行状态的最优估计。理论上来讲，在不考虑计算误差的情况下，

与

是等价的。换句话说，如果将相机与IMU之间的几何关系考虑进去，那么

与

应该一致。

为了验证本发明方法的可靠性和稳定性，进行了多次实验和数据处理，以下介绍其中一次实验过程和数据处理结果。实验地点位于北京，载体车上装有立体相机和IMU等数据采集设备。另外还加装了一个GPS接收机，与基站GPS作差分，差分结果达到厘米级的精度，因此作为参考值来检验本发明方法的精度。在实验前已经完成了立体相机内方位元素检校和相对外方位元素检校，以及相机与IMU之间的相对位置和姿态的检校。实验过程中，首先利用已知绝对坐标信息的地标点进行相机外方位元素初始化。初始化完成之后，控制载体车在实验区采集IMU数据、影像数据、GPS数据。IMU的输出频率为100Hz，其陀螺漂移为0.005°/s，加速度计零偏为10mg，通过IMU的内置时钟授时立体相机以2Hz的频率通过同步曝光获取立体影像，立体相机的水平和垂直视场角分别为42°和31°。GPS接收机的输出频率为1Hz。实验中，移动实验平台累计行进约1150m，实验时长约45min，获取了约4300帧立体影像。

对实验数据采用两种导航方法进行处理。方法一是纯视觉导航解算，当视觉导航解算过程出现失败时，利用匀速运动模型预测载体的运动，填补视觉导航缺口，方法二是视觉导航与惯性导航进行组合。实际上，作者也尝试进行了纯惯性导航计算，但是对于长达45min的实验数据，其导航误差达到10000m以上的量级，因此纯惯性导航已经没有意义，这里不作讨论。以下给出两种不同方法处理的结果，以及GPS参考轨迹，见图4。

图4中的图例，GPS表示差分GPS结果，VO/INS表示视觉/惯性组合导航结果，VO表示纯视觉导航结果。从图中可以看出，视觉/惯性组合导航的轨迹与GPS轨迹很接近，其精度高于纯视觉导航的结果。表1列出了两种不同导航方法的误差情况。从表中可以看出，视觉/惯性组合导航在1150m的行驶距离中，累计位置误差为8.6m，相对精度为0.75％。

表1不同导航方法的误差情况

数据处理方法	位置误差(m)	相对误差
			视觉导航	18.67	1.6％
视觉/惯性组合	8.63	0.75％

实验过程中，载体车有时横跨光秃的路面，有时转弯的角度较大，导致出现6次短时间的视觉导航解算失败，这时组合***完全依靠惯性导航提供导航参数。从图4中的VO/INS组合轨迹可以看出，在视觉导航解算失败时，***并未出现明显的精度下降，这体现出本方法的可靠性和高精度。

图5和图6显示了在视觉***修正惯导***的过程中，卡尔曼滤波器的新息情况。在本发明方法所采用的卡尔曼滤波器中，新息表示的是惯性导航计算得到的导航参数与视觉导航解算得到的导航参数两者之间的差值。

从图5可以看出，位置差值的最大值在0.15m以内，且绝大部分情况下都在0.05m以内，从图6可以看出，姿态差值的最大值在0.1°以内，且绝大部分情况下都在0.01°以内，这说明滤波过程是比较平稳的，视觉导航和惯性导航两个***能够很好的组合，结合前面采用GPS绝对参考值的精度评估，充分显示了本发明所提出的组合算法的可靠性和优越性。

Claims (7)

1.一种视觉导航/惯性导航的全组合方法，其特征在于，该方法包括以下步骤：

1)视觉导航解算：相机在载体运动过程中获取影像，以共线方程为基础列立观测方程，采用最小二乘原理平差求取载***置和姿态参数X_vo，并计算参数之间的方差-协方差阵P(X_vo)；

2)惯性导航计算：在当地水平坐标系中做导航计算，得到每个时刻载体的位置、速度和姿态参数X_ins，同时根据惯导***的误差方程，按协方差传播律，计算参数之间的方差-协方差阵P(X_ins)；

3)视觉***修正惯导***：以卡尔曼滤波为手段，取惯导***的误差方程作为状态方程，利用视觉导航解算得到的载***置、姿态，与惯导***计算得到的载***置、姿态分别求差，作为卡尔曼滤波的量测，估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正，从而得到惯导***所有参数的最优估值，记为 

4)惯导***修正视觉***：从惯导***的参数中分离出IMU的位置和姿态，考虑相机与IMU之间的几何关系，将IMU的位置和姿态转化成相机的位置和姿态信息，建立观测方程，采用序贯平差处理，得到视觉***所有参数的最优估值，记为 

2.根据权利要求1所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的视觉导航解算具体步骤包括：

11)相机外方位元素的初始化：在视觉导航起始位置的周围布设控制点，通过后方交会求取相机的外方位元素；

12)载体不停的运动，利用相机获取当前时刻的影像，进行特征点提取和匹配；

13)以共线方程为基础，对左右影像匹配成功的所有特征点都建立观测方程：

上式中， 指左相机的外方位元素改正数， 指右相机的外方位元素改正数， 指已知点的物方坐标改正数， 

指待定点的物方坐标改正数，y₁指已知点的物方坐标v指观测值残差，l为常量， 

为左右相机之间的约束关系，W_x为约 束方程中的常量；

14)根据最小二乘原理求解上式，求得左右相机的外方位元素，以及所有特征点的物方坐标。

3.根据权利要求1所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的特征点匹配包括两个方面：一是将当前时刻的左右影像进行特征点匹配，二是将当前时刻的左影像与上一时刻的左影像进行特征点匹配；所述的特征点包括已知点和待定点：已知点是指在上一帧影像中已经出现过的特征点，待定点则是指当前影像中新增加的特征点。

4.根据权利要求1所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的惯性导航计算步骤具体包括：

(1)姿态更新：利用陀螺输出的角速度观测值，采用四元数法作姿态更新，然后将更新后的四元数转化成姿态矩阵，记为 

k表示上一时刻，k+1表示当前时刻，L为导航坐标系，b为载体坐标系；

(2)姿态提取：根据姿态矩阵 

中元素的值，计算载体的姿态，包括航向角、俯仰角、翻滚角；

(3)比力分解：利用姿态矩阵，将加速度计输出的比力观测值f^b由载体坐标系转换到导航坐标系，如下所示

(4)有害加速度的补偿：对f^L进行哥氏加速度修正和正常重力补偿，得到载体在地球表面的加速度a，如下所示

其中， 为地球自转角速度三维分量构成的反对称阵， 

为L系相对于地固系旋转角速度三维分量构成的反对称阵，v^L为速度矢量，g^L为地球重力矢量；

(5)积分求取速度：利用加速度进行积分得到速度增量，进而得到当前时刻的速度 

如下所示

(6)积分求取位置：利用速度进行积分得到位置增量，进而得到当前时刻的位置 

如下所示

。

5.根据权利要求1所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的视觉***修正惯导***步骤具体包括：

31)取惯导***的误差方程作为卡尔曼滤波器的状态方程，形式如下

其中，X为误差状态向量，包括位置误差δr^L、速度误差δv^L、平台失准角δε^L、陀螺漂移d和加速度计零偏b，

X＝(δr^L，δv^L，δε^L，d，b)

32)将惯性导航计算得到的位置与视觉导航解算的位置之差，以及惯性导航计算得到的姿态与视觉导航解算的姿态之差作为量测值，量测方程如下

Z_r，Z_a分别表示位置误差量测和姿态误差量测，H_r，H_a分别表示位置误差量测矩阵和姿态误差量测矩阵，V_r，V_a分别表示位置误差量测和姿态误差量测的残差；

位置误差Z_r按如下方式计算

Z_r＝r_ins-r_vo

上式中，r_ins指惯性导航计算得到的坐标，r_vo表示视觉导航解算得到的坐标；位置误差量测矩阵H_r的形式如下

式中，I为单位阵，0为零矩阵，姿态误差Z_a的计算方式如下

Z_a＝a_ins-a_vo

其中a_ins表示惯性导航计算得到的姿态，a_vo表示视觉导航解算得到的姿态；考虑相机与IMU之间的旋转矩阵，将视觉导航解算得到的相机姿态归算到载体的姿态：

a_vo＝(p′，r′，y′)^T

式中p′，r′，y′表示由相机姿态归算得到的载体俯仰角、翻滚角和航向角；姿态误差量测矩阵H_a的形式如下

A阵的形式如下 

式中，p，r，y分别表示由惯性导航计算得到的载体俯仰角、翻滚角和航向角；

33)通过滤波估计出惯导***的导航参数误差和器件误差，并进行补偿和反馈校正。

6.根据权利要求1所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的惯导***修正视觉***步骤具体包括：

41)从导航参数中分离出IMU的位置和姿态，根据相机与IMU之间的几何关系，将IMU的位置和姿态转化成相机位置和姿态的观测值；

42)采用序贯平差处理，完成惯导***对视觉***的修正。

7.根据权利要求6所述的一种视觉导航/惯性导航的全组合方法，其特征在于，所述的步骤42)具体包括：

惯导***修正视觉***的函数模型写成以下形式：

其中， 

分别为两个方程中对应的常量。

将上式整理成观测方程的形式，如下所示：

上式中，矩阵B的形式如下

B＝(I 0 0 0)

观测值的随机模型，即方差-协方差阵如下： 

上式中，P(r_ins，a_ins)为由P(X_ins)中分离出的与位置、姿态相关的子阵，根据上述的函数模型和随机模型，可以导出视觉***的修正公式，如下所示

上式中，x_vo是指由视觉导航解算得到的视觉参数， 

经过修正后的视觉参数， 是经修正后的方差-协方差阵。 

Images