ukf性能评估（国内的ukf真实度）

ukf性能评估,国内的ukf真实度(5)

引文格式：杨高朝, 王庆, 蔚保国, 等. 基于抗差LM的视觉惯性里程计与伪卫星混合高精度室内定位[J]. 测绘学报，2022，51(1)：18-30. DOI: 10.11947/j.AGCS.2022.20200251

ukf性能评估,国内的ukf真实度(6)

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分开内插求解)：