CN109443355A - 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 - Google Patents

Abstract

本发明涉及视觉‑惯性紧耦合组合导航方法，具体是一种基于自适应高斯PF的视觉‑惯性紧耦合组合导航方法。本发明改善了视觉‑惯性紧耦合组合导航方法的实时性和导航精度。基于自适应高斯PF的视觉‑惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计；步骤S2：建立线性状态方程；建立非线性量测方程；步骤S3：采用自适应高斯PF对视觉‑惯性紧耦合组合导航系统进行非线性滤波，实现视觉‑惯性紧耦合组合导航系统的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正。本发明适用于视觉‑惯性紧耦合组合导航。

Description

基于自适应高斯PF的视觉-惯性紧耦合组合导航方法

技术领域

本发明涉及视觉-惯性紧耦合组合导航方法，具体是一种基于自适应高斯PF的视觉-惯性紧耦合组合导航方法。

背景技术

近年来，基于各种原理的单独导航系统不断发展，其性能日趋完善。但是，任何一种单独的导航子设备或者子系统不可能完全满足日益提高的导航要求，因此可以实现优势互补的组合导航技术的应用正在不断扩展，并受到了越来越广泛的重视。

捷联惯导系统具有低成本、小体积、全自主、隐蔽性好、采样频率高等优势，但是，其误差随时间发散。基于视觉传感器的视觉里程计是目前新兴的一种导航设备，具有价格低、耗能少、信息量丰富等优势，因此视觉里程计迅速得到了广泛的关注与应用。由于将捷联惯导系统与基于视觉传感器的视觉里程计进行组合可以实现优势互补，有效克服捷联惯导系统的长时发散，从而提高整个导航系统的精度，因此，基于捷联惯导系统/视觉里程计的组合导航技术是组合导航领域的一个十分重要的发展方向。捷联惯导系统/视觉里程计的组合导航分为松耦合和紧耦合，松耦合是捷联惯导系统和视觉里程计在各自完成运动解算的基础上进行耦合的方式，组合导航滤波器计算量较小，但是没有利用视觉里程计的原始图像信息；紧耦合利用视觉里程计图像匹配的原始坐标信息和捷联惯导系统进行耦合，能够取得比松耦合更高精度的组合导航结果。本发明属于视觉-惯性紧耦合组合导航。

由于视觉-惯性紧耦合组合导航的观测方程为强非线性，因此，高效地实现高精度的非线性组合滤波成为视觉-惯性紧耦合组合导航中的关键技术之一。高斯粒子滤波(高斯PF)可以通过平方根容积卡尔曼滤波对粒子正态分布参数进行优化，从而取得较高精度的紧耦合组合滤波结果，但是，由于平方根容积卡尔曼滤波在时间更新和量测更新阶段都进行容积点的计算以及加权求和，计算量比较大，实时性较差。针对这些不足，本发明根据视觉-惯性紧耦合组合导航的状态方程为线性而观测方程为强非线性的特点提出了一套完整的技术方案实现视觉-惯性紧耦合组合导航。在自适应平方根容积卡尔曼滤波的时间更新阶段，不采用平方根容积卡尔曼滤波的方法而是根据线性KF的方法更新状态量的均值和方差，避免了容积点计算以及加权求和过程，使计算量下降；在自适应平方根容积卡尔曼的量测更新阶段，采用基于自适应模糊推理的平方根容积卡尔曼滤波实现紧耦合组合导航，使紧耦合组合导航的精度不会因量测噪声统计特性随着环境和运动状态变化而下降。然后依据Levenberg-Marquard法实现对自适应平方根容积卡尔曼滤波结果的迭代更新，从而得到进一步优化的粒子正态分布均值和方差参数估计，最后采用高斯PF实现紧耦合组合导航，另外，采用线性变换替代传统高斯PF中的高斯采样生成相同分布的粒子，使计算量进一步下降。本发明能够较好地解决视觉-惯性紧耦合组合导航系统的非线性滤波问题，一方面改善了视觉-惯性紧耦合组合导航的实时性，同时能够一定程度地提高视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。

发明内容

本发明针对现有技术的不足，提供了一种基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，改善了视觉-惯性紧耦合组合导航的实时性和在环境和运动状态变化时的精度。

本发明是采用如下技术方案实现的：

基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：

步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；

步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；

步骤S3：采用自适应高斯PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；

步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

本发明的有效效果：一、由于在自适应平方根容积卡尔曼滤波时间更新阶段没有采用平方根容积卡尔曼滤波的方法，而是根据视觉-惯性紧耦合组合导航系统的状态方程为线性的特点，利用线性KF的方法完成更新，使计算量下降，另外，在高斯PF中，采用线性变换替代传统高斯PF中的高斯采样生成相同分布的粒子，计算量进一步下降，改善了视觉-惯性紧耦合组合导航系统的实时性。二、由于在自适应平方根容积卡尔曼滤波的时间更新阶段采用的线性KF状态更新过程是最优估计，而标准的平方根容积卡尔曼滤波状态更新过程是次优的，同时，在自适应平方根容积卡尔曼滤波中采用模糊推理系统对量测噪声的统计特性进行自适应的调整，使组合导航过程中采用的量测噪声统计特性更接近真实值，提高了视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。

本发明的效果通过如下实验得到验证：

利用德国卡尔斯鲁厄理工学院和丰田技术研究所(Karlsruhe Institute ofTechnology and Toyota Technological Institute，KITTI)的开源数据对本发明的性能进行验证。KITTI研究所的车载实验是以一辆名为Annieway的自主车为平台，搭载有多种传感器来采集周围环境中的数据。实验中陀螺常值漂移为36°/h，加速度计常值漂移为1.0204×10^-3g，捷联惯导系统的数据采集频率为100Hz，双目视觉里程计的分辨率为1226×370像素，基线距离为54cm，高度为1.65m，图像采集频率为10Hz。图2为本次实验中运载体的真实运动轨迹。本实验采用传统方法和本发明对实验中的数据进行处理，分别实现视觉-惯性紧耦合组合导航系统的导航定位，实验结果如图3～图4。从图3～图4可以看出，与传统方法相比，本发明所取得的运动轨迹更接近运载体的真实运动轨迹，本发明的定位误差最大值约为13.89m，而传统方法的定位误差最大值约为17.76m。图3～图4中的位置误差尖峰是由于提供真实轨迹信息的GPS信号的短暂缺失导致的，并不是真实的位置误差信息，平稳阶段的位置误差信息是真实的误差信息。为了排除不同计算机对滤波时间的影响，本实验以传统方法的相对平均单步滤波时间为参照对相对平均单步滤波时间进行了归一化。从图5可以看出，与传统方法相比，本发明的相对平均单步滤波时间降为传统方法的65.36％。综上所述，本发明一方面有效改善了导航实时性，另一方面进一步提高了导航精度。

本发明改善了视觉-惯性紧耦合组合导航方法的实时性和导航精度，适用于视觉-惯性紧耦合组合导航。

附图说明

图1是本发明中步骤S3～S4的流程示意图。

图2是实验中运载体的真实运动轨迹。

图3是实验中本发明和传统方法的X方向位置误差对比示意图。

图4是实验中本发明和传统方法的Y方向位置误差对比示意图。

图5是实验中本发明和传统方法的相对平均单步滤波时间对比示意图。

具体实施方式

基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：

步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；

步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；

步骤S3：采用自适应高斯PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；

步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

所述步骤S3包括如下步骤：

步骤S3.1：设定组合导航滤波器状态量均值的初值和组合导航滤波器状态量方差的初值P_k-1；

步骤S3.2：计算出时间更新后的组合导航滤波器状态量均值和时间更新后的组合导航滤波器状态量方差P_k/k-1；计算公式如下：

式中：Φ_k/k-1表示视觉-惯性紧耦合组合导航系统的状态转移矩阵的离散矩阵；Q_k表示视觉-惯性紧耦合组合导航系统的过程噪声的协方差矩阵；

步骤S3.3：计算出时间更新后的组合导航滤波器状态量方差P_k/k-1的乔列斯基分解因子S_k/k-1；计算公式如下：

S_k/k-1＝Chol(P_k/k-1)；

步骤S3.4：计算出平方根容积卡尔曼滤波算法的容积点容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值量测预测值新息方差矩阵的平方根协方差的平方根滤波增益组合导航滤波器状态量均值组合导航滤波器状态量方差P_k；计算公式如下：

式中：ξ_j表示容积点计算参数；n表示视觉-惯性紧耦合组合导航系统的状态维数；f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；w_j表示容积点权值；Tria表示对矩阵进行QR分解运算；R_k表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；z_k表示视觉-惯性紧耦合组合导航系统的量测值；

步骤S3.5：设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：

式中：I表示单位矩阵；μ表示可调参数；

步骤S3.6：计算出基于LM算法的迭代策略第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值计算公式如下：

式中：f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；R_k表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；

步骤S3.7：当基于LM算法的迭代策略的迭代次数达到N时，基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值从而得到优化的正态分布设定公式如下：

式中：表示基于LM算法的迭代策略第N次迭代得到的组合导航滤波器状态量均值；

步骤S3.8：建立高斯PF算法的初始粒子群其中每个粒子均从N(0,1)独立采样得到，通过对每个粒子进行线性变换得到粒子集

式中：U表示高斯PF算法的粒子数目；

步骤S3.9：采用高斯PF算法计算出组合导航滤波器状态量均值μ_k和组合导航滤波器状态量方差δ_k；计算公式如下：

式中：w_l,k表示归一化后的第l个粒子的权值；表示第l个粒子的权值；表示似然函数；表示先验概率密度；表示重要性密度函数；

步骤S3.10：对视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵R_k进行更新；更新公式如下：

R_k+1＝ε_kR_k；

式中：ε_k表示调节因子，其值由模糊推理系统进行自适应调整；

所述步骤S4包括如下步骤：

步骤S4.1：从计算出的组合导航滤波器状态量均值μ_k中抽取得到捷联惯导系统的误差估计

步骤S4.2：根据捷联惯导系统的误差估计对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果T_k；校正公式如下：

式中：表示捷联惯导系统的角度误差；δV_E、δV_N、δV_U表示捷联惯导系统的速度误差；δR_E、δR_N、δR_U表示捷联惯导系统的位置误差。

所述步骤S3.10中，模糊推理系统的输入为量测噪声失配比m_k，模糊推理系统的输出为调节因子ε_k；

所述量测噪声失配比m_k定义为：

式中：表示新息方差矩阵的迹；表示理论新息方差矩阵的迹；M表示新息方差矩阵的累加窗口；

模糊推理系统以1位参考，设S表示模糊集合小，E表示基本等于1，F表示模糊集合大，建立如下模糊推理规则：

A.如果m_k∈S，那么ε_k∈S；

B.如果m_k∈E，那么ε_k∈E；

C.如果m_k∈F，那么ε_k∈F。

所述步骤S2中，视觉-惯性紧耦合组合导航系统的线性状态方程的建立过程如下：

首先，将组合导航滤波器的状态量X定义为：

式中：表示捷联惯导系统的角度误差；δV_E、δV_N、δV_U表示捷联惯导系统的速度误差；δR_E、δR_N、δR_U表示捷联惯导系统的位置误差；ε_bx、ε_by、ε_bz表示沿载体坐标系三个方向的陀螺仪随机漂移误差；ζ_x、ζ_y、ζ_z表示加速度计的随机漂移误差；δp_x、δp_y、δp_z表示上张图像采集时的名义位置误差；δθ_x、δθ_y、δθ_z表示上张图像采集时的名义角度误差；E、N、U表示地理坐标系的三个方向；

然后，根据捷联惯导系统的误差特性，得到视觉-惯性紧耦合组合导航系统的线性状态方程：

W(t)＝[w_εx,w_εy,w_εz,w_ζx,w_ζy,w_ζz]^T；

式中：F(t)表示组合导航滤波器的状态转移矩阵；G(t)表示组合导航滤波器的噪声变换矩阵；W(t)表示组合导航滤波器的噪声向量；F_N(t)表示经典的惯性系统误差9×9矩阵；F_S(t)表示导航参数与惯性元件误差之间的变换矩阵；表示姿态矩阵；w_εx、w_εy、w_εz表示陀螺仪的噪声项；w_ζx、w_ζy、w_ζz表示加速度计的噪声项。

所述步骤S2中，视觉-惯性紧耦合组合导航系统的非线性量测方程的建立过程如下：

首先，根据组合导航滤波器的状态量X和捷联惯导系统的名义状态X_n，计算出右摄像机后一时刻相对前一时刻的旋转R_R、左摄像机后一时刻相对前一时刻的旋转R_L、右摄像机后一时刻相对前一时刻的位移t_R、左摄像机后一时刻相对前一时刻的位移t_L，计算公式表示如下：

式中：表示由组合导航滤波器的状态量对捷联惯导系统的名义状态得到捷联惯导的真实状态信息；R和t分别表示由捷联惯导系统的真实状态信息得到旋转量和位移量的函数关系；

然后，令前一时刻右摄像机的坐标系与世界坐标系对准，即令前一时刻右摄像机的投影矩阵P_R,k为：

P_R,k＝K_R[I|0]；

式中：K_R表示右摄像机的标定矩阵；

通过标定获取前一时刻左摄像机的相对位姿[R_C|t_C]，由此将前一时刻左摄像机的投影矩阵P_L,k表示为：

P_L,k＝K_L[R_C|t_C]；

式中：K_L表示左摄像机的标定矩阵；R_C表示左摄像机相对右摄像机的旋转；t_C表示左摄像机相对右摄像机的位移；

后一时刻右摄像机的投影矩阵P_R,k+1表示为：

P_R,k+1＝K_R[R_R|t_R]；

后一时刻左摄像机的投影矩阵P_L,k+1表示为：

P_L,k+1＝K_L[R_L|t_L]；

然后，计算出右摄像机采集到的连续两帧图像的三焦点张量T_R和左摄像机采集到的连续两帧图像的三焦点张量T_L；计算公式如下：

T_R＝T(K_R,K_L,R_C,t_C,R_R,t_R)；

T_L＝T(K_R,K_L,R_C,t_C,R_L,t_L)；

式中：T表示三焦点张量的计算公式；

然后，根据三焦点张量的非线性转移函数H，得到视觉-惯性紧耦合组合导航系统的非线性量测方程：

x_R,k+1＝H(T_R,x_R,k,x_L,k)；

x_L,k+1＝H(T_L,x_R,k,x_L,k)；

式中：x_R,k+1表示后一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；x_L,k+1表示后一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；x_R,k表示前一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；x_L,k表示前一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；

然后，令x_k+1＝[x_R,k+1,x_L,k+1]^T，则视觉-惯性紧耦合组合导航系统的非线性量测方程表示为：

根据以上视觉-惯性紧耦合组合导航系统的非线性量测方程产生由组合导航滤波器状态量X到像素点坐标信息的非线性映射关系：

x_k+1＝f(X)。

Claims (5)

1.一种基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：

步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；

步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；

步骤S3：采用自适应高斯PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；

步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

2.根据权利要求1所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：

所述步骤S3包括如下步骤：

步骤S3.1：设定组合导航滤波器状态量均值的初值和组合导航滤波器状态量方差的初值P_k-1；

步骤S3.2：计算出时间更新后的组合导航滤波器状态量均值和时间更新后的组合导航滤波器状态量方差P_k/k-1；计算公式如下：

式中：Φ_k/k-1表示视觉-惯性紧耦合组合导航系统的状态转移矩阵的离散矩阵；Q_k表示视觉-惯性紧耦合组合导航系统的过程噪声的协方差矩阵；

步骤S3.3：计算出时间更新后的组合导航滤波器状态量方差P_k/k-1的乔列斯基分解因子S_k/k-1；计算公式如下：

S_k/k-1＝Chol(P_k/k-1)；

步骤S3.4：计算出平方根容积卡尔曼滤波算法的容积点容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值量测预测值新息方差矩阵的平方根协方差的平方根滤波增益组合导航滤波器状态量均值组合导航滤波器状态量方差P_k；计算公式如下：

式中：ξ_j表示容积点计算参数；n表示视觉-惯性紧耦合组合导航系统的状态维数；f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；w_j表示容积点权值；Tria表示对矩阵进行QR分解运算；R_k表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；z_k表示视觉-惯性紧耦合组合导航系统的量测值；

步骤S3.5：设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：

式中：I表示单位矩阵；μ表示可调参数；

步骤S3.6：计算出基于LM算法的迭代策略第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值计算公式如下：

式中：f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；R_k表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；

步骤S3.7：当基于LM算法的迭代策略的迭代次数达到N时，基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值从而得到优化的正态分布设定公式如下：

式中：表示基于LM算法的迭代策略第N次迭代得到的组合导航滤波器状态量均值；

步骤S3.8：建立高斯PF算法的初始粒子群其中每个粒子均从N(0,1)独立采样得到，通过对每个粒子进行线性变换得到粒子集

式中：U表示高斯PF算法的粒子数目；

步骤S3.9：采用高斯PF算法计算出组合导航滤波器状态量均值μ_k和组合导航滤波器状态量方差δ_k；计算公式如下：

式中：w_l,k表示归一化后的第l个粒子的权值；表示第l个粒子的权值；表示似然函数；表示先验概率密度；表示重要性密度函数；

步骤S3.10：对视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵R_k进行更新；更新公式如下：

R_k+1＝ε_kR_k；

式中：ε_k表示调节因子，其值由模糊推理系统进行自适应调整；

所述步骤S4包括如下步骤：

步骤S4.1：从计算出的组合导航滤波器状态量均值μ_k中抽取得到捷联惯导系统的误差估计

步骤S4.2：根据捷联惯导系统的误差估计对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果T_k；校正公式如下：

式中：表示捷联惯导系统的角度误差；δV_E、δV_N、δV_U表示捷联惯导系统的速度误差；δR_E、δR_N、δR_U表示捷联惯导系统的位置误差。

3.根据权利要求2所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S3.10中，模糊推理系统的输入为量测噪声失配比m_k，模糊推理系统的输出为调节因子ε_k；

所述量测噪声失配比m_k定义为：

式中：表示新息方差矩阵的迹；表示理论新息方差矩阵的迹；M表示新息方差矩阵的累加窗口；

模糊推理系统以1位参考，设S表示模糊集合小，E表示基本等于1，F表示模糊集合大，建立如下模糊推理规则：

A.如果m_k∈S，那么ε_k∈S；

B.如果m_k∈E，那么ε_k∈E；

C.如果m_k∈F，那么ε_k∈F。

4.根据权利要求1或2或3所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S2中，视觉-惯性紧耦合组合导航系统的线性状态方程的建立过程如下：

首先，将组合导航滤波器的状态量X定义为：

式中：表示捷联惯导系统的角度误差；δV_E、δV_N、δV_U表示捷联惯导系统的速度误差；δR_E、δR_N、δR_U表示捷联惯导系统的位置误差；ε_bx、ε_by、ε_bz表示沿载体坐标系三个方向的陀螺仪随机漂移误差；ζ_x、ζ_y、ζ_z表示加速度计的随机漂移误差；δp_x、δp_y、δp_z表示上张图像采集时的名义位置误差；δθ_x、δθ_y、δθ_z表示上张图像采集时的名义角度误差；E、N、U表示地理坐标系的三个方向；

然后，根据捷联惯导系统的误差特性，得到视觉-惯性紧耦合组合导航系统的线性状态方程：

W(t)＝[w_εx,w_εy,w_εz,w_ζx,w_ζy,w_ζz]^T；

式中：F(t)表示组合导航滤波器的状态转移矩阵；G(t)表示组合导航滤波器的噪声变换矩阵；W(t)表示组合导航滤波器的噪声向量；F_N(t)表示经典的惯性系统误差9×9矩阵；F_S(t)表示导航参数与惯性元件误差之间的变换矩阵；表示姿态矩阵；w_εx、w_εy、w_εz表示陀螺仪的噪声项；w_ζx、w_ζy、w_ζz表示加速度计的噪声项。

5.根据权利要求1或2或3所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S2中，视觉-惯性紧耦合组合导航系统的非线性量测方程的建立过程如下：

首先，根据组合导航滤波器的状态量X和捷联惯导系统的名义状态X_n，计算出右摄像机后一时刻相对前一时刻的旋转R_R、左摄像机后一时刻相对前一时刻的旋转R_L、右摄像机后一时刻相对前一时刻的位移t_R、左摄像机后一时刻相对前一时刻的位移t_L，计算公式表示如下：

式中：表示由组合导航滤波器的状态量对捷联惯导系统的名义状态得到捷联惯导的真实状态信息；R和t分别表示由捷联惯导系统的真实状态信息得到旋转量和位移量的函数关系；

然后，令前一时刻右摄像机的坐标系与世界坐标系对准，即令前一时刻右摄像机的投影矩阵P_R,k为：

P_R,k＝K_R[I|0]；

式中：K_R表示右摄像机的标定矩阵；

通过标定获取前一时刻左摄像机的相对位姿[R_C|t_C]，由此将前一时刻左摄像机的投影矩阵P_L,k表示为：

P_L,k＝K_L[R_C|t_C]；

式中：K_L表示左摄像机的标定矩阵；R_C表示左摄像机相对右摄像机的旋转；t_C表示左摄像机相对右摄像机的位移；

后一时刻右摄像机的投影矩阵P_R,k+1表示为：

P_R,k+1＝K_R[R_R|t_R]；

后一时刻左摄像机的投影矩阵P_L,k+1表示为：

P_L,k+1＝K_L[R_L|t_L]；

然后，计算出右摄像机采集到的连续两帧图像的三焦点张量T_R和左摄像机采集到的连续两帧图像的三焦点张量T_L；计算公式如下：

T_R＝T(K_R,K_L,R_C,t_C,R_R,t_R)；

T_L＝T(K_R,K_L,R_C,t_C,R_L,t_L)；

式中：T表示三焦点张量的计算公式；

然后，根据三焦点张量的非线性转移函数H，得到视觉-惯性紧耦合组合导航系统的非线性量测方程：

x_R,k+1＝H(T_R,x_R,k,x_L,k)；

x_L,k+1＝H(T_L,x_R,k,x_L,k)；

式中：x_R,k+1表示后一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；x_L,k+1表示后一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；x_R,k表示前一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；x_L,k表示前一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；

然后，令x_k+1＝[x_R,k+1,x_L,k+1]^T，则视觉-惯性紧耦合组合导航系统的非线性量测方程表示为：

根据以上视觉-惯性紧耦合组合导航系统的非线性量测方程产生由组合导航滤波器状态量X到像素点坐标信息的非线性映射关系：

x_k+1＝f(X)。