基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法技术

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

Visual-Inertial Tightly Coupled Integrated Navigation Method Based on PF Optimized by Firefly Colony

The present invention relates to a visual-inertial tightly coupled integrated navigation method, in particular to a visual-inertial tightly coupled integrated navigation method based on firefly swarm optimization PF. The invention improves the real-time performance and navigation accuracy of the visual-inertial tightly coupled integrated navigation method. The method of visual-inertial tightly coupled integrated navigation based on firefly swarm optimization PF 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; and utilizing PF based on firefly swarm optimization for visual-inertial tightly coupled integrated navigation system Nonlinear filtering is used to realize data fusion of the visual-inertial tightly coupled integrated navigation system. Firstly, the results of strapdown inertial navigation system are corrected according to the results of nonlinear filtering. The present invention is applicable to the visual and inertial tightly coupled integrated navigation.

【技术实现步骤摘要】
基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法
本专利技术涉及视觉-惯性紧耦合组合导航方法，具体是一种基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法。
技术介绍
近年来，基于各种原理的单独导航系统不断发展，其性能日趋完善。但是，任何一种单独的导航子设备或者子系统不可能完全满足日益提高的导航要求，因此可以实现优势互补的组合导航技术的应用正在不断扩展，并受到了越来越广泛的重视。捷联惯导系统具有低成本、小体积、全自主、隐蔽性好、采样频率高等优势，但是，其误差随时间发散。基于视觉传感器的视觉里程计是目前新兴的一种导航设备，具有价格低、耗能少、信息量丰富等优势，因此视觉里程计迅速得到了广泛的关注与应用。由于将捷联惯导系统与基于视觉传感器的视觉里程计进行组合可以实现优势互补，有效克服捷联惯导系统的长时发散，从而提高整个导航系统的精度，因此，基于捷联惯导系统/视觉里程计的组合导航技术是组合导航领域的一个十分重要的发展方向。捷联惯导系统/视觉里程计的组合导航分为松耦合和紧耦合，松耦合是捷联惯导系统和视觉里程计在各自完成运动解算的基础上进行耦合的方式，组合导航滤波器计算量较小，但是没有利用视觉里程计的原始图像信息；紧耦合利用视觉里程计图像匹配的原始坐标信息和捷联惯导系统进行耦合，能够取得比松耦合更高精度的组合导航结果。本专利技术属于视觉-惯性紧耦合组合导航。由于视觉-惯性紧耦合组合导航的观测方程为强非线性，因此，高效地实现高精度的非线性组合滤波成为视觉-惯性紧耦合组合导航中的关键技术之一。粒子滤波(PF)可以通过迭代容积卡尔曼滤波对重要密度函数进行优化，从而取得较高精度的紧耦合组合滤波结果，但是，由于迭代容积卡尔曼滤波在时间更新和量测更新阶段都进行容积点的计算以及加权求和，计算量比较大，实时性较差。同时，粒子滤波存在重采样导致的粒子贫化以及需要大量粒子才能进行状态估计的问题。针对这些不足，本专利技术根据视觉-惯性紧耦合组合导航的状态方程为线性而观测方程为强非线性的特点提出了一套完整的技术方案实现视觉-惯性紧耦合组合导航。在迭代容积卡尔曼滤波的时间更新阶段，不采用容积卡尔曼滤波的方法而是根据线性KF的方法更新状态量的均值和方差，避免了容积点计算以及加权求和过程，使计算量下降；在迭代容积卡尔曼滤波的量测更新阶段，依据Levenberg-Marquard法实现量测迭代更新，从而得到优化的重要密度函数，最后采用能够在精度相当的前提下减少所需粒子数目的萤火虫群优化PF实现紧耦合组合导航，在一定程度上克服了PF的粒子贫化和计算量较大的问题。本专利技术能够较好地解决视觉-惯性紧耦合组合导航系统的非线性滤波问题，一方面改善了视觉-惯性紧耦合组合导航的实时性，同时能够一定程度地提高视觉-惯性紧耦合组合导航系统的导航精度。
技术实现思路
本专利技术针对现有技术的不足，提供了一种基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法，改善了视觉-惯性紧耦合组合导航的实时性和导航精度。本专利技术是采用如下技术方案实现的：基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；步骤S3：利用基于萤火虫群优化的PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。本专利技术的有效效果：一、由于在迭代容积卡尔曼滤波的时间更新阶段没有采用容积卡尔曼滤波的方法，而是根据视觉-惯性紧耦合组合导航系统的状态方程为线性的特点，利用线性KF的方法完成更新，使计算量下降，同时，在粒子滤波中，采用萤火虫算法对粒子进行迭代优化，从而在保证滤波精度相当的前提下，需要的粒子数目减少，使计算量进一步下降，改善了视觉-惯性紧耦合组合导航系统的实时性。二、在迭代容积卡尔曼滤波的时间更新阶段采用的线性KF状态更新过程是最优估计，而标准的容积卡尔曼滤波状态更新过程是次优的，因此，本专利技术能够获得精度更高分布更合理的重要密度函数，从而有效地提高视觉-惯性紧耦合组合导航系统的导航精度。本专利技术的效果通过如下实验得到验证：利用德国卡尔斯鲁厄理工学院和丰田技术研究所(KarlsruheInstituteofTechnologyandToyotaTechnologicalInstitute，KITTI)的开源数据对本专利技术的性能进行验证。KITTI研究所的车载实验是以一辆名为Annieway的自主车为平台，搭载有多种传感器来采集周围环境中的数据。实验中陀螺常值漂移为36°/h，加速度计常值漂移为1.0204×10-3g，捷联惯导系统的数据采集频率为100Hz，双目视觉里程计的分辨率为1226×370像素，基线距离为54cm，高度为1.65m，图像采集频率为10Hz。图2为本次实验中运载体的真实运动轨迹。本实验采用传统方法和本专利技术对实验中的数据进行处理，分别实现视觉-惯性紧耦合组合导航系统的导航定位，实验结果如图3～图4。本实验中传统方法和本专利技术提的粒子数目分别为2500和600。从图3～图4可以看出，与传统方法相比，本专利技术所取得的运动轨迹更接近运载体的真实运动轨迹，本专利技术的定位误差最大值约为15.52m，而传统方法的定位误差最大值约为18.29m。图3～图4中的位置误差尖峰是由于提供真实轨迹信息的GPS信号的短暂缺失导致的，并不是真实的位置误差信息，平稳阶段的位置误差信息是真实的紧耦合组合导航位置误差信息。为了排除不同计算机对滤波时间的影响，本实验以传统方法的相对平均单步滤波时间为参照对相对平均单步滤波时间进行了归一化。从图5可以看出，与传统方法相比，本专利技术的相对平均单步滤波时间降为传统方法的31.22％。综上所述，本专利技术有效改善了导航实时性。本专利技术改善了视觉-惯性紧耦合组合导航方法的实时性和导航精度，适用于视觉-惯性紧耦合组合导航。附图说明图1是本专利技术中步骤S3～S4的流程示意图。图2是实验中运载体的真实运动轨迹。图3是实验中本专利技术和传统方法的X方向位置误差对比示意图。图4是实验中本专利技术和传统方法的Y方向位置误差对比示意图。图5是实验中本专利技术和传统方法的相对平均单步滤波时间对比示意图。具体实施方式基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像本文档来自技高网...

【技术保护点】
1.一种基于萤火虫群优化PF的视觉‑惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉‑惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉‑惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉‑惯性紧耦合组合导航系统的量测值，建立视觉‑惯性紧耦合组合导航系统的非线性量测方程；步骤S3：利用基于萤火虫群优化的PF对视觉‑惯性紧耦合组合导航系统进行非线性滤波，实现视觉‑惯性紧耦合组合导航系统的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉‑惯性紧耦合组合导航系统的导航结果。

【技术特征摘要】
1.一种基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；步骤S3：利用基于萤火虫群优化的PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。2.根据权利要求1所述的一种基于萤火虫群优化PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S3包括如下步骤：步骤S3.1：从先验分布p(x0)中抽取M个初始化状态粒子并设定第j个粒子的权值的初值wj,0；设定公式如下：wj,0＝1/M；步骤S3.2：对每个粒子j进行迭代容积卡尔曼滤波，从而获得优化的重要密度函数：步骤S3.2.1：对第j个粒子，计算出时间更新后的组合导航滤波器状态量均值和时间更新后的组合导航滤波器状态量方差Pj,k/k-1；计算公式如下：式中：Φk/k-1表示视觉-惯性紧耦合组合导航系统的状态转移矩阵的离散矩阵；Qk表示视觉-惯性紧耦合组合导航系统的过程噪声的协方差矩阵；步骤S3.2.2：对第j个粒子，设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：式中：I表示单位矩阵；μ表示可调参数；步骤S3.2.3：对第j个粒子，计算出基于LM算法的迭代策略第i次迭代时滤波增益第i次迭代时组合导航滤波器状态量均值计算公式如下：式中：f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；Rk表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；zk表示视觉-惯性紧耦合组合导航系统的量测值；步骤S3.2.4：对第j个粒子，当基于LM算法的迭代策略的迭代次数达到N时，基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值从而得到优化的正态分布设定公式如下：式中：表示基于LM算法的迭代策略第N次迭代时组合导航滤波器状态量均值；步骤S3.3：对每个粒子j，利用优化的正态分布进行重要性采样：式中：Aj,k表示第j个粒子的位置；步骤S3.4：计算出每个粒子j的权值计算公式如下：式中：p(zk|Aj,k)表示似然函数；p(Aj,k|Aj,k-1)表示先验概率密度；q(Aj,k|Aj,k-1,zk)表示重要性密度函数；步骤S3.5：将各个粒子视作萤火虫群优化算法中的萤火虫，将各个粒子的权值视作萤火虫群优化算法中的萤火虫亮度；通过对萤火虫亮度进行排序，得到萤火虫亮度的最优值，由此得到全局最优值粒子；然后，计算出萤火虫群优化算法第κ次迭代时每个粒子j和全局最优值粒子之间的吸引度βj，k(κ)、第κ次迭代时第j个粒子和全局最优值粒子之间的笛卡尔距离rj，k(κ)；计算公式如下：式中：β0表示最大吸引度，其值取常数1；γ表示空气对光的吸收率，其值取γ∈[0.110]；τ(κ)表示萤火虫群优化算法第κ次迭代时的衰减因子；ψ表示设定的常数；Aj，k(κ)表示萤火虫群优化算法第κ次迭代时第j个粒子的位置；Amax，k(κ)表示萤火虫群优化算法第κ次迭代时全局最优值粒子的位置；Aj，k,(p)(κ)表示萤火虫群优化算法第κ次迭代时第j个粒子的空间坐标的第p个分量；Amax，k,(p)(κ)表示萤火虫群优化算法第κ次迭代时全局最优值粒子的空间坐标的第p个分量；d表示视觉-惯性紧耦合组合导航系统的状态维数；步骤S3.6：更新每个粒子j的位置，更新公式如下：Aj，k(κ+1)＝e-u(κ)/u(κ-1)Aj，k(κ)+βj，k(κ)(Aj，k(κ)-Amax，k(κ))+α(rand-0.5)；式中：α表示随机步长，且α∈[0,1]；rand表示0至1的随机数；w(Aj，k(κ))表示萤火虫群优化算法第κ次迭代时第j个粒子的权值；w(Amax，k(κ))表示萤火虫群优化算法第κ次迭代时全局最优值粒子的权值；步骤S3.7：判断是否达到萤火虫群优化算法的迭代终止条件：当萤火虫群优化算法第κ次迭代时全局最优值粒子的权值w(Amax，k(κ))达到设定阈值或萤火虫...

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