基于抗差LM的视觉惯性里程计与伪卫星混合高精度室内定位
杨高朝1,2
, 王庆1,2
, 蔚保国3, 刘鹏飞1,2,李爽3
1. 东南大学仪器科学与工程学院, 江苏 南京 210096;
2. 东南大学智慧城市研究院, 江苏 南京 210096;
3. 卫星导航系统与装备技术国家重点实验室, 河北 石家庄 050050
基金项目：国家重点研发计划（2020YFD110011-01）
关键词：视觉惯性里程计 伪卫星 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]。
1 多源数据融合约束方程的建
立
1.1 数据内插
多传感器的融合时间戳对齐是非常关键的一步。时间戳无法对齐或对齐错误最后可能得出一个错误的轨迹，很可能使优化算法失效。为了保证融合算法的稳健性，视觉传感器的时间戳与伪卫星的时间戳相差不超过10ms。
(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待定。根据二阶微分连续性，可得
1.2 多传感器约束方程的建立
由于伪卫星不能求出旋转向量，因此在优化的时候只能利用坐标向量和速度向量。总的融合约束方程可以写为
(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 多传感器融合流程 Fig. 1 Flowchart of multi-sensor fusion
图选项
3 试验验证及分析
3.1 试验验证
声明：本站部分文章内容及图片转载于互联 、内容不代表本站观点，如有内容涉及侵权，请您立即联系本站处理，非常感谢！