论文总字数：20235字

摘 要

无论是机器人，还是无人驾驶领域都离不开自主导航技术。现有的自主导航技术大致分为两大类：一类是基于滤波的，另一类是基于优化的。导航技术中采用的比较经典的传感器有相机、惯性测量单元、激光雷达、轮速编码器等等，使用单一的传感器进行导航具有稳定性差、精度低等缺点，因此实际应用中研究者们一般采用多传感器联合的方式实现自主导航。本文所针对研究的MSCKF（Multi-State Constraint Kalman Filter，多状态约束卡尔曼滤波）算法就是采用的视觉/惯性组合且基于滤波的导航算法。

本文从视觉、惯性测量单元两部分出发，对视觉SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）技术以及IMU（Inertial Measurement Unit，惯性测量单元）中常用的方法和基础理论进行研究，接着进一步对MSCKF导航算法的各个步骤进行详细的研究和理论分析，最后对该自主导航算法进行仿真与评估。

关键词：自主导航；MSCKF；视觉/惯性组合

Abstract

Whether it is robots or unmanned driving, it is inseparable from autonomous navigation technology. Existing autonomous navigation technologies are roughly divided into two categories: one is based on filtering, and the other is based on optimization. The more classic sensors used in navigation technology are cameras, inertial measurement units, lidar, wheel speed encoders, etc. The use of a single sensor for navigation has the disadvantages of poor stability and low accuracy. The multi-sensor combined approach enables autonomous navigation. The MSCKF (Multi-State Constraint Kalman Filter) algorithm studied in this paper is a combined visual / inertial and filter-based navigation algorithm.

This article starts from the two parts of the visual and inertial measurement unit, researches the visual SLAM (Simultaneous Localization and Mapping, simultaneous positioning and map construction) technology and the IMU (Inertial Measurement Unit, inertial measurement unit) commonly used methods and basic theory, and then further Carry out detailed research and theoretical analysis on each step of the MSCKF navigation algorithm, and finally simulate and evaluate the autonomous navigation algorithm.

Key Words：Autonomous navigation；MSCKF；Visual / inertial combination

目 录

第1章 绪论 1

1.1 研究背景 1

1.2 研究目的及意义 1

1.3 国内外研究现状 2

1.3.1视觉SLAM算法研究状况 2

1.3.2 视觉/惯性组合算法研究状况 2

第2章 视觉SLAM 4

2.1 视觉传感器种类 4

2.2 视觉SLAM框架 4

2.3 旋转理论基础 5

2.3.1 旋转矩阵 5

2.3.2 四元数 6

2.4 特征点提取与匹配 6

2.4.1 关键点与描述子 7

2.4.2 实践：特征点提取与匹配 7

2.5 特征点三角化 9

第3章IMU传感器 11

3.1 IMU数学模型 11

3.2 IMU预积分 11

第4章 卡尔曼滤波与扩展卡尔曼滤波 13

4.1 卡尔曼滤波 13

4.1.1 离散时间线性时变系统 13

4.1.2 卡尔曼滤波过程 13

4.2 扩展卡尔曼滤波 14

4.2.1 线性化 14

4.2.2 扩展卡尔曼滤波过程 15

第5章 多状态约束卡尔曼滤波 17

5.1 滑动窗口 17

5.2 状态向量结构 17

5.3 IMU状态传递 18

5.4 相机状态扩增 19

5.4.1 状态量扩增 19

5.4.2 协方差扩增 20

5.5 观测模型 20

5.6 滤波更新 22

5.7 实践：ROS仿真 23

5.7.1 实践操作过程 23

5.7.2 实践结果分析 25

5.7.3 算法精度分析 27

5.8 算法评估 28

第6章 总结与展望 30

6.1 研究总结 30

6.2 展望 30

参考文献 31

致 谢 33

第1章 绪论

1.1 研究背景

近些年来，随着无人驾驶、无人机、机器人等技术在商业、科技、军事领域的广泛应用，其中尤为重要的组成部分——导航算法也在不断的更新换代。SLAM方法最早由Smith和Cheeseman于1986 年提出，并应用于移动机器人领域，其目的是在环境先验信息未知情况下，装有特定传感器的载体于运动过程中构建环境模型，同时估计自身的状态和位置信息，从而实现自主定位与导航。

根据所用传感器的不同种类，SLAM又可细分为视觉SLAM（VSLAM）和激光SLAM等。视觉SLAM是使用视觉相机作为传感器感知周围环境的SLAM，是目前比较常用的技术。但单纯的视觉SLAM存在一些局限性，其严重依赖于图像信息的复杂程度，如果图像信息过于单一，就无法利用纯视觉技术准确的估计出相机的运动状态。基于此，相关研究人员将视觉SLAM与其他传感器联合使用，其中最常见的是将视觉SLAM与IMU相结合，利用IMU所提供的加速度和角速度信息可以弥补视觉SLAM的不足。本文所研究的MSCKF算法就是将视觉SLAM和IMU进行融合的经典算法。

1.2 研究目的及意义

基于多状态约束的MSCKF算法是一种具有典型性的视觉/惯性组合导航算法。MSCKF算法采用的扩展卡尔曼滤波(Extended Kalman Fliter，EKF)的误差状态向量的滤波更新形式，并在经典EKF算法的基础上进行了优化，将算法的效率和误差估计的一致性大幅度提升。经典EKF的做法是将特征点的信息直接加入到状态向量中，这样会导致数据冗杂并且需要耗费大量的计算资源和存储空间。MSCKF算法并没有直接使用特征点的信息，而是利用多个时刻的相机位姿和特征点之间的观测约束建立观测模型，从而进行滤波更新过程。此外MSCKF算法还设置了一个动态的滑动窗口以及阈值，不断实时更新滑动窗口中相机信息。基于以上几点，MSCKF算法在一定程度上提高了工作效率并节省计算资源。

此外，MSCKF相对于单纯的视觉SLAM算法，能够适应更剧烈的运动、一定程度的纹理缺失的环境等，具有更高的鲁棒性。相对于基于优化的视觉/惯性组合算法（VINS，OKVIS），MSCKF精度相当，速度更快，适合在计算资源有限的嵌入式平台运行。在机器人、无人机、AR/VR领域，MSCKF都有较为广泛的运用。

研究MSCKF算法的目的是为了学习传感器融合的方法，MSCKF算法与其他传感器融合算法在一定程度上具有相似性，学习并研究该算法对深入传感器融合的研究领域有很大的帮助。此外，研究该导航算法之前还需要对视觉和IMU的基础理论以及常用的处理方法进行深入学习和总结，这一点也是研究目的之一。

1.3 国内外研究现状

视觉/惯性组合导航技术在无人驾驶领域已经有大规模的研究和应用，并且大多数都是研究紧耦合的方法，因为这样做算法的精度会很高。下面主要讨论视觉SLAM以及视觉/惯性组合算法的研究现状。

1.3.1视觉SLAM算法研究状况

SLAM技术主要分为视觉SLAM和激光SLAM。随着视觉技术在定位与建图领域的不断发展，视觉SLAM也逐渐成为研究领域的热点。

视觉SLAM起步相对于激光SLAM较晚。早期的视觉SLAM一般都是对相机采集的数据进行处理后，再利用处理好的数据实现定位与建图，不能满足实时定位与建图的目的。2007年，A.J.Davisont提出的Mono-SLAM^[1][2]是第一个实现实时在线的单目SLAM系统，其技术是基于EKF的。2013年浙江大学计算机视觉组提出了面向动态场景的RDSLAM^[3]，该算法可以适用于大量遮挡或者错误匹配的场景，鲁棒性更高。针对视觉SLAM研究现状高翔等人^[4]做出了较为全面的讲解与介绍。

1.3.2 视觉/惯性组合算法研究状况

视觉/惯性组合也分为松耦合和紧耦合（如图1所示）。比如Konlige^[5]将IMU仅作为倾角传感器和航向估计器来进行立体视觉优化。而MSCKF是紧耦合中较为经典的一种算法，最早由Mounkis^[6][7]于2007年提出。MSCKF在以往的EKF基础上进行了改良，通过维护一个按照时间先后顺序排列的位姿队列，解决了维数爆炸问题，并提高了系统的鲁棒性。此外，针对MSKCF算法Richard Guillemard^[8]等人提出了惯性和视觉融合方法来区分SoftStop和HardStop，并将区分信息送入MSCKF框架中，以减少速度漂移，从而进一步提高系统状态估计的准确性和鲁棒性。

在视觉/惯性组合导航算法方面，国内研究的相对较少。针对基于优化的VIO， Xiangyu Deng^[9]等人提出了一种紧耦合的非线性优化 RGB-D-惯性融合系统。

请支付后下载全文，论文总字数：20235字