CN109443353A - 基于模糊自适应ickf的视觉-惯性紧耦合组合导航方法 - Google Patents

Abstract

本发明涉及视觉‑惯性紧耦合组合导航方法，具体是一种基于模糊自适应ICKF的视觉‑惯性紧耦合组合导航方法。本发明改善了视觉‑惯性紧耦合组合导航方法的实时性和导航精度。基于模糊自适应ICKF的视觉‑惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计；步骤S2：建立线性状态方程；建立非线性量测方程；步骤S3：采用线性KF算法进行时间更新；步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正。本发明适用于视觉‑惯性紧耦合组合导航。

Description

基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法

技术领域

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

背景技术

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

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

由于视觉-惯性紧耦合组合导航的观测方程为强非线性，因此，高效地实现高精度的非线性组合滤波成为视觉-惯性紧耦合组合导航中的关键技术之一。迭代容积卡尔曼滤波(ICKF)通过一组容积点近似系统状态变量的分布，能够取得较高精度的紧耦合组合滤波结果，但是，由于其在时间更新和量测更新阶段都进行容积点的计算以及加权求和，计算量比较大。另外，视觉里程计中的每一帧采用相同的迭代容积卡尔曼滤波步骤进行处理，整体计算量较大，不利于改善视觉-惯性紧耦合组合导航的实时性。针对这些不足，本发明把视觉里程计的图像分为关键帧和非关键帧，并根据其状态方程为线性而观测方程为强非线性的特点提出了一套完整的技术方案实现视觉-惯性紧耦合组合导航。在时间更新阶段，不采用容积卡尔曼滤波的方法而是根据线性KF的方法更新状态量的均值和方差，避免了容积点计算以及加权求和过程，使计算量下降；在量测更新阶段，双目视觉里程计的图像分为关键帧和非关键帧，关键帧采用模糊自适应系统修正量测噪声协方差阵，使紧耦合组合导航的精度不会因量测噪声统计特性随着环境和运动状态变化而下降，提高了视觉-惯性紧耦合组合导航系统的自适应能力；非关键帧把容积卡尔曼滤波算法的迭代量测更新过程转换为求解非线性最小二乘解问题，采用Levenberg-Marquard方法求解最终的状态和方差估计，从而以较快的收敛速度实现状态估计，改善了视觉-惯性紧耦合组合导航的实时性。本发明能够较好地解决视觉-惯性紧耦合组合导航系统的非线性滤波问题，一方面改善了视觉-惯性紧耦合组合导航的实时性，同时能够一定程度地提高视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。

发明内容

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

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

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

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

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

步骤S3：采用线性KF(Kalman Filter，卡尔曼滤波)算法对视觉-惯性紧耦合组合导航系统进行时间更新；

步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；

步骤S4.1：若为关键帧，则采用ICKF算法对视觉-惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉-惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；

步骤S4.2：若为非关键帧，则采用基于LM(Levenberg-Marquard)算法的迭代策略对视觉-惯性紧耦合组合导航系统进行量测更新；

步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

本发明的有效效果：一、视觉-惯性紧耦合组合导航系统中视觉里程计的图像分为关键帧和非关键帧，关键帧采用基于模糊自适应推理的迭代容积卡尔曼滤波实现紧耦合组合导航，而非关键帧把容积卡尔曼滤波算法的迭代量测更新过程转换为求解非线性最小二乘解问题，采用Levenberg-Marquard方法以较快的收敛速度求解最终的状态和方差估计，在保证组合导航精度相当的前提下，改善了视觉-惯性紧耦合组合导航系统的实时性。二、由于在时间更新阶段没有采用容积卡尔曼滤波的方法，而是根据视觉-惯性紧耦合组合导航系统的状态方程为线性的特点，利用线性KF的方法完成更新，使计算量下降，从而改善了视觉-惯性紧耦合组合导航系统的实时性。三、由于在时间更新阶段采用的线性KF状态更新过程是最优估计，而标准的容积卡尔曼滤波状态更新过程是次优的，同时，在关键帧的量测更新过程中采用模糊推理系统对量测噪声的统计特性进行自适应的调整，使组合导航过程中采用的量测噪声统计特性更接近真实值，提高了视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。

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

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

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

附图说明

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

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

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

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

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

具体实施方式

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

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

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

步骤S3：采用线性KF算法对视觉-惯性紧耦合组合导航系统进行时间更新；

步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；

步骤S4.1：若为关键帧，则采用ICKF算法对视觉-惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉-惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；

步骤S4.2：若为非关键帧，则采用基于LM算法的迭代策略对视觉-惯性紧耦合组合导航系统进行量测更新；

步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

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

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

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

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

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

步骤S4.1.1：设定ICKF算法的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：

步骤S4.1.2：计算出ICKF算法第i次迭代得到的容积点第i次迭代得到的容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值第i次迭代得到的量测预测值第i次迭代得到的新息方差第i次迭代得到的协方差第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值第i次迭代得到的组合导航滤波器状态量方差计算公式如下：

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

步骤S4.1.3：判断是否达到ICKF算法的迭代终止条件；

若未达到ICKF算法的迭代终止条件，则返回步骤S4.1.2，由此继续进行ICKF算法；

若达到ICKF算法的迭代终止条件，则ICKF算法迭代终止，由此设定ICKF算法的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值P_k；设定公式如下：

式中：表示ICKF算法第N次迭代得到的组合导航滤波器状态量均值；表示ICKF算法第N次迭代得到的组合导航滤波器状态量方差；N表示ICKF算法迭代终止时的迭代次数；

ICKF算法的迭代终止条件如下：

或者i＝N_max；

式中：ε表示ICKF算法迭代终止误差阈值；N_max表示ICKF算法的最大迭代次数；

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

R_k+1＝ε_kR_k；

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

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

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

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

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

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

步骤S4.2.3：判断是否达到基于LM算法的迭代策略的迭代终止条件；

若未达到基于LM算法的迭代策略的迭代终止条件，则返回步骤S4.2.2，由此继续采用基于LM算法的迭代策略进行迭代；

若达到基于LM算法的迭代策略的迭代终止条件，则基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值P_k；设定公式如下：

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

所述基于LM算法的迭代策略的迭代终止条件如下：

或者i＝N_max；

式中：ε表示基于LM算法的迭代策略迭代终止误差阈值；N_max表示基于LM算法的迭代策略的最大迭代次数；

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

步骤S5.1：从ICKF算法的组合导航滤波器状态量均值迭代终值或者基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值中抽取得到捷联惯导系统的误差估计

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

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

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

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

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

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

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

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

C.如果m_L∈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.一种基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：

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

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

步骤S3：采用线性KF算法对视觉-惯性紧耦合组合导航系统进行时间更新；

步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；

步骤S4.1：若为关键帧，则采用ICKF算法对视觉-惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉-惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；

步骤S4.2：若为非关键帧，则采用基于LM算法的迭代策略对视觉-惯性紧耦合组合导航系统进行量测更新；

步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。

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

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

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

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

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

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

步骤S4.1.1：设定ICKF算法的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：

步骤S4.1.2：计算出ICKF算法第i次迭代得到的容积点第i次迭代得到的容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值第i次迭代得到的量测预测值第i次迭代得到的新息方差第i次迭代得到的协方差第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值第i次迭代得到的组合导航滤波器状态量方差计算公式如下：

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

步骤S4.1.3：判断是否达到ICKF算法的迭代终止条件；

若未达到ICKF算法的迭代终止条件，则返回步骤S4.1.2，由此继续进行ICKF算法；

若达到ICKF算法的迭代终止条件，则ICKF算法迭代终止，由此设定ICKF算法的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值P_k；设定公式如下：

式中：表示ICKF算法第N次迭代得到的组合导航滤波器状态量均值；表示ICKF算法第N次迭代得到的组合导航滤波器状态量方差；N表示ICKF算法迭代终止时的迭代次数；

ICKF算法的迭代终止条件如下：

或者i＝N_max；

式中：ε表示ICKF算法迭代终止误差阈值；N_max表示ICKF算法的最大迭代次数；

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

R_k+1＝ε_kR_k；

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

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

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

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

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

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

步骤S4.2.3：判断是否达到基于LM算法的迭代策略的迭代终止条件；

若未达到基于LM算法的迭代策略的迭代终止条件，则返回步骤S4.2.2，由此继续采用基于LM算法的迭代策略进行迭代；

若达到基于LM算法的迭代策略的迭代终止条件，则基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值P_k；设定公式如下：

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

所述基于LM算法的迭代策略的迭代终止条件如下：

或者i＝N_max；

式中：ε表示基于LM算法的迭代策略迭代终止误差阈值；N_max表示基于LM算法的迭代策略的最大迭代次数；

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

步骤S5.1：从ICKF算法的组合导航滤波器状态量均值迭代终值或者基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值中抽取得到捷联惯导系统的误差估计

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

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

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

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

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

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

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

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

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

4.根据权利要求1或2或3所述的基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤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所述的基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤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)。