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

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

Visual-Inertial Tightly Coupled Integrated Navigation Method Based on Fuzzy Adaptive ICKF

The present invention relates to a vision-inertia tightly coupled integrated navigation method, in particular to a vision-inertia tightly coupled integrated navigation method based on fuzzy adaptive ICKF. The invention improves the real-time performance and navigation accuracy of the visual-inertial tightly coupled integrated navigation method. Based on the fuzzy adaptive ICKF, the visual-inertial tightly coupled integrated navigation method is realized by the following steps: installing strapdown inertial navigation system and binocular visual odometer on the carrier; establishing linear state equation; establishing non-linear measurement equation; adopting linear KF algorithm for time updating; and judging binocular visual mileage. Calculate whether each frame image is a key frame or not; Part 5: Calibrate the solution result of SINS according to the measurement update result. The present invention is applicable to the visual and inertial tightly coupled integrated navigation.

【技术实现步骤摘要】
基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法
本专利技术涉及视觉-惯性紧耦合组合导航方法，具体是一种基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法。
技术介绍
近年来，基于各种原理的单独导航系统不断发展，其性能日趋完善。但是，任何一种单独的导航子设备或者子系统不可能完全满足日益提高的导航要求，因此可以实现优势互补的组合导航技术的应用正在不断扩展，并受到了越来越广泛的重视。捷联惯导系统具有低成本、小体积、全自主、隐蔽性好、采样频率高等优势，但是，其误差随时间发散。基于视觉传感器的视觉里程计是目前新兴的一种导航设备，具有价格低、耗能少、信息量丰富等优势，因此视觉里程计迅速得到了广泛的关注与应用。由于将捷联惯导系统与基于视觉传感器的视觉里程计进行组合可以实现优势互补，有效克服捷联惯导系统的长时发散，从而提高整个导航系统的精度，因此，基于捷联惯导系统/视觉里程计的组合导航技术是组合导航领域的一个十分重要的发展方向。捷联惯导系统/视觉里程计的组合导航分为松耦合和紧耦合，松耦合是捷联惯导系统和视觉里程计在各自完成运动解算的基础上进行耦合的方式，组合导航滤波器计算量较小，但是没有利用视觉里程计的原始图像信息；紧耦合利用视觉里程计图像匹配的原始坐标信息和捷联惯导系统进行耦合，能够取得比松耦合更高精度的组合导航结果。本专利技术属于视觉-惯性紧耦合组合导航。由于视觉-惯性紧耦合组合导航的观测方程为强非线性，因此，高效地实现高精度的非线性组合滤波成为视觉-惯性紧耦合组合导航中的关键技术之一。迭代容积卡尔曼滤波(ICKF)通过一组容积点近似系统状态变量的分布，能够取得较高精度的紧耦合组合滤波结果，但是，由于其在时间更新和量测更新阶段都进行容积点的计算以及加权求和，计算量比较大。另外，视觉里程计中的每一帧采用相同的迭代容积卡尔曼滤波步骤进行处理，整体计算量较大，不利于改善视觉-惯性紧耦合组合导航的实时性。针对这些不足，本专利技术把视觉里程计的图像分为关键帧和非关键帧，并根据其状态方程为线性而观测方程为强非线性的特点提出了一套完整的技术方案实现视觉-惯性紧耦合组合导航。在时间更新阶段，不采用容积卡尔曼滤波的方法而是根据线性KF的方法更新状态量的均值和方差，避免了容积点计算以及加权求和过程，使计算量下降；在量测更新阶段，双目视觉里程计的图像分为关键帧和非关键帧，关键帧采用模糊自适应系统修正量测噪声协方差阵，使紧耦合组合导航的精度不会因量测噪声统计特性随着环境和运动状态变化而下降，提高了视觉-惯性紧耦合组合导航系统的自适应能力；非关键帧把容积卡尔曼滤波算法的迭代量测更新过程转换为求解非线性最小二乘解问题，采用Levenberg-Marquard方法求解最终的状态和方差估计，从而以较快的收敛速度实现状态估计，改善了视觉-惯性紧耦合组合导航的实时性。本专利技术能够较好地解决视觉-惯性紧耦合组合导航系统的非线性滤波问题，一方面改善了视觉-惯性紧耦合组合导航的实时性，同时能够一定程度地提高视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。
技术实现思路
本专利技术针对现有技术的不足，提供了一种基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，改善了视觉-惯性紧耦合组合导航的实时性和在环境和运动状态变化时的精度。本专利技术是采用如下技术方案实现的：基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；步骤S3：采用线性KF(KalmanFilter，卡尔曼滤波)算法对视觉-惯性紧耦合组合导航系统进行时间更新；步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；步骤S4.1：若为关键帧，则采用ICKF算法对视觉-惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉-惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；步骤S4.2：若为非关键帧，则采用基于LM(Levenberg-Marquard)算法的迭代策略对视觉-惯性紧耦合组合导航系统进行量测更新；步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。本专利技术的有效效果：一、视觉-惯性紧耦合组合导航系统中视觉里程计的图像分为关键帧和非关键帧，关键帧采用基于模糊自适应推理的迭代容积卡尔曼滤波实现紧耦合组合导航，而非关键帧把容积卡尔曼滤波算法的迭代量测更新过程转换为求解非线性最小二乘解问题，采用Levenberg-Marquard方法以较快的收敛速度求解最终的状态和方差估计，在保证组合导航精度相当的前提下，改善了视觉-惯性紧耦合组合导航系统的实时性。二、由于在时间更新阶段没有采用容积卡尔曼滤波的方法，而是根据视觉-惯性紧耦合组合导航系统的状态方程为线性的特点，利用线性KF的方法完成更新，使计算量下降，从而改善了视觉-惯性紧耦合组合导航系统的实时性。三、由于在时间更新阶段采用的线性KF状态更新过程是最优估计，而标准的容积卡尔曼滤波状态更新过程是次优的，同时，在关键帧的量测更新过程中采用模糊推理系统对量测噪声的统计特性进行自适应的调整，使组合导航过程中采用的量测噪声统计特性更接近真实值，提高了视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。本专利技术的效果通过如下实验得到验证：利用德国卡尔斯鲁厄理工学院和丰田技术研究所(KarlsruheInstituteofTechnologyandToyotaTechnologicalInstitute，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信号的短暂缺失导致的，并不是真实的位置误差信息，平稳阶段的位置误差信息是真实的误差信息。为了排除不同计算机对滤波时间的本文档来自技高网...

【技术保护点】
1.一种基于模糊自适应ICKF的视觉‑惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉‑惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉‑惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉‑惯性紧耦合组合导航系统的量测值，建立视觉‑惯性紧耦合组合导航系统的非线性量测方程；步骤S3：采用线性KF算法对视觉‑惯性紧耦合组合导航系统进行时间更新；步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；步骤S4.1：若为关键帧，则采用ICKF算法对视觉‑惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉‑惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；步骤S4.2：若为非关键帧，则采用基于LM算法的迭代策略对视觉‑惯性紧耦合组合导航系统进行量测更新；步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉‑惯性紧耦合组合导航系统的导航结果。...

【技术特征摘要】
1.一种基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；步骤S3：采用线性KF算法对视觉-惯性紧耦合组合导航系统进行时间更新；步骤S4：判断双目视觉里程计采集到的每帧图像是否为关键帧；步骤S4.1：若为关键帧，则采用ICKF算法对视觉-惯性紧耦合组合导航系统进行量测更新，并采用模糊自适应对视觉-惯性紧耦合组合导航系统的量测噪声的统计特性进行更新；步骤S4.2：若为非关键帧，则采用基于LM算法的迭代策略对视觉-惯性紧耦合组合导航系统进行量测更新；步骤S5：根据量测更新结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。2.根据权利要求1所述的基于模糊自适应ICKF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S3包括如下步骤：步骤S3.1：设定组合导航滤波器状态量均值的初值和组合导航滤波器状态量方差的初值Pk-1；步骤S3.2：计算出时间更新后的组合导航滤波器状态量均值和时间更新后的组合导航滤波器状态量方差Pk/k-1；计算公式如下：式中：Φk/k-1表示视觉-惯性紧耦合组合导航系统的状态转移矩阵的离散矩阵；Qk表示视觉-惯性紧耦合组合导航系统的过程噪声的协方差矩阵；所述步骤S4.1包括如下步骤：步骤S4.1.1：设定ICKF算法的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：步骤S4.1.2：计算出ICKF算法第i次迭代得到的容积点第i次迭代得到的容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值第i次迭代得到的量测预测值第i次迭代得到的新息方差第i次迭代得到的协方差第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值第i次迭代得到的组合导航滤波器状态量方差计算公式如下：式中：ξj表示容积点计算参数；n表示视觉-惯性紧耦合组合导航系统的状态维数；f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；wj表示容积点权值；Rk表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；zk表示视觉-惯性紧耦合组合导航系统的量测值；步骤S4.1.3：判断是否达到ICKF算法的迭代终止条件；若未达到ICKF算法的迭代终止条件，则返回步骤S4.1.2，由此继续进行ICKF算法；若达到ICKF算法的迭代终止条件，则ICKF算法迭代终止，由此设定ICKF算法的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值Pk；设定公式如下：式中：表示ICKF算法第N次迭代得到的组合导航滤波器状态量均值；表示ICKF算法第N次迭代得到的组合导航滤波器状态量方差；N表示ICKF算法迭代终止时的迭代次数；ICKF算法的迭代终止条件如下：或者i＝Nmax；式中：ε表示ICKF算法迭代终止误差阈值；Nmax表示ICKF算法的最大迭代次数；步骤S4.1.4：对视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵Rk进行更新；更新公式如下：Rk+1＝εkRk；式中：εk表示调节因子，其值由模糊推理系统进行自适应调整；所述步骤S4.2包括如下步骤：步骤S4.2.1：设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：式中：I表示单位矩阵；μ表示可调参数；步骤S4.2.2：计算出基于LM算法的迭代策略第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值计算公式如下：式中：f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；Rk表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；zk表示视觉-惯性紧耦合组合导航系统的量测值；步骤S4.2.3：判断是否达到基于LM算法的迭代策略的迭代终止条件；若未达到基于LM算法的迭代策略的迭代终止条件，则返回步骤S4.2.2，由此继续采用基于LM算法的迭代策略进行迭代；若达到基于LM算法的迭代策略的迭代终止条件，则基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值Pk；设定公式如下：式中：表示基于LM算法的迭代策略第N次迭代得到的组合导航滤波器状态量均值；N表示基于LM算法的迭代策略迭代终止时的迭代次数；所述基于LM算法的迭代策略的迭代终止条件如下：或者i＝Nmax；式中：ε表示基于L...

【专利技术属性】
技术研发人员：李秀源，高文学，张加书，
申请(专利权)人：中北大学，
类型：发明
国别省市：山西,14