近年来，随着光电信息、人工智能及导航制导技术的快速发展，以自动驾驶汽车、轮式机器人为代表的无人车发展迅猛。然而，目前的无人车导航系统对卫星导航信息高度依赖。当其行驶在城市峡谷、室内、地下停车场等遮挡环境或受到电磁压制及干扰时，卫星导航系统无法提供高精度、可靠的导航定位信息，从而导致无人车在工作时会存在极大的安全隐患，严重情况下甚至会丧失自主运行能力。

为了降低对卫星导航信息的依赖，无人车通常会搭载惯性测量单元、里程计、激光雷达、摄像头等不同类型的传感器，利用多种导航信息源综合提升车载导航系统的精度与可靠性。但随着车辆行驶环境的变化，导航信息会呈现出“有无”以及“优劣”的特征。前者主要表现为不同时刻可用的导航信息类型会发生变化，即异步、异类性。而后者主要体现在不同导航信息的精度存在差异且同一导航信息的误差特性会发生改变，即异质性。卡尔曼滤波与因子图优化是目前最普遍的两种多源融合方法，然而这两类方法在处理具有上述特征的多源导航信息时均会面临融合性能下降的问题，难以满足无人车在卫星不可用及干扰环境下的高精度、高可靠导航需求。因此，本文对基于因子图优化的无人车多源融合高精度自适应导航关键技术开展了系统性研究工作，重点解决导航信息冲突与受扰时的自适应抗扰融合问题。

论文首先针对因子图优化的理论方法和融合方式进行了研究与改进。研究了多源融合中的因子图优化与惯性预积分建模方法，并在此基础上总结了传统因子图在惯性基组合导航中仍存在的问题。首先，针对传统方法中由于惯性量测近似离散化所导致的运动失真问题，提出了改进的解析式惯性预积分模型，该模型能够抑制动态情况下系统离散化误差对因子图方法融合性能的影响。其次，针对传统方法在处理异步、异类、异质多源导航信息时出现的估计精度下降问题，提出了等间隔自适应因子图优化融合方法。通过构建等间隔因子图模型，能够实现对不同时频、不同类型导航信息的高精度即插即用。同时，通过对各类导航信息的性能进行在线评估与融合权重的动态调节，可以实现对不同误差特性导航信息的抗扰融合。

论文随后针对因子图方法在无人车自适应导航中的若干关键技术问题展开了讨论。针对卫星受扰时传统惯性/卫星组合导航性能明显下降的问题，本文提出了一种基于高精度回环约束的惯性/卫星紧耦合因子图优化融合方法。首先，构建了面向卫星导航系统的高精度回环约束模型，可有效避免整周模糊度的复杂求解过程，并基于时间差分载波相位信息实现回环校正进而减小误差累积。在此基础上，通过对高精度回环约束与惯性量测信息的融合，能够实现在卫星受扰时的高精度定位。

针对在卫星不可用环境下传统惯性/里程计组合导航方法定位误差快速发散的问题，本文提出了基于增广预积分的惯性/里程计因子图优化融合方法。推导并构建了惯性/里程计增广预积分模型，该模型充分考虑了里程计标度因数、安装杆臂以及安装偏角的影响，可提高相对量测信息的精度，并实现比卡尔曼滤波更高的在线标定精度。通过对里程计信息的充分标定，可以有效提升无人车在卫星不可用环境下的自主导航性能。

针对传统多源融合方法由于缺乏对绝对、相对量测信息的自适应兼容能力进而导致的定位性能恶化问题，本文提出了面向绝对/相对量测信息的等间隔自适应因子图优化融合方法，推导了绝对/相对量测信息的关联模型以及线性化模型，能够有效利用绝对、相对量测信息的互补特性提升估计精度。同时，构建了基于期望最大化的绝对/相对量测信息误差聚类方法。通过对各类量测信息误差参数的在线估计与融合权重的动态调节，能够抑制异常量测的影响，从而提升组合导航系统的抗扰能力。

本文结合无人车在卫星不可用及干扰环境下的高精度导航需求，提出了等间隔自适应因子图优化融合方法，可以实现比传统因子图优化融合方法更高的状态估计精度。在此基础上，分别针对不同车载组合导航模式存在的问题进行了分析，并对因子图优化融合方法在车载导航应用中的理论延伸进行了研究，可以为无人车在卫星不可用及干扰环境下的成功应用提供有效的解决途径。

The unmanned vehicles, typified by autonomous vehicles and wheeled robots, have developed rapidly in recent years with the rapid development of photoelectric information, artificial intelligence and navigation and guidance technology. However, current unmanned vehicle navigation systems are highly dependent on GNSS information. When it is driving in the urban canyon, indoor, underground parking and other blocked environments or is subject to electromagnetic suppression and interference, GNSS cannot provide high-precision and reliable navigation and positioning information, resulting in great safety hazards when unmanned vehicle works. Moreover, the ability to autonomously operate will be lost especially in severe cases.

To reduce the dependence on GNSS information, unmanned vehicles are usually equipped with different types of sensors such as IMU, odometer, LiDAR, camera, and so forth. Multiple navigation information sources can be integrated to improve the accuracy and reliability of vehicular navigation system. However, navigation information presents characteristics of "have or not" and "good and bad" as driving environment of unmanned vehicle changes. The former is mainly showed in that the type of navigation information available changes at different times, namely asynchronism and variability. The latter is reflected in that there exist differences in the accuracy of different navigation information and error characteristics of the same navigation information would change, namely heterogeneity. Kalman filter and factor graph optimization are the two most common multi-source fusion methods. However, these two methods both face the problem of performance degradation when dealing with multi-source navigation information with above characteristics. It is difficult to satisfy the high-precision and high-reliability navigation needs of unmanned vehicles when GNSS is unavailable and interfered. Thereby, this paper carried out the systematic research on key technology of multi-source fusion high-precision adaptive navigation for unmanned vehicles based on factor graph optimization, which focuses on solving the adaptive and anti-jamming fusion problem when there are conflicts and disturbances in navigation information.

This paper firstly conducts the research and improvement on the theoretical method and fusion manner of the factor graph optimization. Factor graph optimization and IMU pre-integration modeling method in multi-source fusion are studied, and the problems of traditional factor graph in inertial-based integrated navigation are summarized. Firstly, aimed at the problem of motion distortion caused by approximate discretization of inertial measurements in traditional methods, an improved analytical IMU pre-integration is proposed, which can suppress the influence of system discretization error on fusion under dynamic conditions. Secondly, aimed at the problem of decreasing estimation accuracy in current methods when handling asynchronous, various, and heterogeneous multi-source navigation information, a fixed-rate adaptive factor graph optimization fusion method is proposed. By building the fixed-rate factor graph model, the high-precision plug-and-play for different time-frequency and different types of navigation information can be achieved. Also, anti-jamming fusion of navigation information with different error characteristics can be realized via the online evaluation of different navigation information and dynamic adjustment of the fusion weight.

This paper then discusses some key technical issues about factor graph in the unmanned vehicle adaptive navigation. Aiming at the problem that the performance of traditional IMU/GNSS fusion decreases when GNSS is disturbed, an IMU/GNSS tightly coupled factor graph optimization fusion method based on high-precision loop closure constraints is proposed. Firstly, a high-precision loop-closure constraint model for GNSS is constructed, which can effectivey avoid the complex solution of integer ambiguity. Meanwhile, the proposed method can reduce the error accumulation by realizing the loop closure correction based on time-differenced carrier phase information. On this basis, high-precision positioning can be achieved when GNSS is disturbed through the fusion of high-precision loop closure constraints and inertial measurement information.

Aiming at the problem that positioning error of traditional IMU/odometer fusion rapidly diverges when GNSS is unavailable, this paper proposes an IMU/odometer factor graph optimization fusion method based on the augmented pre-integration. The IMU/odometer augmented pre-integration model is deduced and constructed, and the influence of odometer scale factor, lever arm and misalignment are fully considered, which can improve the accuracy of the relative measurement information and achieve higher online calibration performance than Kalman filter. By fully calibrating the odometer information, it can effectively improve the autonomous navigation performance of unmanned vehicles when GNSS is unavailable.

Aimed at the problem that the positioning performance degrades in current multi-source fusion method due to the lack of adaptive compatibility with absolute and relative measurements, this paper proposes a fixed-rate adaptive factor graph optimization fusion method for absolute/relative measurements. The correlation model and linearization model of absolute/relative measurements are deduced, which can effectively utilize the complementary characteristics of absolute and relative measurements to improve state estimation accuracy. Meanwhile, an absolute/relative measurements error clustering method using expectation maximization is proposed. Through the online estimation of error parameters and the dynamic adjustment of fusion weights, the effect of abnormal measurements is restrained, thereby improving the anti-interference ability of the integrated navigation system in interference environments.

This paper proposes a fixed-rate adaptive factor graph optimization fusion method based on the high-precision navigation requirements of unmanned vehicles when GNSS is unavailable and interfered, which can achieve higher state estimation accuracy than traditional factor graph optimization methods. On this basis, the problems existing in different vehicular integrated navigation modes are analyzed, and theoretical extension of factor graph optimization fusion method in vehicular navigation is studied, which provides an effective solution for the successful application of unmanned vehicles when GNSS is unavailable and interfered.

[1] 王少博. 动态场景下基于交互性预测的自动驾驶汽车轨迹规划方法研究[D]. 中国科学技术大学, 2020.

[2] 罗敏. 复杂环境移动机器人路径规划与轨迹跟踪研究[D]. 电子科技大学, 2021.

[3] 唐斌, 郑冲, 章林锋, 王正. 美国导航战新进展与启示[J]. 导航定位与授时, 2020, 7(04): 110-116.

[4] 中国科学技术协会. 面向未来的科技——2020重大科学问题和工程技术难题解读[M]. 中国科学技术出版社, 2020.

[5] Bacha A, Bauman C, Faruque R, et al. Odin: Team victortango's entry in the darpa urban challenge[J]. Journal of Field Robotics, 2008, 25(8): 467-492.

[6] Tadjdeh Y. DARPA pursuing technologies to help troops ID enemies[J]. National Defense, 2016, 100(750): 42-45.

[7] Ebadi K, Chang Y, Palieri M, et al. LAMP: Large-scale autonomous mapping and positioning for exploration of perceptually-degraded subterranean environments[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 80-86.

[8] Waldrop M M. Autonomous vehicles: No drivers required[J]. Nature News, 2015, 518(7537): 20.

[9] Kauffman K, Marietta D, Kresge J, et al. Field demonstration of plug and play navigation system using scorpion and smart sensors/cables[C]//ION Joint Navigation Conference. 2017.

[10] 宋丽君, 薛连莉, 董燕琴, 吴亚明, 陈效真. 全源定位与导航的发展与建议[J]. 导航与控制, 2017, 16(06): 99-105+24.

[11] Nine J, Shirey R, Thompson B, et al. Open adaptable architecture (OA2) for agile intelligence surveillance reconnaissance (ISR)[C]//Open Architecture/Open Business Model Net-Centric Systems and Defense Transformation 2019. International Society for Optics and Photonics, 2019, 11015: 110150G.

[12] Shen K, Wang M, Fu M, et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles[J]. IEEE Transactions on Industrial Electronics, 2019, 67(9): 7659-7668.

[13] 杨元喜. 弹性PNT基本框架[J]. 测绘学报, 2018, 47(07): 893-898.

[14] Barfoot T D. State estimation for robotics[M]. Cambridge University Press, 2017.

[15] 冯培德. 论混合式惯性导航系统[J]. 中国惯性技术学报, 2016, 24(03): 281-284+290.

[16] Wang D, Dong Y, Li Z, et al. Constrained MEMS-based GNSS/INS tightly coupled system with robust Kalman filter for accurate land vehicular navigation[J]. IEEE Transactions on Instrumentation and Measurement, 2019, 69(7): 5138-5148.

[17] 张炎华, 王立端, 战兴群, 翟传润. 惯性导航技术的新进展及发展趋势[J]. 中国造船, 2008, 49(S1): 134-144.

[18] Joubert N, Reid T G R, Noble F. Developments in modern GNSS and its impact on autonomous vehicle architectures[C]//2020 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2020: 2029-2036.

[19] Hou P, Zhang B, Liu T. Integer-estimable GLONASS FDMA model as applied to Kalman-filter-based short-to long-baseline RTK positioning[J]. GPS Solutions, 2020, 24(4): 1-14.

[20] 赖际舟, 袁诚, 吕品, 刘建业, 何洪磊. 不依赖于卫星的无人系统视觉/激光雷达感知与自主导航技术[J]. 导航定位与授时, 2021, 8(03): 1-14.

[21] 胡悦, 李旭, 徐启敏, 袁建华, 孔鑫. 卫星拒止环境下基于因子图的智能车可靠定位方法[J]. 仪器仪表学报, 2021, 42(11): 79-86.

[22] 严恭敏. 捷联惯导算法与组合导航原理[M]. 西北工业大学出版社, 2019.

[23] Paul D. Groves. GNSS与惯性及多传感器组合导航系统原理.第2版[M]. 国防工业出版社, 2015.

[24] Weiss S, Siegwart R. Real-time metric state estimation for modular vision-inertial systems[C]//2011 IEEE international conference on robotics and automation. IEEE, 2011: 4531-4537.

[25] Strasdat H, Davison A J, Montiel J M M, et al. Double window optimisation for constant time visual SLAM[C]//2011 International Conference on Computer Vision. IEEE, 2011: 2352-2359.

[26] 潘杨杰. 基于视觉多传感器融合的室内移动机器人定位技术研究[D]. 浙江大学, 2019.

[27] 蒋国华. 视觉传感器及其图像匹配算法[D]. 浙江大学, 2007.

[28] 张香利. 基于激光雷达和视觉信息融合的水面船只检测与识别研究[D]. 上海交通大学, 2017.

[29] 石鹏. 双盲环境下基于激光雷达/惯性融合的无人机自主导航技术[D]. 南京航空航天大学, 2019.

[30] 甘俊杰. 基于ARM平台的激光导航SLAM算法研究与实现[D]. 北京邮电大学, 2017.

[31] Droeschel D, Behnke S. Efficient continuous-time SLAM for 3D LiDAR-based online mapping[C]//2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018: 5000-5007.

[32] 辛贾. 美国陆军的未来战术移动机器人计划DemoIII[J]. 机器人技术与应用, 2001(04): 33-35.

[33] Wan G, Yang X, Cai R, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]//2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018: 4670-4677.

[34] Indelman V, Williams S, Kaess M, et al. Information fusion in navigation systems via factor graph based incremental smoothing[J]. Robotics and Autonomous Systems, 2013, 61(8): 721-738.

[35] Zhu Z, Adams K, Venable D, et al. Goal-driven sensor configuration in a navigation system[C]//Proceedings of IEEE/ION PLANS 2016. 2016: 527-534.

[36] 戴海发, 卞鸿巍, 马恒, 王荣颖. 全源定位与导航的统一理论框架构建[J]. 导航定位与授时, 2018, 5(06): 9-16.

[37] 张靖, 王向东, 邓志宝, 张凌东, 刘宇航. 一种基于因子图的异步信息融合定位算法[J]. 导弹与航天运载技术, 2019(03): 89-95.

[38] Wen W, Kan Y C, Hsu L T. Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization[C]//Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2019). 2019: 3019-3032.

[39] Wisth D, Camurri M, Fallon M. Robust legged robot state estimation using factor graph optimization[J]. IEEE Robotics and Automation Letters, 2019, 4(4): 4507-4514.

[40] Tiziani L, Zhang Y, Dellaert F, et al. Factor graph-based trajectory optimization for a pneumatically-actuated jumping robot[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 9870-9876.

[41] Merfels C, Stachniss C. Pose fusion with chain pose graphs for automated driving[C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016: 3116-3123.

[42] Wen W, Pfeifer T, Bai X, et al. Factor graph optimization for GNSS/INS integration: A comparison with the extended Kalman filter[J]. NAVIGATION, Journal of the Institute of Navigation, 2021, 68(2): 315-331.

[43] Yuan D, Qin Y, Wu Z, et al. A robust multi-state constraint optimization-based orientation estimation system for satcom-on-the-move[J]. IEEE Transactions on Instrumentation and Measurement, 2021, 70: 1-14.

[44] Wei X, Li J, Feng K, et al. Vehicle INS/odometer integrated navigation algorithm based on factor graph[C]//2019 IEEE International Conference on Unmanned Systems (ICUS). IEEE, 2019: 216-220.

[45] Chiu H P, Zhou X S, Carlone L, et al. Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor graphs[C]//2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014: 663-670.

[46] Ch’ng S F, Khosravian A, Doan A D, et al. Outlier-robust manifold pre-Integration for INS/GPS fusion[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019: 7489-7496.

[47] Cioffi G, Scaramuzza D. Tightly-coupled fusion of global positional measurements in optimization-based visual-inertial odometry[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020: 5089-5095.

[48] Indelman V, Williams S, Kaess M, et al. Information fusion in navigation systems via factor graph based incremental smoothing[J]. Robotics and Autonomous Systems, 2013, 61(8): 721-738.

[49] Liu J, Gao W, Hu Z. Optimization-based visual-inertial SLAM tightly coupled with raw GNSS measurements[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 11612-11618.

[50] Li Z, Gao J, Wang J, et al. PPP/INS tightly coupled navigation using adaptive federated filter[J]. GPS solutions, 2017, 21(1): 137-148.

[51] 孙兆妍, 王新龙. SINS/GPS松组合与紧组合导航系统抗干扰性能比较与验证[J]. 航空兵器, 2014(04): 31-35+53.

[52] 单斌, 张复建, 杨波, 王跃钢, 薛亮, 腾红磊. 基于自适应姿态估计的MIMU/GPS紧组合导航算法[J]. 中国惯性技术学报, 2018, 26(06): 760-767.

[53] 陈驵. 高动态下惯性北斗深组合导航性能增强技术研究[D]. 南京航空航天大学, 2019.

[54] Wen W, Bai X, Kan Y C, et al. Tightly coupled GNSS/INS integration via factor graph and aided by fish-eye camera[J]. IEEE Transactions on Vehicular Technology, 2019, 68(11): 10651-10662.

[55] Chen Y, Zhao S, Farrell J A. Computationally efficient carrier integer ambiguity resolution in multiepoch GPS/INS: A common-position-shift approach[J]. IEEE Transactions on Control Systems Technology, 2015, 24(5): 1541-1556.

[56] Vasilyuk N, Vorobiev M, Tokarev D. Heading and attitude determination system with low-cost IMU embedded inside one of multiple antennas[C]//2018 IEEE/ION Position, Location and Navigation Symposium (PLANS). IEEE, 2018: 267-274.

[57] 李安, 冯卡力, 覃方君. 伪距/伪距率/双差分载波相位组合导航方法[J]. 中国惯性技术学报, 2015, 23(05): 602-606.

[58] Wendel J, Meister O, Monikes R, et al. Time-differenced carrier phase measurements for tightly coupled GPS/INS integration[C]//Proceedings of IEEE/ION PLANS 2006. 2006: 54-60.

[59] Han S, Wang J. Integrated GPS/INS navigation system with dual-rate Kalman Filter[J]. GPS Solutions, 2012, 16(3): 389-404.

[60] Wang G, Xu X, Wang J, et al. An enhanced INS/GNSS tightly coupled navigation system using time-differenced carrier phase measurement[J]. IEEE Transactions on Instrumentation and Measurement, 2019, 69(7): 5208-5218.

[61] Soon B K H, Scheding S, Lee H K, et al. An approach to aid INS using time-differenced GPS carrier phase (TDCP) measurements[J]. GPS Solutions, 2008, 12(4): 261-271.

[62] Zhao Y, Becker M, Becker D, et al. Improving the performance of tightly-coupled GPS/INS navigation by using time-differenced GPS-carrier-phase measurements and low-cost MEMS IMU[J]. Gyroscopy and Navigation, 2015, 6(2): 133-142.

[63] Zhao Y. Applying time-differenced carrier phase in nondifferential GPS/IMU tightly coupled navigation systems to improve the positioning performance[J]. IEEE Transactions on Vehicular Technology, 2016, 66(2): 992-1003.

[64] Zhao S, Chen Y, Farrell J A. High-precision vehicle navigation in urban environments using an MEM's IMU and single-frequency GPS receiver[J]. IEEE Transactions on Intelligent Transportation Systems, 2016, 17(10): 2854-2867.

[65] Suzuki T. Time-relative RTK-GNSS: GNSS loop closure in pose graph optimization[J]. IEEE Robotics and Automation Letters, 2020, 5(3): 4735-4742.

[66] 王洪先. 陆用惯性导航系统技术发展综述[J]. 光学与光电技术, 2019, 17(06): 77-85.

[67] Li Z, Wang J, Li B, et al. GPS/INS/Odometer integrated system using fuzzy neural network for land vehicle navigation applications[J]. The Journal of Navigation, 2014, 67(6): 967-983.

[68] Li L, Sun H, Yang S, et al. Online calibration and compensation of total odometer error in an integrated system[J]. Measurement, 2018, 123: 69-79.

[69] Seo J W, Lee H K, Lee J G, et al. Lever arm compensation for GPS/INS/odometer integrated system[J]. International Journal of Control, Automation, and Systems, 2006, 4(2): 247-254.

[70] Ou Y, Wu Y, Chen H. INS/odometer land navigation by accurate measurement modeling[C]//2020 27th Saint Petersburg International Conference on Integrated Navigation Systems (ICINS). IEEE, 2020: 1-9.

[71] 严恭敏. 车载自主定位定向系统研究[D]. 西北工业大学, 2006.

[72] Wu Y. Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning[C]//2014 DGON Inertial Sensors and Systems (ISS). IEEE, 2014: 1-19.

[73] Gao J, Li K, Chen Y. Study on integration of FOG single-axis rotational INS and odometer for land vehicle[J]. IEEE Sensors Journal, 2017, 18(2): 752-763.

[74] Liu Z, El-Sheimy N, Qin Y. Low-cost INS/odometer integration and sensor-to-sensor calibration for land vehicle applications[C]//Proceedings of the IAG/CPGPS International Conference on GNSS+(ICG+ 2016). 2016.

[75] Zhao H, Miao L, Shao H. Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems[J]. The Journal of Navigation, 2017, 70(2): 242-261.

[76] Zhang P, Hancock C M, Lau L, et al. Low-cost IMU and odometer tightly coupled integration with Robust Kalman filter for underground 3-D pipeline mapping[J]. Measurement, 2019, 137: 454-463.

[77] 高军强, 汤霞清, 张环, 武萌. 基于因子图的车载INS/GNSS/OD组合导航算法[J]. 系统工程与电子技术, 2018, 40(11): 2547-2553.

[78] Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual-inertial odometry[J]. IEEE Transactions on Robotics, 2016, 33(1): 1-21.

[79] He Y, Guo Y, Ye A, et al. Camera-odometer calibration and fusion using graph based optimization[C]//2017 IEEE International Conference on Robotics and Biomimetics (ROBIO). IEEE, 2017: 1624-1629.

[80] Quan M, Piao S, Tan M, et al. Tightly-coupled monocular visual-odometric slam using wheels and a mems gyroscope[J]. IEEE Access, 2019, 7: 97374-97389.

[81] Liu J, Gao W, Hu Z. Visual-inertial odometry tightly coupled with wheel encoder adopting robust initialization and online extrinsic calibration[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019: 5391-5397.

[82] Chang L, Niu X, Liu T. GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-Integration[J]. Sensors, 2020, 20(17): 4702.

[83] Chen Q, Zhang Q, Niu X. Estimate the pitch and heading mounting angles of the IMU for land vehicular GNSS/INS integrated system[J]. IEEE Transactions on Intelligent Transportation Systems, 2020, 22(10): 6503-6515.

[84] Roumeliotis S I, Burdick J W. Stochastic cloning: A generalized framework for processing relative state measurements[C]//Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No. 02CH37292). IEEE, 2002, 2: 1788-1795.

[85] Qin T, Li P, Shen S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[86] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[87] Zhang J, Singh S. LOAM: LiDAR odometry and mapping in real-time[C]//Robotics: Science and Systems. 2014, 2(9): 1-9.

[88] 秦永元, 张洪钺, 王叔华. 卡尔曼滤波与组合导航原理[M]. 西北工业大学出版社, 2012.

[89] Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to mav navigation[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013: 3923-3929.

[90] Shen S, Mulgaonkar Y, Michael N, et al. Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV[C]//2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014: 4974-4981.

[91] Chiang K W, Tsai G J, Chu H J, et al. Performance enhancement of INS/GNSS/Refreshed-SLAM integration for acceptable lane-level navigation accuracy[J]. IEEE Transactions on Vehicular Technology, 2020, 69(3): 2463-2476.

[92] Qin T, Cao S, Pan J, et al. A general optimization-based framework for global pose estimation with multiple sensors[J]. arXiv preprint arXiv:1901.03642, 2019.

[93] Mascaro R, Teixeira L, Hinzmann T, et al. Gomsf: Graph-optimization based multi-sensor fusion for robust uav pose estimation[C]//2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018: 1421-1428.

[94] Tao X, Zhu B, Xuan S, et al. A multi-sensor fusion positioning strategy for intelligent vehicles using global pose graph optimization[J]. IEEE Transactions on Vehicular Technology, 2021, 71(3): 2614-2627.

[95] Brossard M, Barrau A, Chauchat P, et al. Associating uncertainty to extended poses for on lie group IMU preintegration with rotating Earth[J]. IEEE Transactions on Robotics, 2021, 38(2): 998-1015.

[96] Kschischang F R, Frey B J, Loeliger H A. Factor graphs and the sum-product algorithm[J]. IEEE Transactions on Information Theory, 2001, 47(2): 498-519.

[97] 周雅婧, 曾庆化, 刘建业, 孙克诚. 因子图发展及其在定位与导航的应用技术[J]. 全球定位系统, 2020, 45(01): 1-11.

[98] Dellaert F, Kaess M. Factor graphs for robot perception[J]. Foundations and Trends? in Robotics, 2017, 6(1-2): 1-139.

[99] Eckenhoff K, Geneva P, Huang G. High-accuracy preintegration for visual-inertial navigation[M]//Algorithmic Foundations of Robotics XII. Springer, Cham, 2020: 48-63.

[100] Sola J. Quaternion kinematics for the error-state KF[J]. Laboratoire dAnalyse et dArchitecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep, 2012.

[101] Agarwal S, Mierle K. Ceres solver: Tutorial & reference[J]. Google Inc, 2012, 2(72): 8.

[102] Pfeifer T, Protzel P. Expectation-maximization for adaptive mixture models in graph optimization[C]//2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019: 3151-3157.

[103] 侯燕青. 多卫星导航系统RTK定位部分整周模糊度解算方法研究[D]. 国防科学技术大学, 2017.

[104] Klobuchar J A. Ionospheric time-delay algorithm for single-frequency GPS users[J]. IEEE Transactions on Aerospace and Electronic Systems, 1987 (3): 325-331.

[105] J, Saastamoinen. Contributions to the theory of atmospheric refraction[J]. Bulletin géodésique, 1972.

[106] Zhang X, Li P. Benefits of the third frequency signal on cycle slip correction[J]. GPS Solutions, 2016, 20(3): 451-460.

[107] Zhao J, Hernández-Pajares M, Li Z, et al. High-rate Doppler-aided cycle slip detection and repair method for low-cost single-frequency receivers[J]. GPS Solutions, 2020, 24: 1-13.

[108] Kong J, Pfeiffer M, Schildbach G, et al. Kinematic and dynamic vehicle models for autonomous driving control design[C]//2015 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2015: 1094-1099.

[109] Wu Y, Goodall C, El-Sheimy N. Self-calibration for IMU/odometer land navigation: Simulation and test results[C]//Proceedings of the 2010 International Technical Meeting of The Institute of Navigation. 2010: 839-849.

[110] Fejes P. Estimation of steering wheel angle in heavy-duty trucks[D]. Uppsala University, 2016.

[111] Mourikis A I, Roumeliotis S I, Burdick J W. SC-KF mobile robot localization: A stochastic cloning Kalman filter for processing relative-state measurements[J]. IEEE Transactions on Robotics, 2007, 23(4): 717-730.

[112] Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to mav navigation[C]//2013 IEEE/RSJ international conference on intelligent robots and systems. IEEE, 2013: 3923-3929.

[113] Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4758-4765.