多源融合定位算法综述

裴凌 李涛 花彤 郁文贤

0 引言

全球导航卫星系统(Global Navigation Satellite Systems，GNSS)信号到达地面已经非常微弱，无法穿透室内、地下等环境，并容易受到有意或者无意的干扰.所以，国家提出建设以北斗为核心、多源互补与信息融合的国家综合 PNT(Positioning，Navigation，Timing)体系.多源融合定位是国家综合PNT体系用户侧的核心技术.在国家综合 PNT 体系泛在的信号源覆盖之下，高可靠、高可信的用户终端需要具有更加灵活和智能的导航架构，用于快捷集成多种异质异构传感器/信号源.为了适应复杂场景和载体动态，更高效合理地进行多源弹性融合，本文将从传感器观测模型、环境场景模型、载体运动行为模型(3M-based)3个视角对多源融合定位算法进行综述.

考虑到通用性和互补性，本文主要关注4类传感器：卫星、惯性传感器、视觉传感器和激光雷达.同一类型传感器的观测模型也不尽相同.例如，卫星导航的观测模型可以考虑伪距、相位、多普勒观测值的交替应用，可以考虑不同观测值之间做组合[1]，也可在不同观测值之间做差分[2]消除一定的误差.惯性导航是不依赖于环境和基础设施的一种内感型导航方式.惯性传感器的观测模型主要由其本身的精度决定，低精度惯导可以考虑使用较为简单的模型，而高精度惯导在处理时需要更加精细化的模型来避免一些误差的累积.视觉传感器观测模型从间接法特征点[3]、特征线[4]、特征面[5]的方式，到直接法[6]对整个图像进行光度误差最小化.激光雷达的观测模型也从点到直线[7]、点到平面[8]的残差到将整个空间离散到栅格中并用正态分布去描述之后配准整个分布[9].无论什么观测模型，其本质反映的都是载体运动状态到传感器观测之间的联系.

环境场景模型对运动状态的约束主要是由传感器的观测间接构建.因为不同传感器在不同场景下会有不一样的观测特性.例如，卫星导航在遮挡环境的观测会受到多路径和非视距卫星的影响，在室内卫星导航几乎都不会有任何信号.在特征退化的环境下，依靠环境进行相对定位的视觉传感器和激光雷达都会受到较大的影响.惯性传感器在长时间的运动下会出现误差累积进而导致估算轨迹漂移.所以，对场景识别和理解有助于融合策略中的传感器选择[10].

载体复杂的运动行为通常会影响观测的可靠性，但是如果能够充分利用不同类型载体的运动特性，也可以约束载体的运动状态.比如：车辆正常行驶速度主要来源于车的前向[11]，静止时，三轴方向的速度都是0；行人运动过程中脚触地时刻速度为0，正常行走时加速度呈正弦周期性变化；无人机可以利用加速度估计飞行动力学参数，从而间接估计飞行速度等.因此，建立载体运动行为模型对于融合定位具有非常重要的状态约束作用.

传感器观测模型、环境场景模型、载体运动行为模型最终需要在多源融合定位算法框架中进行统一表示.一般融合算法分为滤波和优化两类，可根据应用场景和载体特性进行选择.本文对当前主流的多源融合定位算法从3M视角进行归纳概括，具体如表1所示.

表1中，GNSS表示卫星导航，IMU表示惯性测量单位，Vision表示视觉SLAM，LiDAR表示LiDAR-SLAM，NHC表示非完整性约束，ZUPT表示零速更新.

1 传感器及其观测模型

1.1 卫星导航系统

全球卫星导航系统(GNSS)在多源融合定位算法中能够提供全局的定位并且不随时间发散.GNSS接收机的原始观测值主要包括伪距、载波相位和多普勒频移3类.通过对应的算法，GNSS可以提供定位、测速和授时服务.GNSS最基础的3个观测方程为

ρ(n)=r(n)+c·δtr+δe(n)-c·δt(n)+I(n)+T(n)+

(1)

(2)

l(n)=r(n)+c·δtr+δe(n)-c·δt(n)+λ·N-I(n)+

(3)

实时动态(Real-Time Kinematic,RTK)载波相位差分技术[2]通过位置信息已知的基站和高精度的载波相位测量值，实时获得高精度的定位结果.其基本原理为构建站间差分和星间差分方程如式(4)所示，即双差观测模型：

(4)

RTK算法精度与流动站、基站之间的距离相关，距离越远精度往往越低，需要地面布设30～50 km间隔的基准站.为了减少建设基站投入，精密单点定位(Precise Point Positioning,PPP)越来越受关注[25].PPP发展经历了实数解、固定解以及PPP-RTK 3个阶段.在PPP实数解阶段，观测模型是研究人员最关注的问题.其中消电离层组合通过电离层误差一阶项与频率的平方成反比的特性消除电离层一阶项影响，UofC模型通过同频率伪距和载波相位的电离层误差互为相反数的特性消除电离层一阶项影响[1].PPP固定解主要是要解决整数模糊度与硬件延迟之间的耦合问题.常用的方法为未检校的相位延迟(Uncalibrated Phase Delay,UPD).UPD的原理是窄巷模糊度的小数部分在短时间内是稳定的[26].有UPD的辅助使得PPP固定解成为可能，但仍旧要经历长时间的观测才能获得PPP固定解，使得PPP固定解应用受限.造成PPP固定解需要长时间观测的主要原因是大气影响，于是PPP-RTK被提出，它利用局域网观测数据，精化求解UPD以及补充求解大气改正，使得达到固定解的速度更快[27-28].

表1 常见多源融合定位算法

1.2 惯性导航系统

惯性导航系统包括惯性测量单元(Inertial Measurement Unit,IMU)和惯导解算算法两部分.其中IMU一般由加速度计和陀螺仪两部分组成，分别测量加速度和角速度.在多源融合定位导航算法中能够提供几乎不受环境影响的高频物理状态测量.从加速度计获得的加速度将被积分1次以获得线速度，积分2次提供位置数据，而从陀螺仪测得的角速度将被积分1次用来提供姿态数据.

依据陀螺仪是否能感应到地球自转角速度，本文将IMU分为低精度和高精度两大类.若陀螺仪噪声数量级大于等于地球自转角速度，则惯导被认为是低精度惯导，它无法通过陀螺仪感应到地球自转从而无法估计航向角；若陀螺仪噪声数量级小于地球自转角速度，则惯导被认为是高精度惯导，它可以通过感应地球自转估计航向角.高精度的惯导算法需要考虑地球自转角速度，而低精度的惯导算法不需要.

惯导运动学微分方程包含姿态、位置和速度三个微分方程.一般均以解姿态微分方程开始，有旋转矩阵法、四元数法.姿态更新中因不可交换误差产生的圆锥误差，通过等效旋转矢量[29]及其多子样算法进行补偿.速度中因不可交换误差产生的划桨误差[30]，也可通过多子样算法进行补偿.这些都是在高精度惯导算法中需要考虑的问题，对于低精度惯导均无需考虑.

惯导一般被认为是多源传感器融合定位算法的核心.基于滤波框架多源传感器融合定位算法将惯导的误差状态方程作为状态方程.然而，对于扩展卡尔曼滤波框架而言，线形化误差会造成滤波器的不一致性，导致过于乐观的协方差估计，这一现象已有较多研究[31-34].对此有四类解决方案.第一种是以FEJ(First-Estimates Jacobian)[32]技术为代表的修正线性化点的方案，提出固定首次线性化的雅可比矩阵，保证了系统可观性矩阵的零空间不退化，该方案被用于OC-EKF[33]和MSCKF的改进版本[35-36]中.第二种是以R-VIO[37]为代表的机器人中心建图的EKF框架，以机器人为中心建图可以回避与全局重力向量的对齐，并且不会遇到以世界为中心的VIO方法面临的可观性不匹配问题.第三种是文献[38]中使用的随机克隆(Stochastic Cloning)的方法.随着李群理论研究的深入，Bonnabel等尝试将速度与路标点纳入到位姿之中形成一种新颖的李群结构，并由此提出了第四种解决一致性问题的方案——不变扩展卡尔曼滤波(Invariant Extended Kalman Filter)[39-40]，这种新方案的一致性在文献[41] 中得到证明，并进一步应用到了MSCKF[42-43]、UKF[44]中.表2对一致性问题的解决方案进行了总结.

表2 系统一致性问题的解决方案

1.3 视觉传感器

由于视觉传感器的低成本和便捷性，基于视觉传感器的实时定位和建图技术，即视觉SLAM(visual Simultaneous Localization and Mapping,vSLAM)技术一直是学术界和工业界研究的重点.一般vSLAM使用的视觉传感器为RGB相机，围绕其设计的算法框架也是当前主流.除此之外，热红外相机[45-47]、事件相机[48-49]等也逐渐受到关注，可以在特殊场景下取得较好的效果.近年来，vSLAM算法在计算机视觉、AR技术、机器人领域有了长足的发展并得到广泛应用.vSLAM算法分为基于特征法和直接法.基于特征的vSLAM方法又可分为基于滤波器的算法和基于光束平差法的方法.MonoSLAM[50]被认为是基于滤波器的代表性方法，它使用了扩展卡尔曼滤波器来估计相机运动与3D特征点的位置.StructSLAM[4]也是基于滤波器的vSLAM算法，它在视觉前端采用结构特征线而不是传统的特征点.基于光束平差法(Bundle Adjustment,BA)的代表性算法为ORB-SLAM系列[3].然而，随着vSLAM技术的发展和视觉定位技术在各个领域的使用，相机在特征退化场景中的使用也逐渐增加.传统的基于特征点的方法对这类环境缺乏良好的适应性，定位精度低，经常因为特征不足出现跟踪失败从而导致定位失败.因此为了应对缺乏纹理特征的环境，研究者提出直接使用整个图像进行跟踪的vSLAM技术，例如LSD-SLAM[51]、DSO[6]等.SVO[52]采用半直接法，即一种类似直接法的方式对特征点图像块进行跟踪.表3给出了典型vSLAM算法及其特点.

表3 典型vSLAM算法特点

此外，Attention-SLAM[53]把注意力模型引入到视觉SLAM框架中，将注意力更关注的部分的特征点权重设置得更高；TextSLAM[54]在视觉SLAM中提取文本的信息，并将文本上的特征点用平面进行约束；IVPR[55]通过结构线条进行快速的视觉位置识别.

1.4 激光雷达

激光雷达通过在一定角度范围内发射激光束并接受回传信号来获得其测量范围内的障碍物信息.基于其工作原理，使用激光雷达采集到的障碍物信息表现为一系列离散的，且具有准确角度和距离信息的点，这些点被称为点云.通常使用激光SLAM算法对激光雷达测得的点云数据进行处理，对不同时刻的两组点云数据进行对比从而获得传感器相对位姿变化及地图信息.

二维的激光SLAM有FastSLAM[56]、GMapping[57]、 Hector-SLAM[58]、 KartoSLAM[59]、Cartographer[60]等.FastSLAM[56]使用粒子滤波作为状态估计器，是最早能够实时输出栅格地图的激光SLAM方案.GMapping[57]也是基于粒子滤波但是为了采样更加精准，通过运动模型预测位姿后进行一次扫描匹配，且GMapping[57]通过限制重采样次数的策略来避免粒子耗散.Hector-SLAM[58]只有扫描匹配部分，没有后端，初值敏感.KartoSLAM[59]是首个基于图优化的激光SLAM方案，但其运行速度较慢.Cartographer[60]前端使用了子图的概念，将每帧激光雷达测量通过子图进行匹配，其后端具有回环检测模块.

三维的激光SLAM包括LiDAR Odometry and Mapping(LOAM)[61]、Lightweight and Ground-Optimized LiDAR Odometry and Mapping(LeGO-LOAM)[62]、Implicit Moving Least Squares SLAM(IMLS-SLAM)[63]、MULLS[64]等.

在LOAM[61]中特征点选择为边缘点和平面点，其中点云曲率是提取特征点的表征，曲率较大的为边缘点特征，曲率较小的为平面点特征.其得到的特征点云通过点到直线和点到平面的迭代最近点算法进行点云匹配.帧到帧之间的配准以高频运行，地图构建以低频运行，将若干帧点云扫描和局部的地图做精化配准，即可获得这些点云的精确位姿，然后把这些点云放入地图中.

LeGO-LOAM[62]在LOAM[61]的基础上增加了地面优化，并且整个系统更加轻量级.在特征提取前.LeGO-LOAM[62]先提取出地面点，并将除地面点之外的点进行分割，并去除噪声点(比如微小物体)得到分割点.在后面特征提取边缘点特征时，就不使用地面点而仅使用分割点，相比LOAM这样的特征提取更快速.在前端帧对帧配准部分，LeGO-LOAM[62]将LOAM[61]的一步优化六个自由度位姿改成了两步优化，即先优化z方向位移、俯仰角和横滚角，再优化平面的x，y坐标以及航向角.第一步优化利用了地面在相邻帧间基本保持不变的特性，所以可利用点到平面的约束计算垂直维度的位姿变化.将第一步优化的结果作为初值放入第二步优化，提升计算效率.

IMLS-SLAM[63]采用scan-to-model匹配方法，使用隐式移动最小二乘模型来表示地图.在分割阶段，通过删除聚类后体积小于一定尺寸的点云以达到移除动态物体的目的.在特征点提取上，IMLS-SLAM[63]通过设置9个指标来提取对位姿贡献大的点云作为特征点.

MULLS[64]通过多度量线性最小二乘实现了通用的激光SLAM.其前端通过双阈值滤波算法对地面点特征进行提取，对非地面点使用主成分分析法将特征点分成立面、屋顶面、柱、横梁、顶点，且对特征进行邻域分类编码.对于不同的分类，做点云配准时的权重也有不同的设定.后端做地图匹配时，先使用TEASER方法做初始匹配，然后和前端一样用MULLS-ICP做精匹配.

激光雷达在长廊或者低特征的场景中建图精度和一致性会降低，在场景中有行人、车辆等动态的障碍物时，也容易定位失败.另外，在不平坦的地面上，载体上下颠簸，会使激光雷达的精度迅速下降.

2 环境场景模型

2.1 场景分类

场景分类是一个计算机视觉领域中很经典的问题.基于图像的场景识别具有广泛的应用场景，从特征层面可以将基于图像的场景识别方法分为以下四类：基于底层特征的方法(比如SIFT特征描述子)、基于中层语义的方法(比如Li等[65]提出的视觉词袋的方法)、基于高层特征的方法(比如Objectbank方法[66])、基于学习特征的方法(即使用深度学习的方法).基于学习特征的方法的场景识别以MIT(麻省理工大学)的Zhou等[67]做的工作为代表，他们构造了Places365数据集，这是一个以场景识别结果为标签的数据集，总共有365个典型场景，每个场景标签的样本超过5 000个.

定位导航领域的场景识别更关注对定位导航有影响的要素.比如文献[68]提出通过生成测试区域的三维建筑模型来定义场景的城市化程度，利用三维建筑模型的天顶方向的平均掩模仰角来判断城市化程度，仰角越低城市化指数越低.另外还通过周围的车辆数定义交通情况.

定位导航领域对于场景的分类不局限于城市化程度和交通情况.本文认为，物理空间和环境变量组成了定位导航领域中场景分类的主要因素，具体分类如图 1所示.其中地面开放场景中，卫星接收无任何遮挡，但是一般来说此类场景特征离传感器距离较远，不利于SLAM.地面近开放环境会使卫星信号受到多路径效应影响，但此时特征也更靠近传感器.室内、地下、水下和深空环境一般无导航卫星信号，人造的室内环境结构特征较好，地下如矿井等环境光照较差，水下环境对卫星和SLAM都有巨大的挑战，一般依赖于高精度惯导，空中卫星导航信号较好但环境特征较少.环境变量包括光照环境、电磁环境、温度环境和运行环境.光照环境对视觉传感器、激光雷达会有比较大的影响，电磁环境会影响GNSS信号，温度环境对所有电磁器件都会带来一定的影响，运行环境主要包括动态干扰，可能对卫星导航接收机、视觉传感器、激光雷达等带来影响.

图1 场景分类主要因素：物理空间+环境变量Fig.1 Main factors of scene classification:physical space plus environment variables

2.2 场景自适应的传感器观测选择

对于不同的场景条件，应该选择合适的传感器观测组合进行多源融合定位.对于任何一个传感器观测，当其偏差较大时，就通过传感器选择算法对其进行剔除.对GNSS观测值进行剔除的操作被称作RAIM(Receiver Autonomous Integrity Monitoring)，即接收机自主完好性监测.GNSS以及惯导[69]、视觉[70]、激光雷达[18,71]都可以辅助进行RAIM.对于不良的视觉和激光雷达观测，通过动态物体判断[72-73]对质量低的观测进行剔除.

3 载体运动行为模型

3.1 非完整性约束

非完整性约束(Nonholonomic Constraint,NHC)是指车辆行驶过程中假定车辆不出现侧滑、漂移、弹跳等，车辆的侧向和垂向速度为零所构造出的虚拟观测.文献[11,78]阐述了非完整性约束的基本原理，构建了其量测模型.文献[79]提出在GPS信号中断情况下使用轮速计和NHC构成的三维速度辅助更新，在MEMS/GPS导航中显著改善了精度.除此之外，NHC在跟踪避障[80]、SFM(Structure From Motion)[81]等方面也有广泛的应用.在将NHC技术应用到实际场景时，其效果与观测噪声的设置有很大关联.由于NHC所做的假设在真实环境中未必严格成立，如在发生车辆侧滑或者拐弯时，侧向和垂向速度未必为零.因而，需要实时地改变约束的松紧程度.文献[82]研究了NHC的噪声设置的方法，这种自适应的NHC能较好地抑制GNSS中断期间组合导航系统的误差发散.作为对非完整约束模型的扩展，文献[83]对全向轮模型进行了详细的理论分析，将里程计转速和车轮转角相结合，构造轮式机器人的运动学约束，以约束惯性导航系统的快速漂移误差.对于NHC的引入带来的系统可观性的改变，文献[17]进行了分场景的讨论，指出NHC在所有运动状态下都可以增强对横滚的估计，而在加速或者转弯等剧烈运动下可以增强对偏航和俯仰的估计.

3.2 零速校正

零速校正(Zero-Velocity Update，ZUPT)[84]指以载体停车时的速度误差作为观测量，来对载体的其他信息进行修正[8].虽然零速校正是对速度进行观测，但它不仅可以修正速度误差，姿态误差和IMU传感器误差也可以得到修正，因而是高精度定位定向系统误差抑制与补偿的主要技术之一[85-87].传统的零速校正分为曲线拟合、滤波估计、平滑估计等[88]方法，文献[89]对几种常见的ZUPT方法进行仿真，实验证明卡尔曼滤波方法可以有效改善惯导的定位精度.由于零速校正的持续时间一般不长，卡尔曼滤波时往往误差收敛较慢，而且卡尔曼滤波的状态量较多，需要设置大量的噪声方差参数，再加上不可观测的偏航角，滤波容易发散.对此，文献[85,90]对零速校正提出了改进策略.除此以外，如何检测静止状态是准确地进行零速校正的前提，如果惯导系统在运动时被误检测为静止，会对导航精度造成恶劣的影响.静止状态检测方法可以分为4类：人工检测[91]、基于IMU检测[92-98]、基于GNSS检测[99-100]、基于视觉检测[15,98].在文献[91]中，系统一旦检测到GNSS中断期间的定位误差增加，就会让使用者手动执行零速更新.文献[93]利用一段时间的IMU数据均值与阈值比较判断静止状态，但如果零偏过大，这种方法会失效.文献[94-95]则是检查IMU数据的局部最小值，这种方法也会遗漏静止状态，并且没有扩展到常用的地面车辆模型中.文献[96]提出使用模式识别的一种方法建立特征识别静止状态.文献[95,97]从频域出发，证实了频域检测的鲁棒性，并且文献[97]更进一步地提高了频域方法对低加速度和低速运动的敏感性.最新的开源工作[98]则是利用卡方检测筛选静止状态.文献[99]利用高精度GNSS接收机的多普勒量测来进行室内的零速校正，文献[100]则是通过GNSS测速来确定静止状态.除此之外，文献[15]提出一种新颖的依据视觉信息进行零速校正的技术，在视觉惯性里程计中较好地抑制了滤波发散，而文献[98]也采用了类似的方法，并与IMU的检测方案进行了结合.

此外，载体静止时陀螺仪的三轴角速度输出为零的虚拟观测也可以修正运动状态，被称为零角速度校正(Zero Angular Rate Update).ZARU对陀螺仪的测量噪声进行修正，同时因为航向误差角和陀螺仪的测量噪声存在直接的联系，因此 ZARU同样也可以达到对航向误差角的修正作用.文献[92]构建了其与ZUPT的观测模型，都是作为静止时刻的约束信息.ZUPT和ZARU都被文献[101-102]应用到行人航位推算(Pedestrian Dead-Reckoning)中.文献[82]使用无迹卡尔曼滤波(Unscented Kalman Filtering,UKF)[103]构建ZUPT和ZARU的观测模型，进一步提高了方位估计.

3.3 其他载体运动行为模型

文献[104]提出一种将多旋翼无人机气动模型引入状态估计滤波器中的方法.该滤波器的状态中考虑了无人机转子转速变化时的阻尼系数.滤波器的观测包括了光流、加速度计、陀螺仪和磁力计.实验结果表明，该估计方法具有很强的鲁棒性，在弱光条件下能够较好地解决基于视觉的估计方法无法解决的问题，并且气动模型辅助速度估计器能够在低光照条件和很少纹理的具有挑战性的场景中工作，并为鲁棒飞行控制提供了可靠的反馈源.

VIMO[105]是第一种通过联合估计运动和外力来进行状态估计的方法.在VIMO中，动力学和外力同时被放入一个预积分的残差中，与视觉观测一起形成一个紧密耦合的滑动窗口估计器.需要注意的是，VIMO通过将未知外力建模为零均值的高斯白噪声，这一假设只在估计的外力接近零或只发生在短时间内成立.所以在无人机接受大的或持续的推力时，VIMO容易估计失败.

VID-FUSION[21]解决了VIMO将外力建模成零均值的高斯白噪声这一问题.通过引入转子转速测量单元的转子推力观测F1，同时将IMU测得的加速度转换成动力F2，VID-FUSION认为无人机所受外力是F1与F2的差值.

图2 基于滤波的3M-based融合定位导航算法总体框架Fig.2 Framework of 3M-based fusion positioning and navigation algorithm with filter

4 融合算法

4.1 滤波算法

基于滤波的3M-based融合定位导航算法总体框架如图2所示，核心是以惯导的误差方程作为滤波器的状态方式，其余的传感器观测值、环境场景和载体运动行为都通过滤波框架中的观测更新部分对惯导的系统漂移项进行估计，从而补偿.

基于滤波的融合算法主要包含扩展卡尔曼滤波(Extended Kalman Filter,EKF)[106]、无迹卡尔曼滤波、粒子滤波(Particle Filter,PF)[107].早年由Montemerlo等[56]提出的Fast-SLAM就是将Rao-Balckwellised粒子滤波器应用于机器人SLAM领域.对于一般的粒子滤波算法，高维空间的采样和搜索会增加计算量，粒子容易退化，导致滤波发散.当满足高斯分布模型时，扩展卡尔曼滤波更有效.按照传感器的耦合方式，滤波算法也可以分为松耦合和紧耦合，文献[38,108-109]提出了松耦合的VIO方案，其中IMU 和相机执行各自的状态估计.由于视觉定位方法被视为黑盒模块，不与IMU信息融合，在视觉定位困难的情况下不够鲁棒.紧耦合方案一般对姿态与路标点一起进行估计[50,110]，然而随着轨迹的行进与可观测到的路标点的增多，状态向量的维度会逐渐增大.为此，文献[111]采用对特征点分组的策略来平衡轨迹漂移和计算复杂之间的矛盾，ROVIO[112]对路标点进行了方向向量和距离的参数分解以实现更好的复杂度表现.另一类解决思路是以MSCKF[113]为代表的紧耦合VIO框架.MSCKF选择将有限长度窗口的相机位姿代替路标点加入状态向量，具有定位准确和轻量的巨大优势.基于该理论，文献[114]进一步扩展出双目MSCKF的框架.除此以外，文献[112]在前端进行了改进，引入图像块特征的提取与跟踪以及上述的路标点参数化，进一步提高系统的鲁棒性.StructVIO[115]通过MSCKF框架，融合了结构线条和IMU进行鲁棒的位姿估计.

文献[116-119]等研究主要集中在基于滤波的GNSS/INS/LiDAR融合算法上.Srinara等[116]基于NDT算法GNSS/INS/LiDAR进行了多传感器融合方案.Schütz等[117]将RTK、INS和LiDAR融合在研究中.但文献[30]中只使用了迭代最近点(ICP)[120]算法，导致LiDAR-Odometry随距离的快速漂移.为了克服快速漂移，Chiang等[118]利用基于网格的SLAM融合GNSS和INS.所有这些研究都使用了松耦合方法，从而更容易保持传感器之间的独立性.但是，当可用卫星数量少于四颗时，松耦合方法不能受益于GNSS观测.此外，文献[18]表明SLAM位姿的准确协方差难以估计，这导致SLAM与GNSS松散耦合时融合算法的一致性难以保持.文献[16]为基于扩展卡尔曼滤波的激光/惯性紧耦合系统.

图3 基于优化的3M-based融合定位导航算法总体因子框架Fig.3 The factor graph framework of 3M-based fusion positioning and navigation algorithm with optimization

4.2 优化算法

基于优化的3M-based融合定位导航算法总体因子框架如图 3所示，主线为IMU预积分，其余传感器观测、载体运动行为模型都作为观测因子接入.环境场景主要通过场景分类以及配合传感器选择算法将不同因子的权重进行自适应的调整.

VINS-Fusion[121]是一个通用的基于优化的框架，用于多个传感器，如GNSS、磁力计和气压计等和VIO的松耦合.Gong等[122]提出一种由VINS-Fusion改进的自适应融合GNSS和VIO的系统，利用IMU预积分的深度不确定度估计方法来评估VIO的状态不确定度.文献[123]提出一种基于VINS-Fusion的PPP和双目VIO的松耦合框架.DVIGO[124]基于直接稀疏法融合了四个互补但异步的传感器(立体相机、IMU、磁力计和GNSS)的测量结果.Liu等[125]提出一种基于优化的框架，将视觉、IMU和原始GNSS测量(包括伪距和多普勒频移)紧密耦合.在退化[126]的情况下，由于VIO的尺度漂移，文献[125]的初始化考虑了VIO的尺度参数.最近，文献[12]提出一个名为GVINS的系统，该系统使用了开源代码.与文献[125]相比，GVINS采用在线粗细化方法对GNSS视觉惯性态进行初始化.此外，GVINS还考虑了时间同步、电磁干扰和接收机时钟跳变等工程难题.在GVINS的基础上，P3-VINS[19]增加了相位观测信息到观测因子中，并在状态中增加了载波相位模糊度，同时采取无电离层组合的方式.

Shan等[13]提出LIO-SAM融合卫星导航/惯性导航/激光雷达.在LIO-SAM前端，激光雷达和惯导通过预积分紧密耦合，后端采用全局因子图对卫星导航进行松散耦合.在LIO-SAM中，采用robot_localization[127]来获取大地坐标系与局部坐标系的转换.但是robot_localization算法只使用了陀螺仪而未使用加速度计，因此，LIO-SAM中初始化的精度是有限的.Shan等[14]在LIO-SAM的基础上增加了视觉传感器，提出场景适应性更强的LVI-SAM算法.

5 总结与展望

本文从传感器观测模型、环境场景模型、载体运动行为模型三方面综述了基于多源融合定位算法.本文主要关注的传感器为卫星导航、惯性导航、视觉传感器和激光雷达.环境场景模型主要涉及场景分类和基于场景分类的传感器选择.载体运动行为模型本文主要关注了非完整性约束、零速校正以及一些无人机动力学等.最后，本文从滤波和优化两个方面对涉及卫星导航、惯性导航、视觉传感器和激光雷达的多源融合定位算法进行了综述.未来多源融合定位算法也将从更多源的传感器、更加智能的环境场景模型辅助和载体运动行为辅助以及更优的融合框架这几个方面发展.