留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位

万文辉 李宇 胡文敏 赵强 孙逊 张秋昭 邸凯昌 郭杭 吴立新

万文辉, 李宇, 胡文敏, 赵强, 孙逊, 张秋昭, 邸凯昌, 郭杭, 吴立新. 基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J]. 武汉大学学报 ● 信息科学版, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
引用本文: 万文辉, 李宇, 胡文敏, 赵强, 孙逊, 张秋昭, 邸凯昌, 郭杭, 吴立新. 基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J]. 武汉大学学报 ● 信息科学版, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
WAN Wenhui, LI Yu, HU Wenmin, ZHAO Qiang, SUN Xun, ZHANG Qiuzhao, DI Kaichang, GUO Hang, WU Lixin. Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter[J]. Geomatics and Information Science of Wuhan University, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
Citation: WAN Wenhui, LI Yu, HU Wenmin, ZHAO Qiang, SUN Xun, ZHANG Qiuzhao, DI Kaichang, GUO Hang, WU Lixin. Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter[J]. Geomatics and Information Science of Wuhan University, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286

基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位

doi: 10.13203/j.whugis20150286
基金项目: 

国家重点基础研究发展计划 2012CB719902

国家自然科学基金 41171355

国家自然科学基金 41374039

详细信息
    作者简介:

    万文辉, 博士, 助理研究员, 主要从事深空探测器导航定位理论与方法研究。wanwh@radi.ac.cn

    通讯作者: 邸凯昌, 博士, 研究员。dikc@radi.ac.cn
  • 中图分类号: P228

Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter

Funds: 

The National Program on Key Basic Research Project 2012CB719902

the National Natural Science Foundation of China 41171355

the National Natural Science Foundation of China 41374039

More Information
    Author Bio:

    WAN Wenhui, PhD, assistant professor, specializes in localization of planetary probe. E-mail: wanwh@radi.ac.cn

    Corresponding author: DI Kaichang, PhD, professor. E-mail: dikc@radi.ac.cn
图(7) / 表(2)
计量
  • 文章访问数:  3167
  • HTML全文浏览量:  53
  • PDF下载量:  433
  • 被引次数: 0
出版历程
  • 收稿日期:  2016-04-01
  • 刊出日期:  2018-01-05

基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位

doi: 10.13203/j.whugis20150286
    基金项目:

    国家重点基础研究发展计划 2012CB719902

    国家自然科学基金 41171355

    国家自然科学基金 41374039

    作者简介:

    万文辉, 博士, 助理研究员, 主要从事深空探测器导航定位理论与方法研究。wanwh@radi.ac.cn

    通讯作者: 邸凯昌, 博士, 研究员。dikc@radi.ac.cn
  • 中图分类号: P228

摘要: 在无GPS信号的受限环境中,基于序列立体影像的运动平台视觉定位精度较高,能够改正航迹推算方法的误差累积,但在纹理贫乏或光照不足的环境下容易定位失败。为提高受限环境下运动平台定位的精度与稳健性,提出一种基于联邦滤波的立体相机、惯性测量单元(inertial measurement unit,IMU)及里程计组合导航方法。该方法在联邦滤波中利用IMU分别同里程计与立体相机构成子滤波器,有效避免立体视觉定位失效而导致的系统定位失败,提高了定位稳健性。地下巷道实验结果证明,所提方法能有效提高运动平台导航定位的精度,并且在立体视觉定位失效的情况下仍能实现连续定位。

English Abstract

万文辉, 李宇, 胡文敏, 赵强, 孙逊, 张秋昭, 邸凯昌, 郭杭, 吴立新. 基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J]. 武汉大学学报 ● 信息科学版, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
引用本文: 万文辉, 李宇, 胡文敏, 赵强, 孙逊, 张秋昭, 邸凯昌, 郭杭, 吴立新. 基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J]. 武汉大学学报 ● 信息科学版, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
WAN Wenhui, LI Yu, HU Wenmin, ZHAO Qiang, SUN Xun, ZHANG Qiuzhao, DI Kaichang, GUO Hang, WU Lixin. Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter[J]. Geomatics and Information Science of Wuhan University, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
Citation: WAN Wenhui, LI Yu, HU Wenmin, ZHAO Qiang, SUN Xun, ZHANG Qiuzhao, DI Kaichang, GUO Hang, WU Lixin. Mobile Platform Localization by Integration of Stereo Cameras, IMU and Wheel Qdometer Based on Federated Filter[J]. Geomatics and Information Science of Wuhan University, 2018, 43(1): 101-106. doi: 10.13203/j.whugis20150286
  • 在太空或地下矿井等无GPS信号的受限环境中,运动平台的导航定位技术对于探测或救援任务的实施至关重要。航迹推算方法将惯性测量单元(Inertial measurement unit,IMU)同里程计组合起来, 能够实时地提供连续的定位结果,得到了广泛的应用[1]。然而IMU漂移和地形起伏导致车轮打滑会使得定位误差迅速累积,无法满足远距离高精度定位的需要。相机作为一种功耗低、体积小、信息获取丰富的传感器,在导航定位研究中已获得了广泛的关注。文献[2]较早提出利用多相机进行视觉定位的方法。文献[3]提出基于立体相机的视觉测程定位方法。文献[4]引入光束法平差提高了视觉定位的精度。文献[5-6]应用滑动窗口光束法平差方法优化影像位姿估计结果,提高了序列影像定位精度。文献[7]提出了一种几何关键帧自动选取的视觉测程方法,并分析了不同相机配置对定位精度的影响。相关的研究成果已成功应用于国内外深空探测任务中,为执行任务的巡视器和着陆器提供了高精度的定位结果[8-10]。总的来说,基于视觉的定位方法误差与时间和地形均无关,定位精度较高,能够消除或减小航迹推算定位误差。

    尽管视觉定位方法精度较高,但在深空或地下环境中,光照及纹理条件复杂,若图像纹理质量不佳,无法进行特征点追踪,最终将导致定位失败。文献[11]采用无迹卡尔曼滤波方法,将IMU与视觉组合进行系统标定。文献[12]利用扩展卡尔曼滤波方法将视觉测程和IMU组合,修正了定位累积误差。文献[13]将扩展卡尔曼滤波用于立体相机、IMU及里程计的集成定位。这些方法多采用集中式滤波,系统中局部处理异常会影响整体滤波效果,定位稳健性与精度有待提高。联邦滤波是一种分散式滤波方法,系统由若干子滤波器组成。在部分子滤波器失效的情况下,系统仍能输出可靠结果,稳定性强。文献[14]较早将联邦滤波应用多传感器集成的导航系统分散式滤波中。文献[15]将联邦滤波用于INS/GPS的组合导航,提高了系统稳定性。文献[16]研究了基于联邦滤波的月球车自主导航方法,通过滤波融合航迹推算与天文导航的结果,提高了定位精度与稳定性。视觉定位在贫纹理环境下易出现定位失败情况,采用联邦滤波有助于提高定位稳健性。目前,针对立体相机/IMU/里程计集成定位的相关研究还有待深入,对贫纹理条件下的组合导航稳健性的分析也不多。

    本文提出一种基于联邦滤波的IMU/立体相机/里程计的组合导航方法,定位精度优于单独采用航迹推算与视觉定位方法,并能在视觉定位失败的情况下依然稳健地获取连续定位结果。通过地下巷道实验对所提方法的有效性与容错性进行了分析与验证。

    • 联邦滤波是一种两级结构分散化滤波方法,由若干子滤波器与一个主滤波器组成。滤波计算时,各子滤波器独立进行时间与量测更新,主滤波器则将各子滤波器的结果进行融合得到全局最优估计[14]。联邦滤波方法的分散化结构有利于降低系统计算量,同时系统在部分子滤波器失效时仍能输出滤波结果,提高了整个系统的容错性。

      考虑到系统中IMU能够输出稳定的测量结果,将IMU分别同里程计与立体相机构成子滤波器1与子滤波器2,再对两个子滤波器的结果进行融合,最终输出最优组合结果。滤波中,若系统中因立体相机视觉定位解算失败导致子滤波器2失效,则由子滤波器1继续进行滤波解算,提高了系统定位解算的容错性。算法流程如图 1

      图  1  联邦滤波方法流程

      Figure 1.  Workflow of the Proposed Federated Filter

    • 里程计可以量测得到运动平台的行驶距离,结合量测时间间隔进而获取行驶速度,以抑制IMU位置漂移。

      基于扩展卡尔曼滤波建立IMU同里程计组合导航模型状态方程:

      $$ \mathit{\boldsymbol{X}}_k^{DR} = \mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k\left| {k-1} \right.}^{DR}\mathit{\boldsymbol{X}}_{k-1}^{DR} + \mathit{\boldsymbol{W}}_{k-1}^{DR} $$ (1)

      系统状态矢量为19维向量:

      $$ {\mathit{\boldsymbol{X}}^{DR}} = {\left[{\varphi \;\;{\delta _v}\;\;{\delta _p}\;\;{\varepsilon _b}\;\;{\nabla _b}\;\;{e_p}\;\;\delta k} \right]^{\rm{T}}} $$ (2)

      ΦDR为状态转移矩阵;WDR为过程噪声;φ为三个方向失准角;δv为三个方向速度误差;δp为位置误差;εb为IMU三个方向陀螺漂移;∇b为三个方向加速度零偏;ep为里程计三个方向位置误差;δk为里程计刻度系数误差。系统观测方程为:

      $$ \mathit{\boldsymbol{Z}}_k^{DR} = \mathit{\boldsymbol{H}}_k^{DR}\mathit{\boldsymbol{X}}_k^{DR} + \mathit{\boldsymbol{V}}_k^{DR} $$ (3)

      式中VkDR为量测噪声;量测矩阵为:

      $\mathit{\boldsymbol{H}}_k^{DR} = \left[{-{\mathit{\boldsymbol{M}}_{3 \times 3}}\;\;{\mathit{\boldsymbol{E}}_{3 \times 3}}\;\;{{\bf 0}_{3 \times 12}}\;\;-\mathit{\boldsymbol{v}}_{od}^n} \right], {\mathit{\boldsymbol{E}}_{3 \times 3}} $为单位矩阵;vodn为里程计在三个方向上的速度; M3×3为运动平台速度的反对称矩阵:

      $$ {\mathit{\boldsymbol{M}}_{3 \times 3}} = \left[{\begin{array}{*{20}{c}} 0&{-\mathit{\boldsymbol{v}}_{od}^n}(3)&{\mathit{\boldsymbol{v}}_{od}^n\left( 2 \right)}\\ {\mathit{\boldsymbol{v}}_{od}^n\left( 3 \right)}&0&{-\mathit{\boldsymbol{v}}_{od}^n\left( 1 \right)}\\ {-\mathit{\boldsymbol{v}}_{od}^n\left( 2 \right)}&{\mathit{\boldsymbol{v}}_{od}^n\left( 1 \right)}&0 \end{array}} \right] $$ (4)
    • 序列立体影像定位是基于相邻立体影像间特征点追踪建立的帧间相对位置与姿态,通过位置与姿态的不断递推,从而计算得到连续的影像位置与姿态。传统基于相邻帧特征点匹配的解算方法定位误差随行驶距离而累积,因而引入多帧影像间连续匹配,进而建立多帧间几何约束,构成几何约束强的区域影像网,应用光束法平差求解网中影像最优的位置姿态参数。

      基于区域网光束法平差的视觉测程定位方法的主要步骤为[7]:①特征点提取; ②特征点匹配与连续追踪; ③粗差剔除; ④定位初解算; ⑤几何关键帧选取;⑥平差定位解算。

      IMU同序列立体影像的组合滤波中,观测量为运动平台的姿态和位置信息。滤波量测方程中观测量是姿态误差角,因此滤波前对相关的角误差做相应的转换,将平台失准角转换成姿态误差角[13],转换关系如式(5),该式进行了相关的简化,略去其中二阶小量:

      $$ \mathit{\boldsymbol{B = }}\left[\begin{array}{l} {\delta _\theta }\\ {\delta _\gamma }\\ {\theta _\psi } \end{array} \right] = \frac{1}{{\cos \theta }}\left[{\begin{array}{*{20}{c}} {-\cos \theta \cos\psi }&{\cos \theta \sin \psi }&0\\ {-\sin \psi }&{-\cos \psi }&0\\ { - \sin \theta \sin \psi }&{ - \sin \theta \cos \psi }&{\cos \theta } \end{array}} \right]\left[\begin{array}{l} {\varphi _E}\\ {\varphi _N}\\ {\varphi _U} \end{array} \right] $$ (5)

      式中,φEφNφU为平台失准角;θγψ为三个姿态角,δθδγδψ为对应姿态误差角。

      立体影像连续定位解算依赖于特征点的连续追踪。滤波过程中,当影像纹理贫乏无法完成特征点追踪时,视觉定位无法输出结果,滤波将失败。为提高定位解算稳健性,通过IMU与里程计组合的子滤波器的联合滤波,提高定位稳健性与精度。

    • 联邦滤波为一种分散化滤波方法,在兼顾精度和容错性情况下,能稳健地输出滤波结果。滤波时,各子滤波器独立进行时间与量测更新,主滤波器又进行时间更新,相应状态估计与协方差矩阵结果进行全局信息融合后依照信息分配系数更新子滤波器与主滤波器。联邦滤波的关键是在满足信息守恒原则下实现各个子滤波器的最优融合。

      设有N个局部状态估计的公共状态为$ {{\mathit{\boldsymbol{\hat X}}}_1}$、${{\mathit{\boldsymbol{\hat X}}}_2} $、…、$ {{\mathit{\boldsymbol{\hat X}}}_N}$, 一个主滤波器估计的公共状态为${{\mathit{\boldsymbol{\hat X}}}_p} $,对应的估计误差协方差阵为P11P22、…、PnnPpp,各个滤波器不相关。信息融合过程中,全局最优滤波器的状态与协方差为[10]

      $$ {\mathit{\boldsymbol{X}}_g} = {\mathit{\boldsymbol{P}}_g}\left( {\sum\limits_{i = 1}^N {\mathit{\boldsymbol{P}}_{ii}^{-1}{{\mathit{\boldsymbol{\hat X}}}_i} + \mathit{\boldsymbol{P}}_{pp}^{-1}{{\mathit{\boldsymbol{\hat X}}}_p}} } \right) $$ (6)
      $$ {\mathit{\boldsymbol{P}}_g}{\left( {\sum\limits_{i = 1}^N {\mathit{\boldsymbol{P}}_{ii}^{-1} + \mathit{\boldsymbol{P}}_{pp}^{-1}} } \right)^{-1}} $$ (7)

      全局噪声总信息量Q-1需分配至各个子滤波器与主滤波器中,即

      $$ {\mathit{\boldsymbol{Q}}^{-1}} = \sum\limits_{i = 1}^N {\mathit{\boldsymbol{Q}}_i^{-1} + \mathit{\boldsymbol{Q}}_p^{-1}} $$ (8)

      式中,Qi-1Qp-1为子滤波器与主滤波器分配得到的信息,相应计算式为:

      $$ {\mathit{\boldsymbol{Q}}_i} = \mathit{\boldsymbol{\beta }}_i^{-1}\mathit{\boldsymbol{Q}}, \;i = 1 \cdots N, p $$ (9)
      $$ {\mathit{\boldsymbol{Q}}_m} = \mathit{\boldsymbol{\beta }}_m^{-1}\mathit{\boldsymbol{Q}} $$ (10)

      式中, βiβm分别为各自子滤波器与主滤波器分配参数。信息分配需满足守恒,即

      $$ \sum\limits_{i = 1}^N {{\mathit{\boldsymbol{\beta }}_i} + {\mathit{\boldsymbol{\beta }}_m} = 1} $$ (11)
    • 为了验证该方法的可行性,分别在中国山东科技大学地下矿井实验场(实验1)以及中国矿业大学(徐州)文昌校区的防空洞内(实验2)进行了两个模拟地下巷道环境的导航平台定位实验。图 2是实验平台,平台上安置了IMU、里程计与立体相机。IMU的陀螺零偏为0.07°/s,采集频率为100 Hz。里程计测量分辨率为4.9 mm,采集频率为2 Hz。立体相机基线长为0.3 m,相机视场角为41°。行驶过程中,运动导航平台由工控机同步采集里程仪、IMU及序列影像数据。图 3为立体相机在两个实验场中获取的典型立体影像。在两个实验进行前,已在三维控制场中对立体相机内方位元素、相机间相对外方位元素及同IMU的相对位置姿态参数进行了标定[1]

      图  2  运动实验导航平台

      Figure 2.  Experimental Mobile Platform in the Experiment Field

      图  3  获取的立体影像

      Figure 3.  Stereo Images Acquired in Two Experiments

      地下矿井内组合导航实验1中,移动平台由起点进入巷道,至终点后原路返回起点,全程行驶约98 m,以2帧/s获取了1 448帧立体影像,联邦滤波中子滤波器1、子滤波器2和主滤波器的信息分配参数分别为0.2、0.65、0.15。为了分析不同定位方法的精度,分别采用IMU与里程计航迹推算方法、序列影像视觉测程方法及联邦滤波进行解算,通过闭合差大小评价定位精度,定位解算结果表明三种解算方法定位精度分别为5.07%、1.75%及0.49%,定位轨迹如图 4。结果表明联邦滤波器定位精度明显优于子滤波器1和子滤波器2单独的定位精度。

      图  4  实验1三种方法确定的行驶轨迹

      Figure 4.  Localization Results in Experiment 1 from Dead-reckoning, Visual Odometry and Federated Filter

      防空洞内组合导航实验2中,在行驶路径上布控了7个检查点,其局部坐标由全站仪测得,作为真值进行定位精度验证。实验过程中,运动平台由检查点1出发,按点号顺序依次行驶一圈后回到起始点,形成闭合环。采集数据过程中,控制移动平台在检查点处停留短暂时间,并记录此时获取图像与IMU数据的序号,以作为后续真值比较的数据索引。实验中,实验平台共行进约238 m,以4 fps获取了7 100帧立体图像。与实验1类似,分别采用IMU与里程计航迹推算方法、序列影像视觉测程方法及联邦滤波进行解算。联邦滤波参数与实验1中所设参数值相同。精度评定时,依据先前在经过检查点时记录的数据索引编号,确定在检查点处计算得到的定位结果,通过同检查点数据的对比进行精度评定。三种方法得到的行驶路径如图 5,在各个检查点的定位误差如图 6表 1,定位精度分别为9.12%、2.35%及2.08%。

      图  5  实验2三种方法确定的行驶轨迹

      Figure 5.  Localization Results in Experiment 2 from Dead-reckoning, Visual Odometry and Federated Filter

      图  6  不同方法定位误差

      Figure 6.  Localization Errors from Dead-Reckoning, Visual Odometry and Federated Filter

      表 1  全路径7个检查点处定位误差统计/m

      Table 1.  Statistic of Localization Errors in 7 Check Points of Traverse Path/m

      解算方法 检查点号
      2 3 4 5 6 7 1
      航迹推算 3.13 3.14 3.12 4.43 6.10 10.57 21.70
      视觉测程 0.25 1.18 0.60 0.57 3.74 5.13 5.59
      联邦滤波(无断帧) 0.47 1.23 0.76 0.97 3.31 4.50 4.96

      为了验证本文所提方法的容错性,实验2中将立体视觉解算强制设置为失败状态,造成断帧情况,以分析定位稳定性与精度。实验中分别设置断帧36帧、136帧、236帧及336帧4种情况。相应定位解算结果显示如图 7表 2。总体上看,定位精度随着断帧数量的增加而逐渐降低,这是因为断帧时系统定位结果主要依靠航迹推算,此时系统误差水平明显增大。但相比只使用航迹推算的系统,立体相机、IMU及里程计集成的定位系统仍然能够取得稳定的高精度定位结果,能在地下等受限环境下的导航定位中发挥作用。

      图  7  不同数量断帧下的定位误差

      Figure 7.  Localization Errors with Different Numbers of Failed Frames

      表 2  联邦滤波中不同数量断帧定位误差对比/m

      Table 2.  Statistic of Localization Errors with Different Numbers of Failed Frames/m

      断帧情况 检查点号
      2 3 4 5 6 7 1
      无断帧 0.47 1.23 0.76 0.97 3.31 4.50 4.96
      断36帧 0.47 1.23 1.05 1.07 3.87 4.87 5.55
      断136帧 0.47 1.23 2.14 1.59 4.93 5.88 6.67
      断236帧 0.47 1.23 1.96 1.84 4.70 5.27 6.41
      断336帧 0.47 1.23 5.03 3.87 7.53 8.93 9.32

      图 7表 2可以看出,出现了断236帧结果优于断136帧的情况,这是由于断帧时系统子滤波器2失效,子滤波器1参数需经振荡调整后趋于合理。由于运动平台状态并非严格平稳,地面环境也非完全一样,因此子滤波器1输出的位置误差并不是随运行时间严格线性增加,而是在增加的趋势上有起伏,随着时间的增加(断帧数增加),存在小概率的定位精度提升,致使重新起算时系统整体精度有所提升。总体趋势上,加上视觉测程滤波器的联邦滤波器系统优于仅用IMU和里程计的滤波器,在部分视觉特征缺乏而视觉测程滤波器失效的环境,联邦滤波器系统仍能输出可靠的结果,实现连续定位。

    • 本文提出了一种无GPS信号的受限环境下基于联邦滤波的IMU/立体相机/里程计运动平台组合导航定位方法,该方法利用IMU分别同里程计与立体相机构成两个子滤波器,有效避免立体视觉定位失效而导致的系统定位失败,提高了定位稳健性与精度。在无GPS信号的地下巷道中的实验结果表明,联邦滤波能够提高运动平台的定位精度,定位精度优于航迹推算和视觉测程定位方法,同时当立体视觉解算失效时仍能够提供高精度的定位结果,提高了定位的稳健性。

参考文献 (16)

目录

    /

    返回文章
    返回