控制与决策  2020, Vol. 35 Issue (8): 1823-1832  
0

引用本文 [复制中英文]

闫德立, 喻薇, 宋宇, 吴春慧, 宋永端. 基于矩阵李群表示及容积卡尔曼滤波的视觉惯导里程计新方法[J]. 控制与决策, 2020, 35(8): 1823-1832.
[复制中文]
YAN De-li, YU Wei, SONG Yu, WU Chun-hui, SONG Yong-duan. A new method for visual inertial odometry based on cubature Kalman filter and matrix Lie group representation[J]. Control and Decision, 2020, 35(8): 1823-1832. DOI: 10.13195/j.kzyjc.2018.1596.
[复制英文]

基金项目

国家自然科学基金项目(61773081, 61860206008, 61803053, 61833013, 61573053, 11972238);中央高校基本科研业务费专项基金项目(2018CDPTCG0001/43);河北省自然科学基金项目(E2016210104);河北省教育厅项目(Z2017022)

作者简介

闫德立(1981−), 男, 博士生, 从事多传感器融合、机器人同时定位与地图构建的研究, E-mail: fana8010@126.com;
喻薇(1987−), 女, 博士生, 从事智能控制、容错控制的研究,E-mail: yuwei@cqu.edu.cn;
宋宇(1979−), 男, 博士生, 从事机器人导航控制、图像处理的研究, E-mail: songyu@bjtu.edu.cn;
吴春慧(1994−), 女, 硕士生, 从事视觉里程计、视觉惯导融合导航的研究, E-mail: 1205105324@qq.com;
宋永端(1962−), 男, 教授, 博士生导师, 从事智能控制、容错控制等研究, E-mail: ydsong@cqu.edu.cn

通讯作者

宋永端(1962−), 男, 教授, 博士生导师, 从事智能控制、容错控制等研究, E-mail: ydsong@cqu.edu.cn

文章历史

收稿日期:2018-11-19
修回日期:2019-05-12
基于矩阵李群表示及容积卡尔曼滤波的视觉惯导里程计新方法
闫德立 1,2, 喻薇 3, 宋宇 1, 吴春慧 2, 宋永端 1,3     
1. 北京交通大学 电子信息工程学院,北京 100044;
2. 石家庄铁道大学 电气与电子工程学院,石家庄 050043;
3. 重庆大学 自动化学院,重庆 400044
摘要:针对滤波方法实现的视觉-惯导里程计(VIO)问题, 为更准确传递旋转运动的不确定性并降低系统线性化误差, 提高位姿估计的精度, 设计并实现了一种高维矩阵李群表示的采用容积卡尔曼滤波框架实现的VIO算法.算法将状态变量构建为一个高维李群矩阵, 并定义了李群变量在容积点采样过程中的‘加法’运算, 将容积点和状态均值、方差等概念由欧氏空间扩展到流形空间; 采用容积变换传递状态均值及方差, 避免了旋转运动复杂的雅克比矩阵计算过程, 降低了模型线性化误差.最后, 使用EuRoc MAV数据集进行算法验证, 结果表明所提出算法在提高位姿估计精度方面是有效的.
关键词视觉-惯导里程计    矩阵李群    容积卡尔曼滤波    位姿估计    
A new method for visual inertial odometry based on cubature Kalman filter and matrix Lie group representation
YAN De-li 1,2, YU Wei 3, SONG Yu 1, WU Chun-hui 2, SONG Yong-duan 1,3     
1. College of Electronic and Information Engineering, Beijing Jiaotong University, Beijing~100044, China;
2. College of Elctric and Electronic Engineering, Shijiazhuang Tiedao University, Shijiazhuang~050043, China;
3. College of Automation, Chongqing University, Chongqing 400044, China
Abstract: Considering robotic visual inertial odometry (VIO) using filtering methods, a VIO algorithm is proposed in order to improve estimation accuracy. This algorithm uses cubature Kalman filter on matrix Lie group to realize it, which can accurately describe system uncertainty in rotation and reduce the linearization error of systems. The characters of the proposed algorithm are that: 1) the state is built by an high dimensional Lie group matrix and the definition of the additional operation for Lie group variant is proposed in cubature point sampling, which can extend the concept of cubature point, state mean and covariance from Euclidean space to manifold; 2) the state mean and covariance are propagated by cubature transformation, which avoids calculating complicated Jacobi matrixes and reduces the linearization error of the system. The performance of the proposed algorithm is tested in the EuRoc MAV dataset, and the results show the effectiveness of the proposed algorithm in improving estimation accuracy.
Keywords: visual inertial odometry    matrix Lie group    cubuture Kalman filter    pose estimation    
0 引言

精确的位姿估计是实现机器人控制的基础, 视觉惯导里程计是通过融合惯导测量单元(inertial measurement unit, IMU)信息和视觉信息, 实现对移动平台位置和姿态的精确估计[1].视觉信息和惯导信息在定位方面有良好的信息互补性, 随着MEMS IMU的发展, 视觉-惯导里程计(VIO)具有精度高、功耗低及微型化等优势, 在移动机器人、无人飞行器及手持设备等方面均有广泛的应用[2].由于视觉-惯性导航系统具有强非线性, 如何有效利用惯性信息和视觉信息的互补性实现精确定位, 一直是该领域研究的热点和难点.

近些年, 关于VIO的研究成果有很多, 按照对视觉信息和惯导信息的耦合方式, VIO可以分为松耦合VIO和紧耦合VIO[3].紧耦合方式VIO在精度和鲁棒性方面具有优势, 因此逐步成为近年来主流实现方式.按照实现方法又可以分为基于滤波方法的VIO, 例如MSCKF算法[4]、ROVIO算法[5]等, 以及基于关键帧的非线性优化的方法VIO, 例如文献[6-8]等提出的算法.

在运动估计方面, 刚体旋转运动是造成系统非线性的主要因素之一, 旋转运动的表示方法主要有欧拉角法[9]、旋转矩阵法、四元数法[4, 10]和李代数[11]等方法.他们各有优缺点, 比如欧拉角方法比较直观, 但在导航计算中存在万向节锁的问题[12]; 旋转矩阵采用3×3维矩阵描述旋转, 存在过参数的问题; 四元数是近年来在里程计和SLAM研究中广泛采用的旋转表示方法, 但四元数同样存在过参数化问题, 并且在滤波计算过程中, 每一步更新均需进行归一化操作以保证其单位化[13].在滤波方法中采用李代数描述位姿, 解决了过参数的问题, 在算法收敛性和位姿估计精度方面均具有优势.

李群被用于状态估计最早可追述20世纪70年代[14], 文献[15]提出了李群及黎曼几何在机器人运动系统上的重要应用.近年来, 在机器人领域, 越来越多的研究方法采用李群上的概率分布估计机器人位姿, 例如文献[16-17].无论是采用滤波的实现方法还是基于优化的实现方法, 李群表示方法均能得到更好的收敛性和估计精度[18].近期, 采用矩阵李群表示状态变量的不变量扩展卡尔曼滤波器(invariant-EKF, IEKF)吸引了越来越多研究者兴趣, 该滤波器结合了对称性保持理论与扩展卡尔曼滤波理论.文献[19]首次将IEKF滤波器应用于SLAM领域.文献[20]提出了右侧不变性EKF(RI-EKF), 并将其应用于2D SLAM实现中, 证明了该实现方法在一致性方面具有更好的效果.文献[21]将IEKF用于视觉惯性导航系统(visual inertial navigation systems, VINS), 提出了RIEKF-VINS算法, 证明了算法在提高状态估计精度和一致性方面的有效性.文献[22]将IEKF与MSCKF算法结合, 提出了RI-MSCKF算法, 同样证明了改进后的视觉-惯导位姿估计性能的提高.文献[23]描述了完整的IEKF方法及其收敛特性, 但IEKF方法需要对运动方程求解复杂雅克比矩阵. Brossard等[24]将基于李群的无迹变换理论与不变量卡尔曼滤波器结合, 提出了Right-UKF-LG算法, 该算法将IMU信息和环境特征点构建为高维李群矩阵, 在UKF框架下实现位姿估计.但当向量维数高于4时, UT变换的中心采样点的权值为负[25], 中心采样点同其他采样点距离会随着系统维数的增加不断增大, 从而产生非局部效应, 可导致传递状态协方差的非正定, 进而导致滤波数值不稳定, 影响里程计位姿估计的精度.

针对以上问题, 本文提出矩阵李群表示的基于容积卡尔曼滤波的VIO算法, 系统状态变量由IMU的位置、姿态、速度及空间3D特征点构成的高维李群矩阵SE2+m(3)表示, 在容积卡尔曼滤波框架下实现机器人位姿的精确估计.由于CKF采用三阶球面-相径容积规则选取容积点, 不存在中心采样点的问题, 同UKF实现方法相比, 在状态向量维数较高情况下, 其稳定性和精度都要更高.该算法将容积点和状态均值、方差等概念由欧氏空间扩展到流形空间, 采用容积变换的方法传递状态均值及方差, 传递后的均值和方差在流形切向空间计算实现.算法不仅继承了不变量卡尔曼滤波在收敛性和一致性方面的优势, 同时避免了IEKF中复杂雅克比矩阵求解过程以及UKF实现方法中可能出现的协方差非正定情况, 从而有效提高了刚体位姿估计的精度和稳定性.

文章的主要贡献包括: 1)将不变量滤波理论与容积卡尔曼滤波器相结合, 定义了李群变量在容积点采样中‘⊕’操作, 实现了容积变换方法传递矩阵李群变量的不确定性; 2)阐述了特征管理整个过程, 采用矩阵李群表示各个时刻相机位姿, 通过李群容积变换估计新增特征点位置和协方差.算法在真实数据集上进行了验证, 通过与近期滤波方法实现的几种算法对比, 验证了所提出算法在提高VIO位姿估计精度方面是有效的.

1 背景知识 1.1 VIO坐标系

本文选用的坐标系包括世界坐标系{W}、惯导坐标系{I}和相机坐标系{C}, 如图 1所示.

图 1中, IWT∈ SE(3)表示从惯导坐标系{I}到世界坐标系{W}的转移, 用于描述IMU在世界坐标系下的位姿, 包括姿态IWR ∈ SO(3)和位置pIR3两部分; CIT表示相机坐标系{C}到惯导坐标系{ I}的转移, 由于相机和惯导刚性连接, CIT在前期标定中已知; pj表示特征点三维空间位置.视觉-惯导里程计目的是根据视觉和惯导测量信息, 采用滤波或优化的方法, 精确估计刚体在各个时刻的位姿IWT.

图 1 VIO坐标系
1.2 矩阵李群及其随机变量

矩阵李群GRN × N是平方可逆矩阵的子集, 满足如下性质:

(1)

其中IN表示N维单位矩阵.对于元素χG, 有该点处G的切向空间向量同它对应, 称为李代数, 他们之间通过指数映射和对数映射运算实现相互映射.旋转矩阵R ∈ SO(3)可以由向量ξR3描述, 指数映射和对数映射计算表示为

(2)

其中: ξ^表示向量ξ的反对称矩阵, expm(·)表示矩阵指数映射运算.

定义在矩阵李群G上的随机变量对加法不封闭[26], 即, 因此无法采用欧氏空间加性噪声的方式描述不确定性.对于随机变量χ, 采用对应的李代数指数映射的方式描述[27], 其概率分布定义为, 表达式为

(3)

其中: 表示典型的欧氏空间向量高斯分布, ξRN, PRN × N. Nr(·, ·)中的r表示不变量右乘表示方法, 该描述方式将李群空间随机变量的概率分布用欧氏空间的高斯分布进行了描述, χ表示变量均值, ξ描述变量的不确定性, 文中称为偏差变量.

2 系统模型

采用Kalman滤波方法实现视觉-惯导里程计, 目的是估计坐标系{I}在{W}中的位姿WIT, 整个过程分为状态预测和状态更新两部分. IMU传感器输出刚体的三轴加速度信息a和三轴角速度信息ω, 在预测阶段实现刚体运动预测; 相机提供对空间特征点的观测, 观测信息用于系统状态更新.采用滤波方法实现VIO过程中需要明确状态变量的结构、传感器运动模型和观测模型.

2.1 系统状态变量

在VIO实现过程中需要首先明确系统传递的状态变量结构, 一般包含需要估计的所有信息, 包括IMU姿态R, 位置pI、速度v、零偏b=[bω, ba], 以及m个特征点的3D空间位置pp = [p1, ..., pm] ∈R3m.将RpIvpp构造为高维的特殊欧氏群矩阵χ ∈SE2+m(3), 结构形式为

(4)

其对应的偏差变量为

(5)

由式(3)可知, χ可以表示为

(6)

向量ξ对应的反对称矩阵结构为

(7)

其指数映射表达式为

(8)

在状态传递过程中, 状态变量除了包含χ, 还包含IMU零偏b, b可以表示为形式, 其中b表示均值, 表示其均值附近的随机偏差.因此, 整个系统的状态变量均值为, 随机偏差向量为.

2.2 IMU数据测量和运动模型

IMU传感器输出刚体的三轴加速度信息和三轴角速度信息, 这些测量值表示在惯导坐标系{I}下, 记为

(9)
(10)

测量信息受到高斯白噪声n和零偏b的影响.其中: Wg表示世界坐标系{W}下的重力加速度, IMU的位姿运动模型为

(11)

式(11)描述的是IMU在连续时间状态下的运动方程, 实际中测量值是离散时间采样, 假设[t, tt]时间间隔内Ia(t)和Iω(t)保持不变, 得到IMU运动函数f(·)如下:

(12)

将式(9)和(10)代入(12), 可以得到刚体在下一个时刻的位姿.数值计算过程中, 假设R(t)在采样时间间隔内是定值, 必然会引入积分计算误差, 实际工程中通常采用高速IMU, 因此计算过程中可忽略该误差影响.

2.3 观测模型

单纯依靠IMU进行位姿估计, 由于受噪声影响, 误差会随着时间迅速增长.在VIO滤波实现方法中, 采用相机的观测信息对姿态进行更新估计, 从而得到更精确的刚体位姿.观测模型为典型的小孔成像模型, 内参数在预处理阶段完成标定, 对空间3D特征点pj的观测为yj, 观测噪声为, 投影方程

(13)

其中: Π为针孔相机的内参数矩阵, WpI表示IMU在世界坐标系{W}下的位置, CIRCIpc为坐标系{C}到{I}的旋转矩阵和平移向量.

3 提出的VIO算法

采用矩阵李群表示系统的状态变量, 在容积卡尔曼滤波框架下实现视觉信息和惯导信息的融合, 从而精确估计机器人的位姿.算法将需要估计的IMU姿态、位置、速度、零偏及空间3D特征点位置构建成一个高维特殊正交群矩阵, 采用三阶球面-相径容积规则选取容积点, 通过容积变换传递状态均值及方差, 传递后的均值和方差在流形切向空间计算实现.本节从状态预测、状态更新和特征管理3个过程介绍采用容积变换方法传递李群变量不确定性, 实现刚体位姿估计和观测特征点估计的过程.

3.1 状态预测

采用CKF实现状态预测, 是利用采样容积点描述状态变量的分布, 经过IMU运动模型传递后, 对容积点加权求和计算传递后的状态均值和方差.系统的状态变量在2.1节已描述, k时刻的状态变量均值为Xk=[χk, bk], 随机偏差向量为, 服从方差为Pk的高斯分布.其协方差平方根因子为Sk, 满足Pk=SkSkT, 对应σk各分量的方差Pk结构如图 2所示.

图 2 状态变量协方差结构
3.1.1 计算采样容积点

k时刻, 系统的控制输入为IMU的角速度和加速度, 即uk =[ωk, ak], 对应的噪声方差为Qu, 其协方差平方根因子为Su.为获得采样容积点, 首先需要用输入控制向量ukSu增广状态变量及其协方差平方根因子, 即

(14)

增广后协方差平方根因子Saug的维数为ls=3m+21.根据容积点采样规则, 对Saug进行采样, 即

(15)

ζj结构包括χ的偏差向量ξkj, IMU零偏向量以及输入噪声, 即.根据容积律方法, 容积点为Xaug ‘ + ’ ζj的形式, 在流形空间李群矩阵对加法不封闭, 因此定义‘⊕’操作符, 容积点具体计算形式为

(16)

容积点的构成分成了3个部分, IMU零偏和输入满足欧氏空间加法, χ部分采用指数映射方式描述李群矩阵的采样容积点.

3.1.2 传递容积点

将容积点集通过式(12)所示的IMU运动方程进行传递, 可以计算得到传递后的容积点集.

(17)

将第j个容积点通过运动方程进行传递, 主要更新容积点中IMU分量.在k时刻, 状态容积点结构形式为

(18)

对应噪声为nuj=[nωj, naj].由式(12), 可求得k+1时刻状态, 求解过程如下:

(19)
(20)

其中Wak表示在{W}坐标系下的加速度, 可根据式(10)计算得到, 即

(21)

进而可以计算得到相应的位置分量

(22)

将状态变量的均值进行传递, 可以得到k+1时刻的IMU位姿和偏差均值, 即

(23)
3.1.3 计算协方差平方根因子

为计算时间更新后状态变量协方差平方根因子Sk + 1|k, 需要计算传递后偏差向量.传递后χ的偏差向量和b的偏差分别为

(24)
(25)

其中ξk + 1|kj由传递后的容积点对数映射实现.

通过对偏差向量矩阵和噪声协方差因子构成的矩阵进行QR分解, 可以计算得到时间更新后的状态变量协方差平方根因子

(26)
(27)
3.2 状态更新

通过对特征点观测跟踪, 实现滤波器的状态更新, 图像特征跟踪采用KLT算法[28].假设连续两个时刻跟踪特征点的数目为m, 其观测值为y, 观测噪声为nR, 方差为QR, 结构形式为

(28)

利用容积变换方法求k+1时刻跟踪特征的观测误差, 首先使用观测特征yk + 1和观测协方差平方根因子SRXk + 1|kSk + 1|k进行状态增广.

(29)

其中: SR =chol(QR) ∈R2m×2m, 增广后的Saug的维数为lm = 5 m + 15.为计算容积点, 首先进行采样, 得到采样点

(30)

其中Cj的结构为.同预测阶段容积点计算类似, 更新过程中容积点如下所示:

(31)

将容积点集通过观测方程(13)进行传导, 可以得到传递后的观测容积点为

(32)

k+1时刻, 观测特征的估计值yk+1|k及每个观测容积点的偏差项ej由下式得到:

(33)
(34)

通过对ej和观测噪声方差平方根因子SR构成的矩阵进行QR分解, 可以得到观测协方差平方根因子Sy, 即

(35)

状态变量和观测之间的互相关协方差矩阵为

(36)

状态更新的卡尔曼增益为K=PXy(SyT) - 1Sy - 1, 可以计算得到状态偏差增量为

(37)

更新后的状态变量均值及协方差平方根因子由下式求得:

(38)
(39)
(40)

由此, 系统通过容积卡尔曼滤波实现了刚体位姿的估计, 整个过程中采用容积变换方法传递状态变量和方差, 避免了对IMU运动方程和观测方程复杂的雅克比求解过程, 同时对李群矩阵进行容积点采样, 将容积变换的方法扩展到了流形空间.

3.3 特征点管理

随着相机运动, 系统跟踪的观测特征点会部分离开相机视域, 同时需要有新的特征点补充到状态变量中.如图 3所示, 在k时刻, 滤波器跟踪的特征点p2离开相机视域, 为保持更新过程中状态变量观测特征数目不变, 需要补充新的特征点pnew.假设pnew的空间位置为pnew, 方差为Pnew, 替换过程只需将式(4)中p2}对应的位置向量更换为pnew, 将图 2中方差Pp2替换为Pnew, 因此特征管理过程中, 需要通过观测值估计pnew的位置和方差.

图 3 跟踪特征点管理过程

pnew中的new表示在k时刻新加入状态变量, 并不是首次观测到.相机在k时刻的观测位姿为TkC = {RkC, pkC}, 对特征点pnew的观测值为yknew.假设在k时刻之前的n个时刻中, 均有观测到该特征点, 各个观测时刻相机位姿为{Tk-n + 1C... TkC}, 观测值为{yk-n+1new... yknew}, 其观测方程为

(41)

需要求解的新特征点位置pnew可以表示为观测值、相机位姿和观测噪声的函数, 即

(42)

g(·)函数表示采用三角化方法[29]求解3D点位置函数.问题的实质是在已知各个时刻相机位姿、方差以及观测噪声条件下, 通过g(·)函数推导得到k时刻特征点的位置和方差.本文采用矩阵李群方法表示各个时刻相机位姿, 位姿不确定性通过对应的偏差向量表示, 通过容积变换方法传递位姿不确定性, 进而求解k时刻新加入特征的位置和方差.

为估计新加入的特征点位姿, 首先确定状态变量为χC = [Tk-n + 1C... TkC], 其对应的偏差向量ξC的结构为

(43)

描述了n个时刻相机的姿态和位置的不确定性, χC对应的方差和平方根因子为{QCSC.采用容积变换方法进行计算, 首先利用跟踪特征点的观测对状态变量和协方差因子进行增广, 增广后的SaugC维数为lC = 8n.

(44)

根据容积点的采样规则, 采样点为

(45)

υj的结构为[ξRC, j ξpC, j nRj], 其中ξRC, jξpC, j分别表示容积点中姿态偏差分量和位置偏差分量, nRj为噪声分量, 因此容积点

(46)

将容积点通过式(42)传递, 可求得传递后的容积点值pnewj, 通过加权求和可得到位置均值pnew.

(47)
(48)

每个容积点的误差项可通过下式计算得到:

(49)

通过对误差项进行QR分解, 可以得到k时刻的pnew的观测的协方差平方根因子Sknew.

(50)

该方法采用高维李群矩阵表示n个时刻相机位姿, 通过容积变换传递相机位姿不确定性, 完成新增特征点位置和方差的估计.求解过程中避免了对g(·)函数复杂雅克比求解过程, 同时为后续估计方法的改进提供了便利.

4 实验结果及分析 4.1 测试数据集及实验参数

为测试算法有效性, 采用公共数据集EuRoc MAV数据库[30]进行算法验证.该数据集是由一个微型飞行器在室内环境下进行的数据采集, 包括双目图像数据和IMU测量信息, 以及由VICON运动捕捉系统采集的真实运动轨迹, 同时该数据集提供标定好的内外参数.双目相机的帧频为20 FPS, IMU的数据输出频率为200 Hz, 测试使用的是左相机的输出图像, 相机模型为针孔相机模型.本文仿真环境为Matlab R2017a, 处理器为Inter Core i7-5500U, 选用数据集中V1_ 02_ medium数据集进行测试.其中, IMU测量噪声为高斯白噪声, 偏差为随机游走模型, 主要的噪声参数和方差见表 1.

表 1 验参数
4.2 实验结果及分析

为验证算法在提高位姿估计精度方面的有效性, 实验针对机器人位置和姿态误差进行了比较.进行对比的算法包括文献[24]提出的R-UKF-LG算法, 文献[31]提出的SE(3)-UKF算法, 文献[21]提出的IEKF算法, 这几种算法均采用滤波方法实现VIO, 并且均采用李群方法表示运动姿态.

4.2.1 图像特征点跟踪与管理情况

测试过程中, 各种算法采用相同的噪声参数和观测特征, 特征跟踪采用的是KLT特征跟踪算法, 滤波器更新过程中图像特征点变化情况如图 4所示.

图 4 视觉-惯导里程计估计轨迹及真实轨迹

图 4中, 左右两张图片是相机在连续两个时刻的观测图片, 图中标识表示了特征点的变化情况.其中圆圈‘º’表示两个时刻匹配的特征点, 左图中星号‘*’表示随着相机运动, 不再被跟踪的特征点, 这些特征点需要移出特征变量; 右图中五角星表示下一个时刻新加入特征变量的跟踪特征点. 图 4显示的跟踪结果对应3.3节中特征管理的过程.

4.2.2 观测特征数目相同条件下各种算法性能对比

图 5直观描述了本文算法在EuRoc数据集上的运行结果, 虚线表示真实的运动轨迹, 实线表示算法估计的运行轨迹, 柱状图表示位置误差变化情况.从位置及误差变化情况可以看到, 算法估计的运动轨迹和真实轨迹基本符合, 由于视觉里程计没有闭环检测, 位置误差存在不断累积趋势.从误差变化来看, 在飞行器急转弯处, 运动角度变化快的地方误差累积较大.各个时刻具体的误差情况由图 6图 7具体描述.

图 5 特征点跟踪与管理
图 6 姿态平均误差对比
图 7 位置平均误差对比

为验证本文所提出算法在提高位姿估计性能方面的有效性, 采用位置和姿态在各个时刻的平均误差以及整个运行轨迹的位置、姿态角均方根误差、linebreak(root mean square error, RMSE)作为评价指标.实验中本文所提出算法命名为SCKF-LG算法, 选取V1_ 02_ medium数据集中四旋翼60 s飞行轨迹信息进行算法验证, 包括1 200幅图片和12 000组IMU信息.各算法进行30次蒙特卡洛实验, 计算各个时刻的位置、姿态角与真实轨迹间的误差, 并求取平均值, 得到图 6图 7的结果.实验过程中, 状态变量观测特征数目设置为30.

图 6描述的是在各个时刻姿态角误差的平均值, 可以看到在初始阶段各种算法性能相似, 随着时间推移, 4种算法的位姿角误差均在增长, 但SCKF-LG算法增长速度和波动幅度明显低于其他3种算法, 说明在姿态估计方面, 本文所提出算法的稳定性和精度均优于其他3种对比算法.

图 7描述了各个时刻估计位置和真实轨迹间的误差平均值.可以看到, 在初始阶段4种算法估计性能近似; 随着运行时间增长, 累积误差均在增长, SCKF-LG算法在运行17 s后平均误差开始略低于其他3种算法; 在运行35 s后, 平均误差明显低于其他3种算法.结果表明, 所提出算法在提高位置估计性能方面是有效的.

为进一步验证算法的性能, 将4种对比算法在EuRoc数据库中V1_ 02_ medium、V2_ 03_ difficult、MH_ 03_ medium和MH_ 05_ difficult数据集上进行了30次蒙特卡洛算法验证, 运行时长均为60 s, 位置和角度平均误差的对比结果在表 2中列出.实验结果表明, 飞行环境不同, 累积误差会有较大差别, 但在4种环境下, 所提出算法在位置和姿态平均误差方面均低于其他3种算法, 说明算法在提高VIO位姿估计性能方面是有效的.

表 2 4种数据集中对比算法的位置和姿态RMSE
4.2.3 不同观测特征数目条件下各种算法性能对比

在本文算法中, 状态变量维数为3m+15, m为观测特征的数目, 采样容积点的个数是增广状态变量维数的2倍, 因此跟踪特征点的数目直接影响算法运行时间, 并且传递后的观测噪声对VIO性能有直接的影响.本节实验主要验证不同的观测特征数目对各种算法的影响, 主要包括位置和姿态的估计精度和滤波状态更新平均耗时两个方面.实验计算了观测特征数目分别为20、25、30、35和40条件下, 4种算法位置和姿态在整个轨迹上的RMSE及4种算法的更新耗时情况.算法运行时间为30 s, 结果为30次实验平均值, 结果在图 8图 9表 3中说明, 单位为s.

表 3可以看出, 随着跟踪特征点的增多, 4种算法耗时均出现不同程度的增加.在30 s运行时间下, IEKF算法在各种特征数目下耗时略少, 但其估计性能并不是最优, 其余3种算法耗时近似.由图 8图 9可以看出, 在5种观测特征数目条件下, 本文所提出算法在位置和角度估计精度方面均优于其他3种算法.在姿态估计方面, 观测特征数目为30和35时, 本文算法估计性能近似, 但35个观测特征在更新步骤会增加51 %的耗时.在位置估计方面, 特征数目为25和30时, 位置RMSE误差相对较小, 但30个特征点相比25个特征点, 状态更新步骤增加24 %的时间支出.通过图表可以说明观测特征数目太多或者太少均会降低算法的估计性能, 均衡估计精度和算法耗时, 选择合理的观测特征数目可以得到较好的估计性能.

图 8 不同观测特征数目条件下的角度RMSE
图 9 不同观测特征数目条件下的位置RMSE
表 3 不同观测特征条件下状态更新步骤耗时
5 结论

本文针对滤波方法实现的机器人视觉惯导里程计问题, 设计并实现了一种矩阵李群表示的采用容积卡尔曼滤波框架的VIO算法.算法将不变量滤波理论与容积卡尔曼滤波器相结合, 通过构建高维李群矩阵表示系统状态变量, 在容积卡尔曼滤波框架下实现机器人位姿的精确估计.

算法的主要特点是:

1) 定义了李群变量的容积点采样过程, 将容积卡尔曼滤波算法由欧氏空间扩展到流形空间;

2) 采用李群表示刚体旋转, 避免了四元数表示方法过参数及归一化约束等问题, 同时状态向量由高维李群矩阵表示, 相比混合表示方法, 结构更加清晰;

3) 采用平方根容积卡尔曼滤波实现方法, 避免了复杂雅克比计算过程, 同时提高了数值计算稳定性.

算法在真实EuRoc数据集上进行了验证, 通过与近期几种滤波VIO算法进行对比, 验证了所提出算法在提高VIO位置、姿态估计精度方面是十分有效的.

参考文献
[1]
Gui J J, Gu D B, Wang S, et al. A review of visual inertial odometry from filtering and optimisation perspectives[J]. Advanced Robotics, 2015, 29(20): 1289-1301. DOI:10.1080/01691864.2015.1057616
[2]
Santoso F, Garratt M A, Anavati S G. Visual-inertial navigation systems for aerial robotics: Sensor fusion and technology[J]. IEEE Transactions on Automation Science and Engineering, 2017, 14(1): 260-275. DOI:10.1109/TASE.2016.2582752
[3]
Paul M K, Wu K J, Hesch J A, et al. A comparative analysis of tightly-coupled monocular, binocular, and stereo VINS[C]. IEEE International Conference on Robotics and Automation (ICRA). Singapore: IEEE, 2017: 260-275.
[4]
Mourikis A I, Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]. IEEE International Conference on Robotics and Automation (ICRA). Roma: IEEE, 2007: 3565-3572.
[5]
Bloesch M, Omari S, Hutter M, et al. Robust visual inertial odometry using a direct EKF-based approach[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg: IEEE, 2015: 298-304.
[6]
Indelman V, Williams S, Kaess M, et al. Information fusion in navigation systems via factor graph based incremental smoothing[J]. Robotices Autonomous Systems, 2013, 61(8): 721-738. DOI:10.1016/j.robot.2013.05.001
[7]
Leutenegger S, Lynen S, Bosse M, et al. Keyframe-based visual–inertial odometry using nonlinear optimization[J]. International Journal of Robotics Research, 2015, 34(3): 314-334. DOI:10.1177/0278364914554813
[8]
Usenko V, Engel J, Stueckler J, et al. Direct visual-inertial odometry with stereo cameras[C]. IEEE International Conference on Robotics and Automation (ICRA). Stochholm: IEEE, 2016: 1885-1892.
[9]
Shen S Y, Mulgaonkar, Michael N, et al. Vision-based state estimation and trajectory control towards high-speed flight with a quadrotor[C]. Proceedings of Robotics Science The Systems. Berlin: IEEE, 2013: 1157-1165.
[10]
Kelly J, Sukhatme G S. Visual-Inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration[J]. International Journal of Robotics Research, 2011, 30(1): 56-79.
[11]
Engel J, Stückler J, Cremers D. Large-scale direct SLAM with stereo cameras[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg: IEEE, 2015: 1935-1942.
[12]
Grewal M S, Weill L R, Andrews A P. Global positioning systems, inertial navigation, and integration[J]. Wiley Interdisciplinary Reviews Computational Statistics, 2007, 3(4): 383-384.
[13]
Marins J L, Yun X, Bachmann E R, et al. An extended Kalman flter for quaternionbased orientation estimation using MARG sensors[C]. Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Maui: IEEE, 2002: 2003-2011.
[14]
Brockett R W. Lie algebras and lie groups in control theory[C]. Geometric Methods in System Theory. Netherlands: Springer, 1973: 43-82.
[15]
Park F C, Bobrow J E, Ploen S R. A Lie group formulation of robot dynamics[J]. The International Journal of Robotics Research, 1995, 14(6): 609-618.
[16]
Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual-inertial odometry[J]. IEEE Transactions on Robotics, 2016, 33(1): 1-21.
[17]
Barfoot T D, Furgale P T. Associating uncertainty with three-dimensional poses for use in estimation problems[J]. IEEE Transactions on Robotics, 2014, 30(3): 679-693.
[18]
Zhang T, Wu K, Song J, et al. Convergence and consistency analysis for a 3D invariant-EKF SLAM[J]. IEEE Robotics Automation Letters, 2017, 2(2): 733-740. DOI:10.1109/LRA.2017.2651376
[19]
Bonnabel S. Symmetries in observer design: Review of some recent results and applications to EKF-based SLAM[C]. Robot Motion and Control 2011. London: Springer, 2012: 3-15.
[20]
Barrau A, Bonnabel S. An EKF-SLAM algorithm with consistency properties[J/EL].[2016-09-02]. https://arxiv.org.labs/1510.06263.
[21]
Zhang T, Wu K, Su D, et al. An invariant-EKF VINS algorithm for improving consistency[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver: IEEE: 2017: 1578-1585.
[22]
Heo S, Chan G P. Consistent EKF-based visual-inertial odometry on matrix Lie group[J]. IEEE Sensors Journal, 2018, 18(9): 3780-3788. DOI:10.1109/JSEN.2018.2808330
[23]
Barrau A, Bonnabel S. The invariant extended Kalman filter as a stable observer[J]. IEEE Transactions on Automatic Control, 2017, 62(4): 1797-1812. DOI:10.1109/TAC.2016.2594085
[24]
Brossard M, Bonnabel S, Barrau A. Invariant Kalman filtering for visual inertial SLAM[C]. The 21st International Conference on Information Fusion (FUSION). Cambridge: IEEE, 2018: 2021-2028.
[25]
闫德立, 宋永端, 宋宇, 等. 一种改进的高斯混合概率假设密度SLAM算法[J]. 控制与决策, 2014, 29(11): 1959-1965.
(Yan D L, Song Y D, Song Y, et al. An improved Gaussian mixture PHD SLAM algorithm[J]. Control and Decision, 2014, 29(11): 1959-1965.)
[26]
高翔, 张涛, 刘毅, 等.视觉SLAM十四讲[M].第1版.北京: 电子工业出版社, 2017: 64-65.
(Gao X, Zhang T, Liu Y, et al. Visual SLAM fourteen lessons[M]. 1st ed. Beijing: Electronic Industry Press, 2017: 64-65.)
[27]
Barfoot T D, Furgale P T. Associating uncertainty with three-dimensional poses for use in estimation problems[J]. IEEE Transactions on Robotics, 2014, 30(3): 679-693.
[28]
Shi J, Tomasi C. Good features to track[C]. IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Seattle: IEEE, 1994: 593-600.
[29]
Richard H, Zisserman A. Multiple view geometry in computer vision[M]. New York: Cambridge University Press, 2000: 312-320.
[30]
Burri M, Nikolic J, Gohl P, et al. The EuRoC micro aerial vehicle datasets[J]. International Journal of Robotics Research, 2016, 35(10): 1157-1163. DOI:10.1177/0278364915620033
[31]
Loianno G, Watterson M, Kumar V. Visual inertial odometry for quadrotors on SE(3)[C]. IEEE International Conference on Robotics and Automation (ICRA). Stackholm: IEEE, 2016: 1544-1551.