基于视觉的惯性导航误差在线修正

0 引言

基于视觉的同步定位与建图(Simultaneous Localization and Mapping,SLAM)，已经被越来越多地应用在机器人导航、无人机导航等领域。但是单纯的视觉SLAM很难胜任复杂的任务，往往需要多传感器融合才能满足任务要求。其中惯性测量单元(Inertial Measurement Unit,IMU)被认为与相机传感器有明显的互补优势，而且十分有潜力在融合之后得到更加完善的SLAM系统[1]。

视觉通过外部特征点的匹配获取自身位姿，所以视觉不需要对时间进行积分，没有像IMU一样的零漂，但是视觉不能直接得到自身的运动状态，所以在大加速度和大角速度的运动下无法给出较为准确的姿态估计，容易导致跟踪失败，同时也无法辨别环境中的动态物体。IMU虽然可以测得角速度和加速度，但这些测量量都存在明显的漂移，尤其对于低成本的IMU来说，长时间积分得到的导航信息非常不可靠，但对于短时间内的快速运动可以提供较好的估计。正是由于这些特点，视觉和IMU可以优缺点互补。视觉SLAM不会随着时间漂移，可以有效地修正IMU数据中的漂移，使其在慢速运动中的位姿估计依然可用。IMU对自身加速度和角速度的测量可以提高系统的动态性能，同时帮助系统判别环境中的动态障碍物。

因此多年来出现了很多视觉和IMU组合导航的算法。其中一大类是基于滤波的组合算法，在文献[2]、文献[3]的视觉惯性导航(Visual Inertial Navigation,VIN)算法中有比较充分的介绍，尽管这类算法在实时性上有不错的表现，但是由于线性化等误差以及误差积累，导致长时间工作时状态估计误差增大，甚至导致滤波发散；另一类算法是基于非线性优化的组合算法，例如文献[4]、文献[5]，但是随着应用环境逐步增大，数据越来越多，实时性变得越来越差。因此发展出基于关键帧的非线性的优化算法，通过优化与跟踪并行的非同步优化方法来提高实时性，如文献[6]、文献[7]。

本文针对廉价的惯性测量单元和视觉融合的算法，提出一种基于Kalman滤波的在线估计陀螺加速度计零偏的算法。因为廉价的惯性器件精度较低，整个SLAM系统并不将其用于提高视觉SLAM算法的精度，而是用于提高系统在大加速度、大角速度下的适应性，因此采用滤波与文献[8]中的非线性优化结合的方法。通过优化算法提高视觉SLAM的精度，将视觉信息作为观测量，利用滤波进行零偏估计，为视觉SLAM提供运动预测，使整个SLAM系统具有很好的动态适应性，并保持很好的实时性。

1 系统模型

1.1 IMU积分模型

在此约定，用W表示世界坐标系。W坐标系是在建图时随机确定的，但一旦确定，在建图过程中和建图完成后的导航任务中，都是固定不变的。B表示IMU坐标系，由于IMU与车体固联，B系亦即载体坐标系。

为载体相对世界坐标系的角速度在IMU坐标系下的表示表示IMU对的测量值，其主要误差为陀螺零偏，用bg表示。同理，aW为载体实际加速度在世界坐标系下的表示，g为重力加速度在世界坐标系下的向量表示，为IMU对加速度的测量，RWB为IMU坐标系到世界坐标系的姿态变换矩阵，ba表示加速度计零偏。假设陀螺零偏和加速度计零偏都是非常缓慢的变化，短时间内近似为常值。有：

(1)

(2)

由于世界坐标系是随机确定的，重力加速度已知，但在世界坐标系各轴分量未知，同时重力加速度和加速度计零偏区分比较困难，因此在算法中需要对重力加速度的分量和IMU的零偏同时进行估计。有了加速度和角速度的测量以后，可以积分得到IMU相对世界坐标系的姿态RBW，以下简称R。本文姿态均用旋转矩阵表示。IMU相对世界坐标系的速度和位置分别用V和P表示。有如下关系：

(3)

(4)

(5)

其中,(·)∧表示一种运算。

明治时代(1868－1912)作为日本近代化国家形成的开端，无论是经济基础还是上层建筑方面都发生了巨大变革，为确立天皇制意识形态而施加于民的儒家思想，受到西方追求个性解放的文化思潮的强烈冲击。作家们以其特有的细腻敏感的笔触，将亲身体验或感知的种种矛盾、复杂通过文字呈现给读者，使得明治中后期的文学呈现出绚烂多元的现代特征。中日学界的明治文学研究一直在持续，表现出一一对应关系的同时，也呈现了一定的本国特色。

(6)

积分式(3)、式(4)、式(5)并代入式(1)、式(2)，可得

(7)

其中，Exp(φ)=exp(φ∧)。

生15：我明白了，在图4中，由于角是转动的，所以如果△AMD∽△DNM只能是∠ADM=∠DMN，那么就会有MN∥AB，而事实上，在转动的过程中它们的位置关系是变化的，不是确定的，所以不可能三个三角形同时相似．

测量结果以100%为基线，代表苯乙烯生产装置在使用市场对标催化剂、常规的低出口压力设置时的副产物生成量。在后一个反应器中，分别测量较低出口压力（34.5 kPa）和较高出口压力（38 kPa）时的副产物生成量。在后一种情况中，预期副产物生成量将会升高。

(8)

(9)

Pi表示IMU在图像Ci帧时的位置，结合式(9)有

1.2 相机模型

相机模型为针孔模型如图1所示，用I表示图像坐标系o′x′y′。P=[X,Y,Z]T表示空间一点相对相机坐标系C的位置在C中的表示。p=[u,v,1]T为点P在I投影的齐次坐标，已知相机的内参矩阵K有

(10)

RWB=RWCRCB

(11)

PWB=RWCPCB+PWC

(12)

1.3 IMU和相机的组合导航算法

IMU数据采集的频率往往比视觉数据采集的频率高很多。如图2所示，视觉中，相邻的图像帧Ci、Cj通过共同观察到的路标集合M建立起相对位姿的关系。具体算法在文献[9]中的ORB_SLAM算法中有比较详细的描述。每一个路标为空间中的一点mi，对应一个观测zi，zi为mi在I中投影的齐次坐标，由式(10)有

系统中，IMU和相机是固连在一起的，其中IMU与相机的位置关系已知。用C表示相机坐标系，用RCB、PCB分别表示由相机坐标系到IMU坐标系的位姿变化。RWC、PWC表示相机相对世界坐标系的位姿，有：

(13)

同时观测zi是当前相机位姿和上一帧位姿的函数记为用PCi、RCi表示在第i帧相机相对于世界坐标系的姿态。有

(14)

关于PCi、RCi的残差rc可以表示为

(15)

通过非线性优化算法[8]优化PCi、RCi使rc取得最小值，得到通过特征匹配优化后位姿有

(16)

根据式(11)、式(12)、式(16)可以得到RWB和PWB。通过非线性优化方法可以很好地消除图像采集和特征提取的噪声，同时可以减小错误匹配带来的影响，得到较为准确的位姿，但式(15)是非凸优化，优化的精度和速度对优化初值很敏感。当初值和真值相差较远，甚至会得到错误的位姿，并且还可能造成特征匹配失败。惯性导航可以为特征匹配和优化算法提供一个更好的初值，提升算法的速度、精度和动态的适应性。

2 模型的线性化

2.1 相关知识

由于旋转矩阵R对加法不封闭，而是属于李群空间的元素，所以引用李群和李代数的知识。这些理论都基于文献[10-11]。记(·)∨为(·)∧的逆运算，若S=ω∧记S∨=ω。对于(·)∧有如下规则：

a∧b=-b∧a ∀a,b∈R3

(17)

(18)

结合式(23)、式(24)有

exp(φ∧)≈I+φ∧

(19)

对于属于李群空间的R记log(·)为：

(20)

(21)

如果规定那么φ=log(R)∨是R在李代数空间的唯一映射。为了书写方便，记

Log(R)=log(R)∨

(22)

如果很小，可以近似地表示为

Exp(φ+δφ)≈Exp(φ)Exp(Jrδφ)

(23)

Jr是李群空间的右乘雅克比矩阵

(24)

对Log运算有如下近似

Log[Exp(φ)Exp(δφ)]≈φ+Jr-1δφ

(25)

在IMU积分线性化的过程中还会用到下边一个很重要的公式

RExp(φ)RT=Exp(Rφ)

(26)

2.2 线性化

本文借鉴文献[12]的预积分方法对模型进行线性化。如图3所示，因为图像和IMU的采集频率不同，本文把每隔一定时间的图像当作关键位置，输出其位姿信息。

在图像帧Ci和Cj之间有若干个IMU数据，IMU的数据采集间隔假设是相同的，如图所示为Δt。在未估计IMU零偏的情况下，对原始数据进行积分可以得到如图3所示的原始积分，原始积分和视觉得到的姿态信息有很大的误差，主要由IMU零偏造成。通过线性化原始积分和IMU零偏关系，然后通过Kalman滤波求解IMU零偏是本文的目的。为了便于书写和理解,用Ri表示IMU在图像Ci帧时IMU的姿态RWB，用表示用表示结合式(7)从Ci帧到Cj帧对进行积分有

[44] Al-Marhubi, F., “Export Diversification andGrowth: an Empirical Investigation”, Applied Economic Letters, No. 7 (2000), pp. 559-562.

(27)

Vi表示IMU在图像Ci帧时的速度，令Δtij≐结合式(8)有

为保证仿真实验时探测器的运动规律和实际工作时相同,对探测器施加外力使其沿飞行方向(横滚轴方向)做变加速往返运动,比较不同姿态解算算法对探测器姿态解算精度的影响。如图6为存在一定运动加速度情况下,未使用加速度分离算法,仅使用普通卡尔曼滤波进行姿态解算的三轴姿态角误差曲线。

(28)

式(7)、式(8)、式(9)就是本文使用的IMU数据的积分模型，其中陀螺零偏bg、加速度零偏ba就是本文所要估计的量。

(29)

假设对式(27)进行线性化，记

(30)

如果很小，有近似公式

(31)

结合式(26)有

(ⅱ) 若δ=0,由于δ是[0,1]上Cauchy-列{xn}的所有收敛子列极限点的下确界,则必存在{xn}的一个收敛子列(当然还是Cauchy-列)收敛到0(易证)。

(32)

其中

大伙儿都同意交物业费，就是阿强不同意。阿强觉得自己家安上了高档防盗门，不要说小偷，就是一只苍蝇也难以飞进。由于他不肯出钱，请物业公司的事只得搁浅。

(33)

将式(32)代入式(27)并结合式(18)、式(6)有

V(t+Δt)= V(t)+gΔt+

选择新鲜、无机械伤、无病害、根茎结实粗壮且大小均匀的莲藕，表皮颜色白嫩略带黄褐色，没有明显黑色斑块[14]。

(34)

对式(34)两侧做Log()运算并结合式(25)有

（3）主成分分析和相关系分析结果表明，草海主要污染物为Cd、Cr、Zn和Hg，且具有同源性，其中土法炼锌是主要的污染原因，尽管已经关闭了土法炼锌，但其污染物仍对草海具有严重的生态危害。

Log(ER)=Jr(ER)Jbgbg

(35)

其中，式(35)就是对式(25)对bg进行线性化后的方程。

为了简便，记记仅线性化式(28)中的一部分，记

(36)

式(36)对bg、ba进行线性化，结合式(17)、式(19)有

(37)

式(37)即为式(36)线性化的结果。

记同理记

(38)

同理有

(39)

式(39)即为式(38)的线性化结果。

3 Kalman滤波估计

3.1 陀螺零偏的估计

在式(35)中，仅有bg一个待估计量，同时其他的观测信息只与陀螺有关，所以该式没有加速度计零偏的干扰，最适合对bg做出估计。记观测矩阵测量矩阵带估计量由于假设bg短时间内不变，所以状态转移矩阵为单位阵I，所以状态转移矩阵为

(40)

假设观测噪声为观测方程为

(41)

记为的协方差矩阵，如图4所示，按照标准的Kalman滤波流程迭代，最终收敛得到bg的估计。

3.2 加速度计零偏的估计

λi=βiRzgεnGE-βiRzgεn∧GEδθ+γiba

在已有研究基础上，本文利用分布决策和随机机会约束规划法，将研究对象拓展为多参数多产品，并深入考虑产品市场需求、参数约束等更多供应链不确定性，构建了不确定环境下的产品供应链碳足迹优化模型。针对模型多约束非线性混合规划特性，设计了一种ISAGA，经过8个经典基准测试函数和5个问题函数的比较分析可见，该算法在搜索精度、适应性、鲁棒性、时效性、准确性等方面均有较为明显的优势，展现了良好的性能和实用性。

(42)

其中，同理将bg代入式(39)中有

(43)

其中，

第一，军民融合知识产权转移转化机制的建立是一项系统工程，必须整体设计、全面打造，确保科学有效。机制建立涉及国家、企业、当地政府，要素上连着技术、资金、知识产权、互联网生态，必须精心设计，全面打造。要坚持政府引导，争取设立专项基金、引导基金等方式，促进知识产权转移转化；坚持纵横联动，充分发挥中央与地方、行业上下、军民之间的联动，在资源配置、任务部署等方面形成合力；坚持机制创新，围绕技术、专利、人才、资本等要素，设置知识产权转移转化机制研究相关子课题，分工协作和深入研究，形成系列机制创新成果。

采用相邻的三帧图像数据，将绝对速度V消去。因为视觉无法提供准确的速度信息，时刻k的绝对速度仅能通过加速度计积分得到，误差会不断的积累，消去绝对速度可以很好地提高估计精度。同时，在得到了IMU的零偏以后，根据式(29)可以求得Ci帧的速度Vi，结合式(28)得到Cj帧的速度Vi，从而通过短时间的惯性导航为视觉SLMA提供高精度的位姿预测。为了表达方便，用(·)ij表示Ci帧到Cj的相关数据，比如式(30)中的ΔR可以记为ΔRij。根据式(29)、式(39)有：

(44)

(45)

联合式(44)、式(45)、式(36)、式(42)、式(43)消去Vi、Vj有

21世纪图书馆项目的目的是研究当前和未来，如何通过图书馆服务来满足或支持各层次的学术和教学需要，以及资源达到最有效的工作效果，提供优质服务。数字人文为民国文献数字化深度开发提供理论和实践的经验和方法，为保护和完善珍贵资源，我们共同推进民国文献数字人文朝着更广泛、更深入的领域发展。

λi=βig+γiba

(46)

其中

由于重力加速度和加速度计零偏很难被区分出来，所以首先假设ba=0，有

λi=βig

(47)

这方程可以比较稳定的解出g，将这个解作为一个初值记为G。并记G方向的单位向量Gn，世界坐标系W的z轴方向单位向量Zn=(0，0，1)T。有

(48)

Zn到Gn的转角记Zn到Gn的旋转矩阵Rzg有

Rzg=Exp(θzgεn)

(49)

世界坐标系中的重力加速度可以表示为

按新准则规定，企业将某项以公允价值计量且其变动计入当期损益的金融资产重分类为以摊余成本计量的金融资产时，应当以其在重分类日的公允价值作为新的账面余额；而如果企业将该以公允价值计量且其变动计入当期损益的金融资产重分类为以公允价值计量且其变动计入其他综合收益的金融资产，则该金融资产应当继续以公允价值进行计量。

(50)

GE为本地重力加速度。假设，存在一个小的扰动，记为δθ=(x，y，0)T，为作用在上的旋转向量，因为只有x、y轴的旋转对有意义，所以z轴设为零。有

g=RzgExp(δθ)εnGR

(51)

式(46)可表示为

λi=βiRzgExp(δθ)εnGE+γiba

(52)

结合式(12)、式(14)有

通过3.1节的估计，在ba的估计中，bg是已知的。代入式(37)中有

(53)

记(αi1，αi2，αi3)=-βiRzgεn∧GE，有：

状态观测

(54)

测量矩阵

(55)

估计量

(56)

观测方程为

Zk=HkXk+Vk

(57)

状态转移矩阵为I，观测噪声协方差为R。如图4进行Kalman滤波得到ba。

4 实验验证

基于ROS系统开发环境[13]，实验数据采用Euroc Dataset发布的公开数据集V2_02_mdium进行验证。该数据集包含视频录像和同步采集到的IMU数据，同时给出了IMU与相机位置关系，满足论文验证的前提条件。图5、图6所示为该数据集在该算法下的运行结果。

图7所示为陀螺零偏估计的结果，X轴为时间，Y轴为陀螺零偏，图中3条曲线分别为零偏bg在IMU坐标系下三轴的分量。其中，z轴的分量估计相对稳定，x轴和y轴的跳动较大，但最终收敛，这是因为该视觉SLAM系统估计姿态时，姿态在x轴和y轴的误差较大，从而引起零偏估计收敛较慢。这也正是用IMU陀螺修正视觉SLAM姿态的优势之一。

图8所示为加速度计零偏估计的结果，X轴为时间，Y轴为加速度计零偏，图中3条曲线为零偏ba在IMU坐标系下的分量。

图9所示为世界坐标系中重力加速度在各轴的分量。X轴为时间，Y轴为重力加速度。

图10所示为估计IMU零偏后的误差err1和未估计IMU零偏的误差err0。该误差表示在该时刻到前20帧图像的时间内IMU导航与视觉导航之间的误差。X轴为时间，Y轴为积分误差，估计零偏后的误差是未估计零偏误差的10.9%，说明本文方法对IMU的积分精度有明显的提高。

5 结论

本文通过合理地应用IMU零偏模型，利用李代数和李群的知识将IMU零偏非线性模型线性化，与用视觉SLAM得到的位姿相结合，消除了速度积分的积累误差，通过Kalman滤波得到陀螺零偏、加速度计零偏，结果稳定可靠，对惯性导航精度有明显的提高。

参考文献

[1] Gui J, Gu D, Wang S, et al. A review of visual inertial odometry from filtering and optimisation perspectives[J]. Advanced Robotics, 2015, 29(20): 1289-1301.

[2] 逯建军，任晓军，孙伟，等.惯性/双目视觉里程计深组合导航方法[J].导航定位与授时，2016，3(3)：6-12.

[3] Jones E S, Soatto S. Visual-inertial navigation, map-ping and localization: A scalable real-time causal approach[J]. The International Journal of Robotics Research, 2011, 30(4): 407-430.

[4] Patron-Perez A, Lovegrove S, Sibley G. A spline-based trajectory representation for sensor fusion and rolling shutter cameras[J]. International Journal of Computer Vision, 2015, 113(3): 208-219.

[5] Indelman V, Williams S, Kaess M, et al. Infor-mation fusion in navigation systems via factor graph based incremental smoothing[J]. Robotics and Autonomous Systems, 2013, 61(8): 721-738.

[6] Strasdat H, Montiel J M M, Davison A J. Real-time monocular SLAM: Why filter?[C]// 2010 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2010: 2657-2664.

[7] Nerurkar E D, Wu K J, Roumeliotis S I. C-KLAM: Constrained keyframe-based localization and mapping[C]// 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014: 3638-3643.

[8] Kümmerle R, Grisetti G, Strasdat H, et al. G2o: A general framework for graph optimization[C]// 2011 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2011: 3607-3613.

[9] Mur-Artal R, Tardos J D.ORB-SLAM2:An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[10] Chirikjian G S. Stochastic models, information theo-ry, and lie groups, Volume 2: Analytic methods and modern applications[M]. Springer Science & Busi-ness Media, 2011.

[11] Wang Y, Chirikjian G S. Nonparametric second-order theory of error propagation on motion groups[J]. The International Journal of Robotics Research, 2008, 27(11-12): 1258-1273.

[12] Forster C, Carlone L, Dellaert F, et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation[C]// Georgia Institute of Technology, 2015.

[13] 刘汝佳，王芳，张强，等.基于ROS的机械臂轨迹规划研究[J].导航定位与授时，2016，3(6)：76-81.