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

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

Visual-Inertial Tightly Coupled Integrated Navigation Method Based on Adaptive Gauss PF

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 adaptive Gauss PF. The invention improves the real-time performance and navigation accuracy of the visual-inertial tightly coupled integrated navigation method. Based on the adaptive Gauss PF visual-inertial tightly coupled integrated navigation method, this method is implemented 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; using adaptive Gauss PF visual-inertial tightly coupled integrated navigation system for non-linear. Sexual filtering is used to realize data fusion of visual-inertial tightly coupled integrated navigation system. 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的方法完成更新，使计算量下降，另外，在高斯PF中，采用线性变换替代传统高斯PF中的高斯采样生成相同分布的粒子，计算量进一步下降，改善了视觉-惯性紧耦合组合导航系统的实时性。二、由于在自适应平方根容积卡尔曼滤波的时间更新阶段采用的线性KF状态更新过程是最优估计，而标准的平方根容积卡尔曼滤波状态更新过程是次优的，同时，在自适应平方根容积卡尔曼滤波中采用模糊推理系统对量测噪声的统计特性进行自适应的调整，使组合导航过程中采用的量测噪声统计特性更接近真实值，提高了视觉-惯性紧耦合组合导航系统在环境和运动状态变化时的导航精度。本专利技术的效果通过如下实验得到验证：利用德国卡尔斯鲁厄理工学院和丰田技术研究所(KarlsruheInstituteofTechnologyandToyotaTechnologicalInstitute，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：在运载体上安装捷联惯导系统和双目视觉里本文档来自技高网...

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

【技术特征摘要】
1.一种基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导系统和双目视觉里程计，捷联惯导系统和双目视觉里程计共同组成视觉-惯性紧耦合组合导航系统；捷联惯导系统根据自身采集到的数据解算出运载体的名义运动信息；采用SURF算法对双目视觉里程计采集到的图像序列进行特征匹配，并解算出连续两帧图像匹配点的像素坐标信息；步骤S2：根据捷联惯导系统的误差特性，建立视觉-惯性紧耦合组合导航系统的线性状态方程；以连续两帧图像匹配点的像素坐标信息为视觉-惯性紧耦合组合导航系统的量测值，建立视觉-惯性紧耦合组合导航系统的非线性量测方程；步骤S3：采用自适应高斯PF对视觉-惯性紧耦合组合导航系统进行非线性滤波，实现视觉-惯性紧耦合组合导航系统的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果。2.根据权利要求1所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S3包括如下步骤：步骤S3.1：设定组合导航滤波器状态量均值的初值和组合导航滤波器状态量方差的初值Pk-1；步骤S3.2：计算出时间更新后的组合导航滤波器状态量均值和时间更新后的组合导航滤波器状态量方差Pk/k-1；计算公式如下：式中：Φk/k-1表示视觉-惯性紧耦合组合导航系统的状态转移矩阵的离散矩阵；Qk表示视觉-惯性紧耦合组合导航系统的过程噪声的协方差矩阵；步骤S3.3：计算出时间更新后的组合导航滤波器状态量方差Pk/k-1的乔列斯基分解因子Sk/k-1；计算公式如下：Sk/k-1＝Chol(Pk/k-1)；步骤S3.4：计算出平方根容积卡尔曼滤波算法的容积点容积点经视觉-惯性紧耦合组合导航系统的非线性量测方程的传递值量测预测值新息方差矩阵的平方根协方差的平方根滤波增益组合导航滤波器状态量均值组合导航滤波器状态量方差Pk；计算公式如下：式中：ξj表示容积点计算参数；n表示视觉-惯性紧耦合组合导航系统的状态维数；f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；wj表示容积点权值；Tria表示对矩阵进行QR分解运算；Rk表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；zk表示视觉-惯性紧耦合组合导航系统的量测值；步骤S3.5：设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代初值和组合导航滤波器状态量方差迭代初值设定公式如下：式中：I表示单位矩阵；μ表示可调参数；步骤S3.6：计算出基于LM算法的迭代策略第i次迭代得到的滤波增益第i次迭代得到的组合导航滤波器状态量均值计算公式如下：式中：f(·)表示由视觉-惯性紧耦合组合导航系统的非线性量测方程产生的非线性映射关系；Rk表示视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵；步骤S3.7：当基于LM算法的迭代策略的迭代次数达到N时，基于LM算法的迭代策略迭代终止，由此设定基于LM算法的迭代策略的组合导航滤波器状态量均值迭代终值和组合导航滤波器状态量方差迭代终值从而得到优化的正态分布设定公式如下：式中：表示基于LM算法的迭代策略第N次迭代得到的组合导航滤波器状态量均值；步骤S3.8：建立高斯PF算法的初始粒子群其中每个粒子均从N(0,1)独立采样得到，通过对每个粒子进行线性变换得到粒子集式中：U表示高斯PF算法的粒子数目；步骤S3.9：采用高斯PF算法计算出组合导航滤波器状态量均值μk和组合导航滤波器状态量方差δk；计算公式如下：式中：wl,k表示归一化后的第l个粒子的权值；表示第l个粒子的权值；表示似然函数；表示先验概率密度；表示重要性密度函数；步骤S3.10：对视觉-惯性紧耦合组合导航系统的量测噪声的协方差矩阵Rk进行更新；更新公式如下：Rk+1＝εkRk；式中：εk表示调节因子，其值由模糊推理系统进行自适应调整；所述步骤S4包括如下步骤：步骤S4.1：从计算出的组合导航滤波器状态量均值μk中抽取得到捷联惯导系统的误差估计步骤S4.2：根据捷联惯导系统的误差估计对捷联惯导系统的解算结果进行校正，由此得到视觉-惯性紧耦合组合导航系统的导航结果Tk；校正公式如下：式中：表示捷联惯导系统的角度误差；δVE、δVN、δVU表示捷联惯导系统的速度误差；δRE、δRN、δRU表示捷联惯导系统...

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