在中秋节前刚刚闭幕的室内定位与导航国际会议(IPIN2022)上，我们团队与高德地图联合组队参赛，获得了IPIN2022 Track 6冠军。这次比赛的赛题相比于前一次(IPIN2020 Track 6)难度明显提升，表现在数据处理的实时模式、手机安装角变化，以及从不缺席的GNSS数据频繁中断和大量粗差。为此，我们采用了滑窗图优化算法来应对粗差，并努力将车辆NHC运动约束用到极致。

以下具体介绍该赛题和我们的参赛方案。

    IPIN2022 Track 6 手机车辆定位赛题的主要任务是利用智能手机自带的IMU、磁力计和GNSS估计车辆运动轨迹。从实时组合导航算法设计的角度，该赛题主要有以下挑战：

1. 实时数据处理：本次比赛在前一次IPIN2020手机车辆定位比赛的基础上将数据处理方式从完全事后处理模式切换成线上准实时提交结果的形式。具体地说，参赛者在指定时间连接比赛服务器，每次向服务器申请1秒钟的智能手机传感器测量值，并在规定时间内返回该秒数据的解算结果。循环该过程直到完成整个轨迹的计算。此外，每条比赛轨迹仅允许提交一次，因此参赛者需要保证自己的算法具有对各种工况的实时适应能力。

2. IMU安装角变化：比赛数据采集过程中，刻意安排了乘客上下测试车的情形。这意味着车上总载荷和载荷分布存在变化。我们知道，由于底盘和车身之间存在减震机构，载荷变化将导致车身与底盘之间的相对角度变化。这一改变会造成IMU相对于车体坐标系的安装角变化，因此会影响车辆运动约束(即非完整性约束，NHC)的正确性，是不可忽略的；同时，该变化又并不很明显，难以被算法察觉，尤其是在手机安装支架存在晃动和震动的情况下。

3.  GNSS粗差：本次比赛GNSS信息提供形式是GNSS单点定位坐标，在某些测试路段存在大量异常值(即粗差)，有些远大于10米，会严重影响最终定位结果。因此，算法抗差能力是该赛题中的重要考察内容。但是，由于提供的信息并不包括GNSS原始观测(伪距、载波相位)，可以使用的抗差策略相当有限。

    结合以前积累的组合导航算法开发和测试经验，针对该赛题的新挑战，我们设计实现了一套基于图优化的组合导航算法。具体而言，我们针对性地做了如下设计：

1. 该算法使用图优化(非线性最小二乘法)取代了常用的卡尔曼滤波，作为状态估计器。为了保证算法能够实时运行，采用滑动窗口优化器[1][2]；为了进一步减小计算负担，采用关键帧优化的机制，间隔1秒选择一个关键帧，在滑窗中保留最新20秒的关键帧。从信息利用的角度来看，图优化和滤波器对当前时刻导航状态的估计所使用的信息完全相同(即当前数据和所有历史数据)，且GNSS/INS组合导航也没有什么明显的非线性，因此不应该带来什么优势。但在该赛题的测试数据中存在大量GNSS定位粗差，采用图优化的主要优势是能够更从容、更灵活地使用滑窗内的所有信息来进行抗差处理，在滑窗时间范围内允许吃“后悔药”。图优化的另一个好处是可以保证滑窗内状态估计的自洽性，从而保证系统导航状态输出的连续性和稳定性。(图1)

2. 为了应对比赛数据中大量存在的千奇百怪的手机GNSS定位异常值的影响(即抗差问题)，本方案充分利用图优化的灵活性，首先实时地使用GNSS轨迹的相对距离信息来修正惯导速度，然后在滑窗内延迟校验和使用GNSS绝对位置信息。在系统的速度估计误差得到较好的约束后，就能对GNSS绝对位置的粗差做更好的判断，从而实现有效的抗差效果(图2、图3)。

3. 尽管事先并不知道比赛中刻意设计了上下乘客造成的IMU安装角变化，但是由于赛题采取准实时提交和一次性提交方式，我们的算法设计思路始终优先保证最好的鲁棒性而不是精度。因此，我们方案中IMU安装角和NHC杆臂都是通过在线估计的方式来实时获取和补偿的。同时，为了保证算法对于不同路况和车辆动态的适应性，该方案采用了一种自适应的NHC约束修正形式。

4. 另外，本方案还使用了磁力计(即磁航向)来约束轨迹航向。尽管很多场景下地磁场受到周围环境中钢铁结构影响而畸变，但其宏观分布仍然可认为是以地磁场为主(即磁干扰的均值为零)。本方案利用滑动窗口内自适应权重的平均磁场约束对航向进行修正，使轨迹方向更为稳定。

图1：图优化和滤波算法的性能比较

（GT：真值；Filter：滤波结果；Graph-Opt：图优化结果；GNSS：GNSS定位）

图2：图优化算法的抗差效果（训练数据）

图3：图优化算法的抗差效果（比赛数据）

    最终，我们以14.7米(75%概率误差)的成绩获得了该赛题冠军，相比于我们在2020年参赛时的后处理结果7.0米(75%概率误差)差了很多[3]，这也体现了实时处理与事后处理的巨大差异。

    复盘本次参赛，该赛题设计得挺有水平的，紧扣车载导航的核心痛点，暗藏杀机，具有很强的现实意义，能够充分考察出参赛方案在真实手机车辆导航应用中的精度和适应性。遗憾的是，该赛题没有提供GNSS原始观测数据(伪距、多普勒、载波相位)，使得参赛队不得不使用松组合，导致融合算法对GNSS抗差所能做的工作受到限制。期待今后该赛题能够开放GNSS原始数据(类似Google Decimeter Challenge比赛)，使大家能够有机会在紧组合架构下同台切磋。

    关于手机车辆定位方案，为了克服严重的GNSS粗差影响，我觉得实时滑窗优化算法是必要的，以便“汇聚”滑窗内的所有信息来对每个GNSS历元进行甄别。当然也可以采用类似多状态约束卡尔曼滤波(MS-CKF)那样改进的滤波算法，其实它也是有一个滑窗可以被利用来抗差的。我们认为这种滑窗抗差的思路也适用于其它观测粗差严重的组合导航问题。

点击链接可以下载我们的获奖答辩PPT： IPINTrack6-WHU-GD-V4.pdf

参考文献：

[1] Tang, Hailiang, et al. "Impact of the Earth Rotation Compensation on MEMS-IMU Preintegration of Factor Graph Optimization." IEEE Sensors Journal (2022), doi: 10.1109/JSEN.2022.3192552.   [PDF]

[2] https://github.com/i2Nav-WHU/OB_GINS

[3] 旷俭, 葛雯斐, 张全, 窦智, 唐爱鹏, 张小兵, 牛小骥* (2020). 基于手机内置传感器的车辆组合定位方法. 中国惯性技术学报, 28(6):701-708. DOI:10.13695/j.   [PDF]