CN109443355A - 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 - Google Patents
基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 Download PDFInfo
- Publication number
- CN109443355A CN109443355A CN201811590667.6A CN201811590667A CN109443355A CN 109443355 A CN109443355 A CN 109443355A CN 201811590667 A CN201811590667 A CN 201811590667A CN 109443355 A CN109443355 A CN 109443355A
- Authority
- CN
- China
- Prior art keywords
- navigation system
- vision
- integrated navigation
- close coupling
- indicate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000008878 coupling Effects 0.000 title claims abstract description 97
- 238000010168 coupling process Methods 0.000 title claims abstract description 97
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 97
- 238000000034 method Methods 0.000 title claims abstract description 36
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 25
- 238000001914 filtration Methods 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 238000005259 measurement Methods 0.000 claims abstract description 26
- 230000004927 fusion Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 45
- 239000002245 particle Substances 0.000 claims description 17
- 238000006073 displacement reaction Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 230000001105 regulatory effect Effects 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 2
- 238000000605 extraction Methods 0.000 claims description 2
- 238000010606 normalization Methods 0.000 claims description 2
- 238000005070 sampling Methods 0.000 claims description 2
- 230000026676 system process Effects 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 238000002474 experimental method Methods 0.000 description 10
- 230000000007 visual effect Effects 0.000 description 10
- 238000007796 conventional method Methods 0.000 description 7
- 230000008901 benefit Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000000295 complement effect Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000007812 deficiency Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明涉及视觉‑惯性紧耦合组合导航方法，具体是一种基于自适应高斯PF的视觉‑惯性紧耦合组合导航方法。本发明改善了视觉‑惯性紧耦合组合导航方法的实时性和导航精度。基于自适应高斯PF的视觉‑惯性紧耦合组合导航方法，该方法是采用如下步骤实现的：步骤S1：在运载体上安装捷联惯导***和双目视觉里程计；步骤S2：建立线性状态方程；建立非线性量测方程；步骤S3：采用自适应高斯PF对视觉‑惯性紧耦合组合导航***进行非线性滤波，实现视觉‑惯性紧耦合组合导航***的数据融合；步骤S4：根据步骤S3的非线性滤波结果对捷联惯导***的解算结果进行校正。本发明适用于视觉‑惯性紧耦合组合导航。
Description
技术领域
本发明涉及视觉-惯性紧耦合组合导航方法，具体是一种基于自适应高斯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：设定组合导航滤波器状态量均值的初值和组合导航滤波器状态量方差的初值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表示捷联惯导***的位置误差。
所述步骤S3.10中，模糊推理***的输入为量测噪声失配比mk，模糊推理***的输出为调节因子εk；
所述量测噪声失配比mk定义为：
式中：表示新息方差矩阵的迹；表示理论新息方差矩阵的迹；M表示新息方差矩阵的累加窗口；
模糊推理***以1位参考，设S表示模糊集合小，E表示基本等于1，F表示模糊集合大，建立如下模糊推理规则：
A.如果mk∈S，那么εk∈S；
B.如果mk∈E，那么εk∈E；
C.如果mk∈F，那么εk∈F。
所述步骤S2中，视觉-惯性紧耦合组合导航***的线性状态方程的建立过程如下：
首先，将组合导航滤波器的状态量X定义为：
式中：表示捷联惯导***的角度误差；δVE、δVN、δVU表示捷联惯导***的速度误差；δRE、δRN、δRU表示捷联惯导***的位置误差；εbx、εby、εbz表示沿载体坐标系三个方向的陀螺仪随机漂移误差；ζx、ζy、ζz表示加速度计的随机漂移误差；δpx、δpy、δpz表示上张图像采集时的名义位置误差；δθ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)表示组合导航滤波器的噪声向量；FN(t)表示经典的惯性***误差9×9矩阵；FS(t)表示导航参数与惯性元件误差之间的变换矩阵；表示姿态矩阵；wεx、wεy、wεz表示陀螺仪的噪声项；wζx、wζy、wζz表示加速度计的噪声项。
所述步骤S2中，视觉-惯性紧耦合组合导航***的非线性量测方程的建立过程如下：
首先，根据组合导航滤波器的状态量X和捷联惯导***的名义状态Xn，计算出右摄像机后一时刻相对前一时刻的旋转RR、左摄像机后一时刻相对前一时刻的旋转RL、右摄像机后一时刻相对前一时刻的位移tR、左摄像机后一时刻相对前一时刻的位移tL，计算公式表示如下：
式中：表示由组合导航滤波器的状态量对捷联惯导***的名义状态得到捷联惯导的真实状态信息；R和t分别表示由捷联惯导***的真实状态信息得到旋转量和位移量的函数关系；
然后，令前一时刻右摄像机的坐标系与世界坐标系对准，即令前一时刻右摄像机的投影矩阵PR,k为：
PR,k＝KR[I|0]；
式中：KR表示右摄像机的标定矩阵；
通过标定获取前一时刻左摄像机的相对位姿[RC|tC]，由此将前一时刻左摄像机的投影矩阵PL,k表示为：
PL,k＝KL[RC|tC]；
式中：KL表示左摄像机的标定矩阵；RC表示左摄像机相对右摄像机的旋转；tC表示左摄像机相对右摄像机的位移；
后一时刻右摄像机的投影矩阵PR,k+1表示为：
PR,k+1＝KR[RR|tR]；
后一时刻左摄像机的投影矩阵PL,k+1表示为：
PL,k+1＝KL[RL|tL]；
然后，计算出右摄像机采集到的连续两帧图像的三焦点张量TR和左摄像机采集到的连续两帧图像的三焦点张量TL；计算公式如下：
TR＝T(KR,KL,RC,tC,RR,tR)；
TL＝T(KR,KL,RC,tC,RL,tL)；
式中：T表示三焦点张量的计算公式；
然后，根据三焦点张量的非线性转移函数H，得到视觉-惯性紧耦合组合导航***的非线性量测方程：
xR,k+1＝H(TR,xR,k,xL,k)；
xL,k+1＝H(TL,xR,k,xL,k)；
式中：xR,k+1表示后一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；xL,k+1表示后一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；xR,k表示前一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；xL,k表示前一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；
然后，令xk+1＝[xR,k+1,xL,k+1]T，则视觉-惯性紧耦合组合导航***的非线性量测方程表示为：
根据以上视觉-惯性紧耦合组合导航***的非线性量测方程产生由组合导航滤波器状态量X到像素点坐标信息的非线性映射关系：
xk+1＝f(X)。
Claims (5)
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表示捷联惯导***的位置误差。
3.根据权利要求2所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S3.10中，模糊推理***的输入为量测噪声失配比mk，模糊推理***的输出为调节因子εk；
所述量测噪声失配比mk定义为：
式中：表示新息方差矩阵的迹；表示理论新息方差矩阵的迹；M表示新息方差矩阵的累加窗口；
模糊推理***以1位参考，设S表示模糊集合小，E表示基本等于1，F表示模糊集合大，建立如下模糊推理规则：
A.如果mk∈S，那么εk∈S；
B.如果mk∈E，那么εk∈E；
C.如果mk∈F，那么εk∈F。
4.根据权利要求1或2或3所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S2中，视觉-惯性紧耦合组合导航***的线性状态方程的建立过程如下：
首先，将组合导航滤波器的状态量X定义为：
式中：表示捷联惯导***的角度误差；δVE、δVN、δVU表示捷联惯导***的速度误差；δRE、δRN、δRU表示捷联惯导***的位置误差；εbx、εby、εbz表示沿载体坐标系三个方向的陀螺仪随机漂移误差；ζx、ζy、ζz表示加速度计的随机漂移误差；δpx、δpy、δpz表示上张图像采集时的名义位置误差；δθ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)表示组合导航滤波器的噪声向量；FN(t)表示经典的惯性***误差9×9矩阵；FS(t)表示导航参数与惯性元件误差之间的变换矩阵；表示姿态矩阵；wεx、wεy、wεz表示陀螺仪的噪声项；wζx、wζy、wζz表示加速度计的噪声项。
5.根据权利要求1或2或3所述的基于自适应高斯PF的视觉-惯性紧耦合组合导航方法，其特征在于：所述步骤S2中，视觉-惯性紧耦合组合导航***的非线性量测方程的建立过程如下：
首先，根据组合导航滤波器的状态量X和捷联惯导***的名义状态Xn，计算出右摄像机后一时刻相对前一时刻的旋转RR、左摄像机后一时刻相对前一时刻的旋转RL、右摄像机后一时刻相对前一时刻的位移tR、左摄像机后一时刻相对前一时刻的位移tL，计算公式表示如下：
式中：表示由组合导航滤波器的状态量对捷联惯导***的名义状态得到捷联惯导的真实状态信息；R和t分别表示由捷联惯导***的真实状态信息得到旋转量和位移量的函数关系；
然后，令前一时刻右摄像机的坐标系与世界坐标系对准，即令前一时刻右摄像机的投影矩阵PR,k为：
PR,k＝KR[I|0]；
式中：KR表示右摄像机的标定矩阵；
通过标定获取前一时刻左摄像机的相对位姿[RC|tC]，由此将前一时刻左摄像机的投影矩阵PL,k表示为：
PL,k＝KL[RC|tC]；
式中：KL表示左摄像机的标定矩阵；RC表示左摄像机相对右摄像机的旋转；tC表示左摄像机相对右摄像机的位移；
后一时刻右摄像机的投影矩阵PR,k+1表示为：
PR,k+1＝KR[RR|tR]；
后一时刻左摄像机的投影矩阵PL,k+1表示为：
PL,k+1＝KL[RL|tL]；
然后，计算出右摄像机采集到的连续两帧图像的三焦点张量TR和左摄像机采集到的连续两帧图像的三焦点张量TL；计算公式如下：
TR＝T(KR,KL,RC,tC,RR,tR)；
TL＝T(KR,KL,RC,tC,RL,tL)；
式中：T表示三焦点张量的计算公式；
然后，根据三焦点张量的非线性转移函数H，得到视觉-惯性紧耦合组合导航***的非线性量测方程：
xR,k+1＝H(TR,xR,k,xL,k)；
xL,k+1＝H(TL,xR,k,xL,k)；
式中：xR,k+1表示后一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；xL,k+1表示后一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；xR,k表示前一时刻右摄像机采集到的连续两帧图像匹配点的像素坐标；xL,k表示前一时刻左摄像机采集到的连续两帧图像匹配点的像素坐标；
然后，令xk+1＝[xR,k+1,xL,k+1]T，则视觉-惯性紧耦合组合导航***的非线性量测方程表示为：
根据以上视觉-惯性紧耦合组合导航***的非线性量测方程产生由组合导航滤波器状态量X到像素点坐标信息的非线性映射关系：
xk+1＝f(X)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811590667.6A CN109443355B (zh) | 2018-12-25 | 2018-12-25 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811590667.6A CN109443355B (zh) | 2018-12-25 | 2018-12-25 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109443355A true CN109443355A (zh) | 2019-03-08 |
| CN109443355B CN109443355B (zh) | 2020-10-27 |
Family
ID=65537866
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811590667.6A Expired - Fee Related CN109443355B (zh) | 2018-12-25 | 2018-12-25 | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109443355B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
| CN112269201A (zh) * | 2020-10-23 | 2021-01-26 | 北京云恒科技研究院有限公司 | 一种gnss/ins紧耦合时间分散滤波方法 |
| WO2022100189A1 (zh) * | 2020-11-16 | 2022-05-19 | 浙江商汤科技开发有限公司 | 视觉惯性***的参数标定方法及装置、电子设备和介质 |
Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101825467A (zh) * | 2010-04-20 | 2010-09-08 | 南京航空航天大学 | 捷联惯性导航***与天文导航***实现组合导航的方法 |
| CN103033189A (zh) * | 2012-12-26 | 2013-04-10 | 北京航空航天大学 | 一种深空探测巡视器惯性/视觉组合导航方法 |
| CN103744100A (zh) * | 2014-01-07 | 2014-04-23 | 北京航空航天大学 | 一种基于卫星导航与惯性导航的组合导航方法 |
| CN103759742A (zh) * | 2014-01-22 | 2014-04-30 | 东南大学 | 基于模糊自适应控制技术的捷联惯导非线性对准方法 |
| CN103983263A (zh) * | 2014-05-30 | 2014-08-13 | 东南大学 | 一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法 |
| CN104376581A (zh) * | 2014-12-02 | 2015-02-25 | 北京航空航天大学 | 一种采用自适应重采样的高斯混合无迹粒子滤波算法 |
| US9026263B2 (en) * | 2011-11-30 | 2015-05-05 | Alpine Electronics, Inc. | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis |
| WO2016073642A1 (en) * | 2014-11-04 | 2016-05-12 | The Regents Of The University Of California | Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction |
| CN105737823A (zh) * | 2016-02-01 | 2016-07-06 | 东南大学 | 基于五阶ckf的gps/sins/cns组合导航方法 |
| CN106767791A (zh) * | 2017-01-13 | 2017-05-31 | 东南大学 | 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法 |
| CN107504969A (zh) * | 2017-07-24 | 2017-12-22 | 哈尔滨理工大学 | 基于视觉和惯性组合的四旋翼室内导航方法 |
| CN107796391A (zh) * | 2017-10-27 | 2018-03-13 | 哈尔滨工程大学 | 一种捷联惯性导航***/视觉里程计组合导航方法 |
| CN108253964A (zh) * | 2017-12-29 | 2018-07-06 | 齐鲁工业大学 | 一种基于时延滤波器的视觉/惯性组合导航模型构建方法 |
| CN108490472A (zh) * | 2018-01-29 | 2018-09-04 | 哈尔滨工程大学 | 一种基于模糊自适应滤波的无人艇组合导航方法 |
| CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
| CN108981687A (zh) * | 2018-05-07 | 2018-12-11 | 清华大学 | 一种视觉与惯性融合的室内定位方法 |
-
2018
- 2018-12-25 CN CN201811590667.6A patent/CN109443355B/zh not_active Expired - Fee Related
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101825467A (zh) * | 2010-04-20 | 2010-09-08 | 南京航空航天大学 | 捷联惯性导航***与天文导航***实现组合导航的方法 |
| US9026263B2 (en) * | 2011-11-30 | 2015-05-05 | Alpine Electronics, Inc. | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis |
| CN103033189A (zh) * | 2012-12-26 | 2013-04-10 | 北京航空航天大学 | 一种深空探测巡视器惯性/视觉组合导航方法 |
| CN103744100A (zh) * | 2014-01-07 | 2014-04-23 | 北京航空航天大学 | 一种基于卫星导航与惯性导航的组合导航方法 |
| CN103759742A (zh) * | 2014-01-22 | 2014-04-30 | 东南大学 | 基于模糊自适应控制技术的捷联惯导非线性对准方法 |
| CN103983263A (zh) * | 2014-05-30 | 2014-08-13 | 东南大学 | 一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法 |
| WO2016073642A1 (en) * | 2014-11-04 | 2016-05-12 | The Regents Of The University Of California | Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction |
| CN104376581A (zh) * | 2014-12-02 | 2015-02-25 | 北京航空航天大学 | 一种采用自适应重采样的高斯混合无迹粒子滤波算法 |
| CN105737823A (zh) * | 2016-02-01 | 2016-07-06 | 东南大学 | 基于五阶ckf的gps/sins/cns组合导航方法 |
| CN106767791A (zh) * | 2017-01-13 | 2017-05-31 | 东南大学 | 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法 |
| CN107504969A (zh) * | 2017-07-24 | 2017-12-22 | 哈尔滨理工大学 | 基于视觉和惯性组合的四旋翼室内导航方法 |
| CN107796391A (zh) * | 2017-10-27 | 2018-03-13 | 哈尔滨工程大学 | 一种捷联惯性导航***/视觉里程计组合导航方法 |
| CN108253964A (zh) * | 2017-12-29 | 2018-07-06 | 齐鲁工业大学 | 一种基于时延滤波器的视觉/惯性组合导航模型构建方法 |
| CN108490472A (zh) * | 2018-01-29 | 2018-09-04 | 哈尔滨工程大学 | 一种基于模糊自适应滤波的无人艇组合导航方法 |
| CN108981687A (zh) * | 2018-05-07 | 2018-12-11 | 清华大学 | 一种视觉与惯性融合的室内定位方法 |
| CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
Non-Patent Citations (3)
| Title |
|---|
| EHSAN ASADI: "Tightly-coupled stereo vision-aided inertial navigation using feature-based motion sensors", 《ADVANCED ROBOTICS》 * |
| 周翟和，等: "粒子滤波的改进优化方法及在组合导航***中的应用", 《中国惯性技术学报》 * |
| 张亚: "视觉里程计辅助的INS_GPS组合导航***研究", 《中国博士学位论文全文数据库信息科技辑》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
| CN112269201A (zh) * | 2020-10-23 | 2021-01-26 | 北京云恒科技研究院有限公司 | 一种gnss/ins紧耦合时间分散滤波方法 |
| CN112269201B (zh) * | 2020-10-23 | 2024-04-16 | 北京云恒科技研究院有限公司 | 一种gnss/ins紧耦合时间分散滤波方法 |
| WO2022100189A1 (zh) * | 2020-11-16 | 2022-05-19 | 浙江商汤科技开发有限公司 | 视觉惯性***的参数标定方法及装置、电子设备和介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109443355B (zh) | 2020-10-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Zuo et al. | Lic-fusion: Lidar-inertial-camera odometry | |
| Heo et al. | Consistent EKF-based visual-inertial odometry on matrix Lie group | |
| CN107796391A (zh) | 一种捷联惯性导航***/视觉里程计组合导航方法 | |
| Heo et al. | EKF-based visual inertial navigation using sliding window nonlinear optimization | |
| CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN110095116A (zh) | 一种基于lift的视觉定位和惯性导航组合的定位方法 | |
| CN109443354A (zh) | 基于萤火虫群优化pf的视觉-惯性紧耦合组合导航方法 | |
| CN112649016A (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
| CN111595334B (zh) | 基于视觉点线特征与imu紧耦合的室内自主定位方法 | |
| CN109443355A (zh) | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 | |
| CN109269511B (zh) | 未知环境下行星着陆的曲线匹配视觉导航方法 | |
| Heo et al. | Consistent EKF-based visual-inertial navigation using points and lines | |
| CN112815939A (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
| CN109443353A (zh) | 基于模糊自适应ickf的视觉-惯性紧耦合组合导航方法 | |
| CN109506660A (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
| CN106352897B (zh) | 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN107270898A (zh) | 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法 | |
| Bistrov | Performance analysis of alignment process of MEMS IMU | |
| CN109741372A (zh) | 一种基于双目视觉的里程计运动估计方法 | |
| CN109871024A (zh) | 一种基于轻量级视觉里程计的无人机位姿估计方法 | |
| CN116772844A (zh) | 一种基于动态环境下的视觉惯性室内机器人的导航方法 | |
| CN114018254B (zh) | 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法 | |
| CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
| Wang et al. | Application of gravity passive aided strapdown inertial navigation in underwater vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20201027 |