本文内容来源于《测绘学报》2022年第1期（审图号GS（2022）104号）
基于抗差LM的视觉惯性里程计与伪卫星混合高精度室内定位
杨高朝1,2
, 王庆1,2
, 蔚保国3, 刘鹏飞1,2,李爽3
1. 东南大学仪器科学与工程学院, 江苏 南京 210096;
2. 东南大学智慧城市研究院, 江苏 南京 210096;
3. 卫星导航系统与装备技术国家重点实验室, 河北 石家庄 050050
基金项目：国家重点研发计划（2020YFD110011-01）
摘要：视觉惯性里程计（VIO）和伪卫星已经广泛应用于室内环境定位中，但在实际应用中，二者各自都有明显的缺陷。视觉里程计依赖于实际场景，在景深变化明显和光照不均匀的环境下会产生粗差，而且误差会不可避免地随着时间累积，但是在相邻帧间能保证相对高精度的位姿测量。由于受到室内多径的影响，伪卫星室内定位的精度和可靠性很难保证。为增加室内定位的可靠性和稳定性，基于抗差LM非线性优化理论，本文主要研究利用视觉惯性里程计的相邻帧间高精度位姿测量和伪卫星融合的室内高精度定位技术。该算法不仅可以抵抗粗差，而且可以减弱不同传感器间权重设置不合理带来的影响。最后使用在室内环境下搭建的高精度动态捕捉设备对组合定位方法进行实验验证。试验结果表明，该方法不依赖回环即可消除视觉惯性里程计的累积误差，有效提高室内定位精度及可靠性。利用改进的LM算法融合后场景1和场景2，相对于VIO单独定位精度分别提高了59.0%和77.5%。
关键词：视觉惯性里程计 伪卫星 LM 抗差估计 室内定位
引文格式：杨高朝, 王庆, 蔚保国, 等. 基于抗差LM的视觉惯性里程计与伪卫星混合高精度室内定位[J]. 测绘学报，2022，51(1)：18-30. DOI: 10.11947/j.AGCS.2022.20200251
YANG Gaochao, WANG Qing, YU Baoguo, et al. High-precision indoor positioning based on robust LM visual inertial odometer and pseudosatellite[J]. Acta Geodaetica et Cartographica Sinica, 2022, 51(1): 18-30. DOI: 10.11947/j.AGCS.2022.20200251
阅读全文：http://xb.sinomaps.com/article/2022/1001-1595/2022-1-18.htm
引 言
视觉导航定位是通过视觉系统在相机运动过程中提取不同的图像，通过检测这些不同图像的变化，提取并且匹配相同的特征点，判断特征点的运动变化来估计相机的运动情况[1-3]。视觉里程计(VO)由于在面对单纯的旋转时无法很好地跟踪，因此在实际应用中通常会与低廉的IMU等惯性传感器进行组合，在组合后视觉惯性里程计(VIO)能够稳健地应用[4-5]。VIO虽然在光照条件、图像质量良好的情况下具有很高的定位精度，但在没有其他信息的情况下由于缺少全局的位置信息参考，其实质是一种局部定位算法，存在累积误差的问题[6]。因此，目前有许多研究通过设计全局的路标或者利用其他全局的信息予以补充，提高视觉定位在长距离范围的适用性。
VIO无法避免累积误差。GNSS是一种全局观测的传感器，每一次观测都是独立的，因此误差并不会累积，但是导航型GNSS定位输出频率和定位精度较低，通常无法满足用户更高的需求。从理论上看，将一个局部精度很高但存在累积误差的VIO和一个局部精度无法保证到不存在累积误差的GNSS相结合，可以互相弥补各自的不足[7]。
由于视觉、IMU等传感器无法获得全局信息，因此被定义为局部传感器。而对于GNSS、磁力计、UWB等能够感知全局信息的传感器，被定义为全局传感器。对于局部传感器，首先进行局部位姿估计，即传统意义上的VO或者VIO。得到局部的位姿估计之后，再与GNSS等全局传感器进行对齐。对齐的方式是建立一个位姿图，每个GNSS时刻建立一个位姿节点，连续的两个节点之间将局部位姿估计得到的相对位姿作为约束。每个节点还与GNSS等全局位置建立约束[8]。
文献[9-10]提出基于滤波的多传感器松耦合方法。主要思想是把IMU作为主传感器，通过积分得到6自由度的位姿。VO/VIO作为相对位姿的估计器，GNSS等作为全局位姿估计器，与IMU积分得到的结果进行EKF，得到更加准确的位置估计。IMU积分受到偏差和噪声的影响，会很快地发散。而VO可以认为在局部范围内误差很小，因此可以修正IMU的偏差。而GNSS不存在累积误差的问题，又可以修正IMU和VO/VIO的累积误差。GNSS的观测方程比较简单，即将待估计的位姿通过外参转化到GNSS坐标系的位姿，因为GNSS无法测量旋转，所以直接取三维位置的差作为观测误差。由于VO的尺度漂移是不稳定的，因此直接把VO的位姿放在全局坐标系下建立观测方程去估计尺度是不够准确的，因此，笔者提出了把VO的pose作为两帧之间相对的位姿去建立观测方程。
文献[11-12]提出基于优化的方式，支持双目配置、多轨迹融合以及GNSS等全局传感器与VO进行联合优化。
文献[11]主要思想是，首先进行局部位姿估计，即传统意义上的VO或者VIO，得到局部的位姿估计之后，再与GNSS等全局传感器进行对齐。对齐的方式是建立一个位姿图，每个GNSS时刻建立一个位姿节点，连续的两个节点之间将局部位姿估计得到的相对位姿作为约束。每个节点还与GNSS等全局位置建立约束。
文献[12]总体思想与文献[11]类似。但细节和试验上更加完备一些。笔者认为，GNSS无法提供旋转的约束，因此基于这种结构优化后的旋转的精度会变差。因此提出了第二种因子图结构。其中GNSS部分的约束保持不变。但相对位姿只利用VO/VIO的平移部分建立节点和节点之间的约束。而旋转则通过建立一个虚拟的局部坐标系，通过计算局部坐标系和全局坐标系(GNSS坐标系)之间的变换，建局部坐标系下的旋转转换成全局坐标系下的旋转，然后建立全局的旋转约束。在初始化阶段，通过直接对齐VO/VIO位姿和GNSS，解SVD来求解。在进行优化之前，笔者把VO/VIO的位姿与IMU进行一次松耦合的滤波。尽量减少与GNSS之间的时延带来的误差。
由于在室内无法接收到GNSS信号，因此在室内就无法利用GNSS消除VIO的累积误差。伪卫星又称“地面卫星”，是从地面某特定地点发射类似于GNSS的导航信号，采用的电文格式与GNSS基本一致。伪卫星系统作为GNSS定位系统的辅助手段和工具，即可以用来辅助增强GNSS在某些恶劣环境下的定位性能，也可以单独使用构建伪卫星定位系统。因此，在室内，可以利用伪卫星与视觉VIO进行组合消除VIO的误差累积[13]。但是伪卫星在室内应用中也面临着两大挑战。
伪卫星钟差的影响。由于GNSS中卫星钟差采用的是原子钟，精度较高，因此，在一般精度定位中可以忽略不计，但是原子钟价格昂贵，室内伪卫星采用不合适。由于伪卫星钟差的影响，单接收机无法完成定位，必须采用站间和星间双差技术，这在实际应用中非常不便。为了解决伪卫星钟差的问题，目前很多研究机构都是采用阵列天线来解决这个问题。阵列天线中所有的伪卫星钟差都是一样的，因此通过星间单差技术可以同时消去接收机钟差和伪卫星钟差[14-16]。
室内多径的影响。GNSS一般应用于室外定位，多径一般都忽略不计。但是室内结构复杂，多径对伪距的影响往往能达到几十米甚至几百米。而伪卫星通常是静止于室内的，无法通过多历元平滑来减弱多径。目前伪卫星多径解决方法主要分为3类：基带解调端、接收机端及数据处理端。因此，在实际室内应用中只利用载波观测值。由于只利用载波观测值会导致秩亏问题(N颗卫星至少需要解算(N 3)个参数)，为了解决这个问题，在动态应用前一般需要在已知位置上进行初始化一分钟[17-19]。
多传感器融合算法一般包括基于滤波和基于非线性的融合方式。基于滤波的算法有卡尔曼滤波(包括一些延伸的算法)、无迹卡尔曼滤波(UKF)及粒子滤波(PF)等，基于非线性优化的算法有最速下降法、高斯-牛顿及LM(Levenberg-Marquard)算法等[20-24]。很多文献表明，由于基于非线性优化的算法可以同时优化多个时间段的数据，因此基于非线性优化的算法要优于基于滤波的算法。本文主要研究基于非线性优化的融合算法。最速下降法无法确定收敛步长，步长较小会导致收敛较慢，步长较大会导致算法发散。高斯牛顿法采用二阶泰勒展开来近似，只有在展开点附近才会有比较好的近似效果，如果Hessian矩阵是非奇异矩阵，可能会导致算法发散。LM算法能够很好地克服上面的缺点，已经广泛应用于很多非线性优化实例中，但是LM算法还有自身的缺陷。尽管LM算法能够克服Hessian矩阵病态解的影响，由于LM算法采用最小二乘估计作为收敛条件从而不能抵御传感器数据中心粗差的干扰，特别是室内伪卫星定位时，由于室内多径比较严重，很难保证伪卫星定位的稳定性和可靠性。若直接使用LM算法进行优化，则会使得求解出的融合解偏离真实值。鉴于VIO和伪卫星单独定位的缺陷，本文主要研究应用抗差LM算法进行VIO和伪卫星融合的问题。
1 多源数据融合约束方程的建
立
1.1 数据内插
多传感器的融合时间戳对齐是非常关键的一步。时间戳无法对齐或对齐错误最后可能得出一个错误的轨迹，很可能使优化算法失效。为了保证融合算法的稳健性，视觉传感器的时间戳与伪卫星的时间戳相差不超过10ms。
一般伪卫星数据的输出频率为1~5Hz，而视觉VIO数据一般可以达到10~20Hz，为了更方便进行后端融合，本文首先根据视觉输出的频率及相应的时间戳对伪卫星输出数据进行内插。由于两个伪卫星输出数据间隔一般只有3~4个视觉输出数据，考虑到内插平滑的精度，本文利用三次样条曲线内插[25]，假设已知的n个坐标点(S(ti),ti)，…，(S(ti n),ti n)，其中S(ti)为ti时间戳对应的三维坐标值x(ti)，y(ti)和z(ti)，本文可以列出样条曲线的微分式。如果函数S(ti)满足以下3个条件(本文为了方便计算，把xi,yi,zi根据ti分开内插求解)：
(1) S(ti)=x(ti),i=(1, 2, …,n)。
(2) S(ti)在每个区间上ti,ti 1(1, 2, …,n-1)是一个三次多项式。
(3) S(ti)在整个区间上t1,tn有连续的一阶及二阶导数。
则称S(ti)为过n个点的三次样条函数。三次样条曲线内插是通过一系列形值点的一条光滑曲线，数学上通过求解三弯矩方程组得出曲线函数组的过程。实际计算时还需要引入边界条件才能完成计算。假设方程为
(1)
式中，ai、bi、ci、di待定。根据二阶微分连续性，可得
由文献[25-26]可知，可列出(n-1)个方程，但是有(n 1)个参数，因此求解该方程组，还需添加其他约束。根据文献[27]，本文选择“自由边界”约束，即：
1.2 多传感器约束方程的建立
为了实时进行伪卫星和VIO的融合，本文选择滑窗融合模式，窗口长度为10。伪卫星坐标系采用室内局部坐标系。为防止秩亏现象发生，首先把融合后的地图坐标系第一帧与伪卫星坐标系对齐。
由于伪卫星不能求出旋转向量，因此在优化的时候只能利用坐标向量和速度向量。总的融合约束方程可以写为
(2)
式中, rC和rS分别是VIO和伪卫星测量残差；pi,Ci 1和qi,Ci 1分别是VIO的平移和旋转测量值；pi,S是伪卫星测量值；χ是需要优化的参数。
式中，pi,G、Vi,G和qi,G是优化后的全局平移、速度和旋转参数；bi,g和bi,a分别是陀螺仪和加速度计偏置；i表示滑动窗口中帧数；C、S和G代表视觉世界、伪卫星和融合后全局坐标系标志。
由于pi,CG、qi,CG和视觉VIO密切相关，而视觉VIO会随着时间有误差累积，因此,pi,CG和qi,CG也会有误差累积。为了使优化后的位姿更加平滑，本次研究选择实时优化这两个向量。
1.2.1 VIO约束
由于VIO的尺度漂移是不稳定的，直接把VIO的位姿放在全局坐标系下建立观测方程去估计尺度是不够准确的，因此，把VIO的两帧之间相对的位姿去建立观测方程
视觉目标函数可写为
(7)
根据经验值选取权值初值PLi,Cq=0.1、PLi,Cp=0.01和PLi,CV=0.01[9]；θ是旋转矩阵的欧拉角；i是滑窗内数据索引(i=1, 2，…10)；pi,C、Vi,C和qi,C是VIO位置、速度和旋转向量；pi,C、pi,s、qi,C和qi,s为后端输出数据；pi,Ci 1、Vi,Ci 1和qi,Ci 1分别为VIO相邻两帧间的平移、速度和旋转变化量。VIO相对于各参数雅克比向量见参考文献[11]。
1.2.2 伪卫星约束
伪卫星约束计算公式为
(8)
式中，PLi,Sp和PLi,SV为伪卫星的定位和测速的精度因子；ωGCC为陀螺仪测量出的视觉相对于伪卫星全局坐标系的角速率；LCGC为伪卫星接收机天线相对于载体坐标系的平移向量；(·)×表示反对称矩阵；Vi,S可以根据瞬时差分多普勒求得。因为伪卫星无法求出旋转角度，因此只利用位置和速度信息进行约束。δpi,S关于pi,G，θi,G，Vi,G，bi,g和bi,a的雅克比矩阵为
(9)
δVi,S关于pi,G、θi,G、Vi,G、bi,g和bi,a的雅克比矩阵为
(10)
1.2.3 基于扩展卡尔曼滤波的全局坐标初始化
pk,G和qk,G为全局坐标系下的平移和旋转矩阵。由于伪卫星只能提供全局位置和速度约束，因此若想求得当前坐标系相对全局坐标系下的旋转矩阵，至少需要提供4个历元的位置约束。假设滑窗宽度为10，首先利用扩展卡尔曼滤波初始化全局坐标。
状态预测方程为
(11)
状态更新方程为
(12)
当i=1时
假设i是当前帧，根据pi,C、qi,C、pi,G及qi,G，可以求得视觉世界坐标系到全局坐标系的旋转和平移矩阵pi,CG和qi,CG为
由于VIO随着时间误差会累积，因此，为了更加准确地求取视觉世界坐标系到全局坐标系的转换，在每次优化完成后重新求取pi,CG和qi,CG。
1.3 有效性检核
为增加定位的稳健性，减少粗差给融合定位带来影响，本文使用下列方案检验数据的有效性。
已知第i 1帧相对视觉世界坐标系(视觉初始化时的参考坐标系)下的平移量pi 1,C，又已知第i帧视觉世界坐标系向全局坐标系的旋转矩阵qi,CG和平移矩阵pi,CG，可以得到第i 1帧全局坐标系下的坐标估值为
假设伪卫星在i 1时刻的坐标为pi 1,S，方差为σ2，可以建立检核方程为
2 改进的抗差LM非线性优化的融合策略
已知LM非线性优化模型为
式中，
因此，LM算法可以看作给变化量Δx添加一个信赖区域来限制Δx的大小，并认为在信赖区域里面近似是有效的，否则近似不准确。
确定信赖区域一个好的办法是通过比较近似模型和实际模型的差异来确定，如果差异小，就增大范围；如果差异太大，就缩小范围。考虑实际模型和近似模型变化量的比值
式(23)可以通过ρ的值来判断泰勒近似的好坏，其中分子是实际模型的变化量，分母是近似模型的变化量。当ρ接近1的时候，表明近似模型较好；如果ρ较小，则实际模型的变化量小于近似模型的变化量，则认为近似模型较差，需要缩小近似范围。反之，当ρ较大时，说明实际模型变化量更大，需要放大近似范围。经典LM算法可以表示如下：
(1) 给定初始迭代值x0(坐标向量初值可由伪卫星给出，旋转向量初值由视觉VIO提供)及调节因子μ0：μ0=max([hii])，其中hii是H0的对角线元素。
(2) 对于第k次迭代，求解优化问题。
(3) 计算ρ。如果ρ>0，则认为近似可行，令xk 1=xk Δxk；若ρ>4/5, 则μk 1=2μk；若ρ>1/5，则μk 1=0.5μk。
(4) 判断算法是否收敛。如果不收敛，跳回步骤(2)，否则结束。
当μk较小时，说明Hk占主导地位，说明二次近似在该范围内是比较好的，LM方法更接近于高斯牛顿法；另一方面，当μk较大时，LM算法更接近一阶梯度下降算法，这说明二次近似不够好。LM算法的求解方式，可以避免线性方程组的矩阵非奇异和病态等问题，提供更稳定、更准确的解法。
在经典LM算法中，从信赖域法角度分析，μk可以看作是每次迭代约束区域的半径，其实是一个球形的区域，该约束认为只有在球内才是有效的。
diag[Hk]指只保留Hk的对角线元素。带上矩阵diag[Hk]后约束区域就是一个椭球，可以约束参数中不同维度的变化。
尽管LM能够克服Hessian矩阵病态解的影响，由于LM算法采用最小二乘估计作为收敛条件从而不能抵御观测数据中心粗差的干扰，特别是室内伪卫星定位时，由于室内多径比较严重，即使伪卫星原始观测数据能够通过上文的有效性检核，仍旧很难保证伪卫星定位的稳定性和可靠性。若直接使用LM算法进行优化，则会使得求解出的融合解偏离真实值。
为了抵消观测值粗差对模型解的影响，可以采用抗差估计来减弱粗差对估值的影响，得到LM算法的抗差解。把式(7)和式(8)中的权P换为等价权P，再计算
采用IGGⅢ等价权函数计算P，令
(25)
式中，p是P的一个元素；
本次研究的多传感器融合具体流程如图 1所示，本文主要研究基于抗差LM优化的VIO与伪卫星后端松耦合技术。下面主要通过试验对比来分析融合的优势。
图 1 多传感器融合流程 Fig. 1 Flowchart of multi-sensor fusion
图选项
3 试验验证及分析
3.1 试验验证
为了验证本文提出的多传感器融合算法，本文利用笔者团队搭建的组合设备在某所C7试验场进行测试。图 2(a)多传感器融合实时测试设备；图 2(b)是试验场景图，图 2(a)中的箭头所指的区域为伪卫星信号发射系统，试验场地一共有8颗伪卫星，为了消除伪卫星钟差，使用了阵列天线，由图 2可以看出，伪卫星结构分布较差，且都分布在房屋顶部；图 2(b)中的箭头所指的区域为光电捕捉系统设备，试验场所二楼一周都布满了该设备，长和宽大约都是20m。本研究利用光电捕捉设备测量的数据作为真值进行参考，视觉定位部分采用小觅双目相机标准版。为了验证本文提出的融合算法，使用搭建的测试设备分别采集两组场景数据(长度分别为41m和271m)进行对比分析。绝对轨迹误差(ATE)直接计算相机位姿的真实值与系统的估计值之间的差，首先根据位姿的时间戳将真实值和估计值进行对齐，然后计算每对位姿之间的差值，该标准适合于估计系统的漂移。
图 2 多传感器融合设备和伪卫星分布 Fig. 2 Multi-sensor fusion equipment and pseudosatellite map
图选项
3.2 结果分析
3.2.1 伪卫星定位性能分析
PDOP(position ditution of precision)大小能够反映伪卫星结构的好坏，PDOP越小，结构越好。图 3是室内试验场地伪卫星PDOP分布。可以看出，越往四周PDOP越差，中间区域结构最好。图 4(a)和4(b)是场景1伪卫星定位轨迹对比图和定位残差图；图 5(a)和5(b)是场景2伪卫星定位轨迹对比图和定位残差图，真值为光电捕捉数据。伪卫星实时坐标解算采用星间单差的模式，由于在伪卫星发射系统中采用了阵列天线，因此，星间单差能够同时消除伪卫星钟差和接收机钟差。由图 4可以看出，尽管伪卫星数据不平滑，噪声较大，但残差较为均匀，基本都在0.5m之内。由图 4和图 5对比可知，由于场景2测试范围较大，从图 3可知PDOP因子较差，因此场景2的伪卫星定位精度比场景1定位精度差。
图 3 室内伪卫星PDOP分布图 Fig. 3 PDOP distribution map of indoor pseudosatellite
图选项
图 4 场景1中伪卫星定位效果 Fig. 4 Pseudosatellite positioning result in scene 1
图选项
图 5 场景2中伪卫星定位效果 Fig. 5 Pseudosatellite positioning result in scene 2
图选项
3.2.2 视觉定位性能分析
图 6是视觉VIO试验场景图。图 7(a)和7(b)分别是场景1的VIO定位轨迹对比图和VIO定位残差图；图 8(a)和8(b)分别是场景2的VIO定位轨迹对比图和VIO定位残差图。由图 7和图 8以看出，虽然VIO数据较为平滑，但RMS不均匀，波动较大，特别是场景2，最大误差达到2.5m。
图 6 VIO试验场景 Fig. 6 VIO experimental scene diagram
图选项
图 7 场景1中VIO定位效果 Fig. 7 VIO positioning result in scene 1
图选项
图 8 场景2中VIO定位效果 Fig. 8 VIO positioning result in scene 2
图选项
视觉里程计定位性能受环境因素和运动状态影响比较大。在强光环境下或者运动状态剧烈的时候会导致误差累积越来越大，甚至会直接使里程计挂掉。
由图 4-图 8可知，尽管伪卫星实时定位噪声较大，但是误差不会像视觉里程计那样有累积，绝对误差的波动幅度会保持在一个相对大小的值。视觉误差尽管会随着距离的增加误差会渐渐累积，但是在短时间内会保持在一个极小的值。因此，二者可以实现优势互补。下文就二者的组合定位性能进行分析。
3.2.3 组合定位性能分析
本文主要研究应用抗差LM算法进行VIO和伪卫星融合的问题。为了对比本文算法的性能，下文主要利用经典的LM算法优化后的效果与本文算法优化后的效果及进行对比分析。
图 9和图 10是视觉世界坐标系与全局坐标系的旋转和平移图，可以通过式(26)求解
图 9 场景1中视觉世界坐标系与全局坐标系的平移和旋转 Fig. 9 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 1
图选项
图 10 场景2中视觉世界坐标系与全局坐标系的平移和旋转 Fig. 10 The translation and rotation of the visual world coordinate system and the global coordinate system in scene 2
图选项
(26)
式中，pi,G和qi,G分别是优化后全局坐标系下的平移和旋转矩阵；pi,C和qi,C是利用视觉惯性里程计输出的在视觉世界坐标系下的平移和旋转矩阵；i指当前最新帧。求解qi,CG和pi,CG的目的是为i 1帧全局坐标系旋转和平移矩阵提供一个初值。
由于两个坐标系都属于参考坐标系，原则上两个坐标系的平移和旋转矩阵应该是固定的。但由于VIO存在误差累积的因素，不同时刻两个坐标系的旋转和平移是发生变化的，每个时间段的视觉世界坐标系和全局坐标系之间的旋转和平移矩阵也会发生变化，因此视觉世界坐标系和全局坐标系之间的平移向量的波动较大并不完全等同于系统融合后的定位残差。由于在测试场地非常整平，因此平移转换时暂不考虑高程的变化。由图 9和图 10可知，全局与视觉坐标系之间平移向量X和Y方向的波动几乎在2m左右。图 9和图 10下半部分是3个旋转欧拉角变化图。由图中可知，除了yaw波动较大外，pitch角和roll角相对都比较稳定。这是由于pitch和roll可以通过校准后的重力方向进行纠正，方向是可观的，而yaw方向无法纠正，因此，yaw角会随着时间误差累积越来越大。
图 11是场景1中经典LM融合定位效果，图 12是场景1中抗差LM融合后定位效果，图 13是场景2中经典LM融合后定位效果，图 14是场景2中抗差LM融合后定位效果。“RMS-xy”代表平面的绝对误差大小。由图 11和图 12可知，经典的LM算法融合后，X和Y残差的最大值都达到了0.2m，而利用改进的LM算法融合后最大只有0.15m。表 1是场景1和场景2关于4种定位方案的RMS对比，表中最后两列分别表示经过LM融合后平面方向精度相对于伪卫星和VIO提高的百分比(计算方式为：百分比=(A-B)/A，其中，A为改进前的RMSE，B为改进后的RMSE)。由表 1可知利用改进的LM算法融合后场景1相对于伪卫星和VIO定位精度分别提高了35.5%和59.0%；场景2相对于伪卫星和VIO定位精度分别提高了27.8%和77.5%。通过对比场景1和场景2相对于VIO精度的增长率可知：轨迹越长，VIO的误差累积越大，组合系统相对于单系统VIO的优势越明显。组合系统能有效消除VIO的误差累积。
图 11 场景1中经典LM融合定位效果 Fig. 11 Positioning result after classic LM fusion in scene 1
图选项
图 12 场景1中抗差LM融合后定位效果 Fig. 12 Positioning result after classic LM fusion in scene 1
图选项
图 13 场景2中经典LM融合后定位效果 Fig. 13 Positioning result after classic LM fusion in scene 2
图选项
图 14 场景2中抗差LM融合后定位效果 Fig. 14 Positioning result after robust LM fusion in scene 2
图选项
表 1 不同传感器组合方案平均定位精度对比 Tab. 1 Comparison of average positioning accuracy of different sensor combination schemes
| line | type | RMSE/m | length/m | increase/(%) | increase/(%) |
| 场景1 | Pseudolite | 0.124 | 41.7 | - | - |
| VIO | 0.195 | - | - | ||
| Classic-LM | 0.091 | 26.6 | 53.9 | ||
| 本文算法 | 0.080 | 35.5 | 59.0 | ||
| 场景2 | Pseudolite | 0.363 | 271.4 | - | - |
| VIO | 1.166 | - | - | ||
| Classic-LM | 0.327 | 9.9 | 71.9 | ||
| 本文算法 | 0.262 | 27.8 | 77.5 |
表选项
3.2.4 遮挡下组合定位分析
由于VIO是VO与IMU采用紧组合建模，视觉在短时间内具备较高的相对定位精度，因此，IMU的加速度计偏差和陀螺仪偏差在短时间内能够通过VO进行修正，因此，VIO在短时间内能够得到较为平滑的高精度定位数据。为了验证伪卫星在遮挡下组合系统中定位系统定位的性能，本试验场景1在12~17s及29~34s两个时段给伪卫星设置全遮挡；场景2在35~65s及85~135s两个时段给伪卫星设置全遮挡；在135~185s时间段给伪卫星设置半遮挡(即在试验中有1~3颗卫星被遮挡。由于伪卫星发射器利用了阵列天线，利用星间单差就可以基本消除伪卫星钟差和接收机钟差，因此4颗卫星就可以完成定位)。
图 15(a)和图 16(a)是伪卫星存在短时间遮挡下与视觉VIO组合定位轨迹图，图 15(b)是场景1定位残差图，16(b)是场景2可用伪卫星数及定位残差图。由图 15(b)可以看出，场景1的最大残差不超过0.2m，除了在30s和50s处有较小的波动外，其余部分都非常稳定；场景2的残差波动较大，但整体都在1m之内。场景2的遮挡分为全遮挡和半遮挡。由图中可以看出，当可用伪卫星个数满足定位要求时，组合定位性能基本不受影响。结合图 2和图 3通过分析可知，由于伪卫星都分布在试验场顶部，而且伪卫星都是静止不动的，PDOP较差，因此在满足定位要求后，伪卫星的个数对定位结果影响较小。可知，即使伪卫星在短暂遮挡下，基本不影响组合系统定位性能，组合系统依然能够输出较高精度定位数据。
图 15 场景1中VIO遮挡下定位效果图 Fig. 15 Positioning result under VIO occlusion in scene 1
图选项
图 16 场景2中VIO遮挡下定位效果图 Fig. 16 Positioning result under VIO occlusion in scene 2
图选项
图 17分别是不同频率(5、1和0.2Hz)的伪卫星输出结果与视觉惯性里程计组合后的定位轨迹和定位残差图，平均定位残差分别是0.254、0.278、0.312m。经过分析可知，伪卫星的频率越高，内插精度也就越高，因此能更精确地修正陀螺仪和加速度计的偏差，组合后的精度也就更高。由于在室内多径现象比较严重，如果采集频率过高，信号可能会跟踪失败，而且5Hz组合后的结果相对于1Hz和0.2Hz提高并不多，因此在实际组合定位中可以选择1Hz或者更低的频率。
图 17 不同伪卫星频率组合后定位效果图 Fig. 17 Positioning result after combining different pseudosatellite frequencies
图选项
4 结 论
本文提出了改进的LM算法，并把改进的LM算法用于视觉VIO和伪卫星融合模型优化中。首先，分别测试了视觉VIO和伪卫星定位的精度。然后，把本文改进的LM算法与经典的LM算法进行对比。通过实例数据分析，场景1中利用本文改进的抗差LM算法融合后相对于伪卫星和视觉VIO定位精度分别提高了35.5%和59.0%；场景2中利用本文改进的抗差LM算法融合后相对于伪卫星和视觉VIO定位精度分别提高了27.8%和77.5%。
本文测试了伪卫星在短暂遮挡下组合系统定位性能，通过试验可以得出，即使伪卫星在遮挡情况下，组合系统定位性能基本不受影响，场景1和场景2的平均定位残差都在0.3m以内，依然能保持定位的可靠性和连续性。最后，测试了不同频率(5、1和0.2Hz)的伪卫星输出结果与视觉惯性里程计组合后的定位性能。通过分析可知，尽管频率越高，组合后定位精度越高，但是精度提高幅度较小，因此在实际组合定位中为了避免信号跟踪失败可以选择1Hz或者更低的频率。
作者简介
第一作者简介：杨高朝(1987—), 男, 博士生, 研究方向为多传感器信息融合导航定位。E-mail: 230189279@seu.edu.cn
通信作者：王庆, E-mail：wq_seu@seu.edu.cn
初审：张艳玲
复审：宋启凡
终审：金 君
往期推荐
资讯
○ 中国大学MOOC 地理信息系统（GIS）实验第六期课程即将开课！
○ 测绘遥感国家重点实验室马盈盈副研究员利用遥感气象要素助力冬奥
○ 山东省水利勘测设计院有限公司2022年公开招聘！含测绘、地信、遥感相关专业
○ 自然资源部：这11家导航电子地图制作甲级测绘资质单位复审换证通过
会议
○ Geoinformatics 2022 暨CPGIS成立30周年年会（二号通知）
○ 会议通知 | 第三届IEEE地质、测绘与遥感国际学术会议
○ 关于召开大地测量与导航2022年综合学术年会的通知（第一轮）
○ 诚邀莅临丨“新测绘 新融合 新跨越”南方测绘2022用户大会
《测绘学报》
○测绘学报 | 刘万科：非完整约束的OD/SINS自适应组合导航方法
○ 测绘学报 | 李德仁：论万物互联时代的地球空间信息学
○ 高被引论文 | 蓝朝桢：异源遥感影像特征匹配的深度学习算法
○ 高被引论文 | 张永军：天空地多源遥感数据的广义摄影测量学
《测绘通报》
○《测绘通报》2021年第12期目录
○ 《测绘通报》2022年第1期目录
○ 地市级实景三维城市建设及应用
○ 图残差神经网络支持下的建筑物群组模式分类
《北京测绘》
○《北京测绘》2022年第1期摘要推荐
○《北京测绘》2021年第12期摘要推荐
○《北京测绘》2021年第11期摘要推荐
○《北京测绘》2021年第10期摘要推荐
《测绘科学技术学报》
○ 摘要 |《测绘科学技术学报》2021年第2期摘要推荐
○ 摘要 |《测绘科学技术学报》2021年第4期摘要推荐
○ 摘要 |《测绘科学技术学报》2021年第5期摘要推荐
○ 摘要 |《测绘科学技术学报》2021年第6期摘要推荐
《地球信息科学学报》
○ 《地球信息科学学报》2022年第1期佳文推介
○ 龚健雅院士：全球位置信息叠加协议与位置服务网技术研究进展与展望
○ 佳文推介 | 大数据城市通勤交通模型的构建与模拟应用
○ 专刊征稿：社会感知与地理大数据挖掘（征稿中）
《测绘工程》
○ 摘要 |《测绘工程》2021年第5期摘要推荐
○ 摘要 |《测绘工程》2021年第4期摘要推荐
○ 摘要 |《测绘工程》2021年第6期摘要推荐
○ 佳文推介 | 单目视觉技术在室内定位中的应用研究
《中国空间科学技术》
○ 火卫二地形地貌探测综述
○《中国空间科学技术》2022年第1期摘要
○《中国空间科学技术》2021年第6期摘要
○《中国空间科学技术》2021年第5期摘要推荐
《卫星应用》
○《卫星应用》2021年第12期摘要○《卫星应用》2021年第11期摘要○《卫星应用》2021年第10期摘要○ 摘要 |《卫星应用》2021年第9期摘要推荐《Journal of Geodesy and Geoinformation Science》
○《测绘学报（英文版）》专刊征稿 | 地图学与地球空间信息教育：理论与实践
○ 《测绘学报（英文版）》专刊征稿 | 用于三维地理信息的摄影测量和计算机视觉
○ Kexian WANG et al. | 《测绘学报（英文版）》（JGGS）精选论文
○ Huan XU et al. | 《测绘学报（英文版）》（JGGS）精选论文
《Satellite Navigation》
○ 徐元博士：面向有色测量噪声下UWB/INS组合行人导航的分布式卡尔曼滤波| SANA佳文速递
○ 杨飞博士：GNSS天顶对流层精化模型的构建与分析| SANA佳文速递
○ 姚铮教授：北斗PPP服务信号调制复用方案设计与质量评估| SANA佳文速递
○ 2022征文| SatNav“普适定位、室内导航与基于位置服务”专题
《自然资源遥感》
○ 《自然资源遥感》入驻“智绘科服”融媒体平台！
○ 《自然资源遥感》征稿：“海岸带空间资源及生态健康遥感监测”专栏
○ 摘要 |《自然资源遥感》2021年第3期摘要推荐
○ 摘要 |《自然资源遥感》2021年第4期摘要推荐
《Journal of Geovisualization and Spatial Analysis》
○《Journal of Geovisualization and Spatial Analysis》入驻“智绘科服”融媒体平台！
○ JGSA国际期刊2021年第5卷第2期论文摘要
○ 高被引论文推荐 | Journal of Geovisualization and Spatial Analysis
,
