奉行卓越设计
打造国际品质

关于无人车的高精度定位问题（特指的是无人车定位，而非建图或者SLAM问题），就目前来看，主要里程碑性质的文章包括（当然有很多公司研发了高精度融合定位技术，没有公开的）:

里程碑1：2007年和2010年斯坦福大学Levinson博士提出了基于因子图优化的方式建立地图，基于粒子滤波的框架实现基于地图的高精度定位。参考以下文章：

• Levinson, Jesse, Michael Montemerlo, and Sebastian Thrun. "Map-based precision vehicle localization in urban environments."Robotics: science and systems. Vol. 4. 2007.
• Levinson, Jesse, and Sebastian Thrun. "Robust vehicle localization in urban environments using probabilistic maps."2010 IEEE International Conference on Robotics and Automation. IEEE, 2010.

里程碑2：2017年，百度无人车前后开源了Apollo，其中定位模块采用误差卡尔曼滤波算法实现多传感器的融合定位，其中地图匹配部分沿袭了Stanford团队的算法，但是用多个高斯建模地图，做出了一些改进。和斯坦福当年的粒子滤波的算法相比，百度的团队采用单一假设（卡尔曼滤波）实现多传感融合。从文章的的测试结果来看，精度可人。参考以下文章：

• Wan, G., Yang, X., Cai, R., Li, H., Zhou, Y., Wang, H., & Song, S. (2018, May). Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes. In2018 IEEE International Conference on Robotics and Automation (ICRA)(pp. 4670-4677). IEEE.

关于上述两个里程碑的文章，其实在数据融合的方式上，还是属于滤波的范畴，首先由于要满足一阶马尔科夫假设，历史信息被无情的抛弃。后面我将介绍，其实在挑战一些的场景下，历史信息是可以有效的帮助当前状态估计的，不论是从线性化，迭代还是时间关联的角度，都是会帮助到当前状态的估计。

里程碑3：有趣的是，自从2017年以后，各大无人车公司，创业团队关于自动驾驶定位方面的文章相当有限。一方面是不想公开自己的定位算法原理，另一方面，如果在不是非常挑战的场景，比如说楼很高，交通很密集的场景，其实现有传感器（比如RTK）就可以获得很好的定位效果，采用一个标准的滤波的方式就可以获得一个比较不错的定位精度。但是，如果真的要实现无人车的全场景的高精度定位，比如城市密集区域，则一个稳定的数据融合框架就显得很重要了。有意思的是，今年的ICRA上，百度的无人车团队又更新了一个新的文章。参考文章（Ding, Wendong, et al. "LiDAR Inertial Odometry Aided Robust LiDAR Localization System in Changing City Scenes."Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). IEEE (May 2020).）。作者对数据融合的框架进行了大改！从滤波的方式改成了基于因子图优化的方式；并且加入了场景变化检测的部分，最后也把实时激光里程计加入到了数据融合框架中，不得不说百度无人车组在定位方面有在做了很棒的工作。关于百度的这篇文章的三个亮点，下面单独介绍一下：

• 关于激光里程计：百度沿袭了google在2016年（Hess, W., Kohler, D., Rapp, H., & Andor, D. (2016, May). Real-time loop closure in 2D LIDAR SLAM. In2016 IEEE International Conference on Robotics and Automation (ICRA)(pp. 1271-1278). IEEE.）的工作，有效的使用高频率的IMU数据，实现激光的运动畸变补偿。这里没有采用ICP，NDT也没有LOAM，当然很大程度上也是考虑了时效性问题，google的cartographer效率非常高。从效果上看，如果由imu的持续平滑，激光里程计在大部分时候是很可靠的，即使在后期GNSS RTK失效，高精度地图由于环境变化失效，激光里程计依旧可以很好的帮助estimator在短时间内不会出现很大的误差；当然，这里面有个很大的前提是我们有效的检测出来了传感器失效！比如检测出RTK GNSS或者地图变更，百度这个文章里面有个很大的贡献就是提出了如何基于submap实现环境变更的检测。
• 关于数据融合框架：其实从应用的成熟度上来说，产业界比较喜欢滤波的方式（比如Kalman滤波）进行数据融合定位，注意这里说的是实时的融合定位，不是指建图或者SLAM问题。一方面是考虑滤波的方式理论上相当成熟；另一方面是滤波的方式实现起来快，实现上非常简便。近年来随着因子图技术的快速发展，特别是在SLAM领域获得了快速的发展；和滤波的方式不同，基于因子图的数据融合将当前数据和历史数据一起建立一个因子图，基于因子图建立一个批优化代价函数，然后将所有的历史信息和当前信息一起优化。关于因子图技术的基本问题，可参考之前的文章（https://zhuanlan.zhihu.com/p/49500590）。在2016年，来自加州大学河滨分校的Zhao博士在文章（Zhao, Sheng, Yiming Chen, and Jay A. Farrell. "High-precision vehicle navigation in urban environments using an MEM's IMU and single-frequency GPS receiver."IEEE transactions on intelligent transportation systems17.10 (2016): 2854-2867.）中比较理论的提到基于因子图的数据融合方式具备诸多优势，比如在因子图优化中，由于多次的迭代，可以有效的减少线性化的误差（多次迭代导致多次线性化，卡尔曼滤波中只有一次线性化）；多次迭代可以更好的趋近最优解（卡尔曼滤波只有一次迭代）。另外由于Zhao博士的文章中测试的场景相对简单，没有很好的验证因子图技术在挑战场景下的数据融合优势。有趣的是，zhao博士去了Google无人车组，为了弥补这个空白，我们组近期的研究（W. Wen, Y.-C. Kan, and L.-T. Hsu, "Performance Comparison of GNSS/INS Integrations Based on EKF and Factor Graph Optimization," presented at the ION GNSS+ 2019, Florida, 2019.）和文章(Wen, W., Bai, X., Kan, Y. C., & Hsu, L. T. (2019). Tightly Coupled GNSS/INS Integration via Factor Graph and Aided by Fish-Eye Camera.IEEE Transactions on Vehicular Technology,68(11), 10651-10662.)针对GNSS/INS融合定位问题，广泛的比较了基于EKF和因子图的融合算法精度分析，并且同时比较了松耦合和紧耦合。我们基于实验，验证基于因子图的数据融合算法具有以下优势：

(1) Multiple Iterations: The multiple iterations effectively help the FGO to approach the optimal state estimation. In the TC GNSS/INS integration, the observation function for pseudorange is a highly non-linear and single iteration in EKF is hard to reach optimal.

(2) Re-linearization: In each iteration of FGO, the linearization is conducted again. As the observation function for pseudorange is highly non-linear, multiple linearizations can effectively mitigate the linearization error. Therefore, multiple iterations and re-linearization are complementary.

(3) Time-correlation: The FGO effectively considered the historical information and all the historical information is connected by the INS factor. As a result, the time-correlation between epochs are explored and used to resist the outliers. In other words, the proper window size of FGO can help to improve the performance.

总结：技术总是快速发展，相比于传统的基于滤波的数据融合方式，大家现在可能比较关心的是，基于因子图的数据融合方式可以实时吗？那是当然，因为可以做sliding window啊，另外，不得不说，现在开源的优秀的非线性优化求解器，比如Ceres Solver很好的催化了因子图优化的快速应用，以上都是当下。关于因子图的前景，在挑战的场景下，其实很多的传感器的误差模型不服从高斯分布，然而传统的卡尔曼滤波依赖于一个很强的假设就是传感器的误差模型服从高斯分布；和传统的卡尔曼滤波不同，因子图虽然也要求传感器误差服从高斯分布，但是最近研究(Pfeifer, Tim, and Peter Protzel. "Robust sensor fusion with self-tuning mixture models."2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.)表明，高斯混合模型可以有效的应用到因子图优化中，该研究采用高斯混合模型对GNSS的误差模型进行建模，最终定位精度明显改善。

Reference：

[1]Levinson, Jesse, Michael Montemerlo, and Sebastian Thrun. "Map-based precision vehicle localization in urban environments."Robotics: science and systems. Vol. 4. 2007.