自主定位与导航技术是无人驾驶车的核心关键技术，也是无人车领域近年来研究的热点。在城市道路环境中，由于交通状况复杂多变且无法预测，社会对无人车安全性、稳定性和经济性要求的不断提高，当前的自主定位与导航方法都存在一定的局限性。本文重点研究城市道路下无人车的自主定位与导航技术，主要研究内容如下：

（1）分别研究相机、IMU和固态激光雷达的传感器模型，对相机/IMU和激光雷达进行联合标定，提出融合视觉/IMU和激光雷达的SLAM框架，采用增量式平滑的因子图优化方法进行状态估计处理，将因子图保存在贝叶斯树中，当有新的因子节点加入时, 识别被影响的变量节点并仅对它们进行优化更新, 从而维持全优化的稀疏特性，实现多传感器融合定位与地图构建。通过构建一个全局的因子图, 各模块向因子图中插入起始因子、配准因子、IMU因子和闭环因子, 每当插入新的因子节点, 通过因子图优化算法对其进行优化更新，实现多种传感器紧密耦合，为无人车感知提供可靠位置、姿态和环境地图信息。分别在KITTI数据集、真实城市道路环境和隧道环境中对算法进行测试，并且与A-LOAM、LOAM-Livox和VINS-Fusion等算法进行对比，实验结果表明所设计的融合视觉/IMU和激光雷达的自主定位与地图构建系统在复杂环境下能够实现精准的自主定位与高精度地图构建，并且在保证精度的前提下依然具有很高的实时性和鲁棒性。

（2）为了实现车辆的自主导航，首先对阿克曼式无人车动力学模型进行分析，计算出阿克曼式无人车需满足的约束，通过处理所构建的高精度地图以获得静态障碍物地图，通过对动态障碍物进行检测和连续时间状态估计，解算出障碍物的位置信息、速度信息、类别信息，通过将动态障碍物约束加入到TEB轨迹规划器中，实现具有实时性和高度安全的轨迹规划。在轨迹规划器生成无人车目标轨迹后，通过基于运动学模型的轨迹跟踪器对轨迹进行跟踪，并对车辆实施平滑的时变反馈控制实现车辆的点到点自主导航。最后，搭建无人车实验验证平台对自主导航方法进行验证，实验结果表明所设计的自主定位与导航系统正确和有效。

Autonomous localization and navigation technology is the technic core of unmanned vehicles, which is also a hot topic in the field of unmanned vehicles in recent years. In a urban road condition, because of the complexity and unpreditity of its traffic conditions, the unmaned vehicles are expected to improve its security, stability and economy. However, there are still some restrictions with current autonomous localization and navigation methods. This paper focuses on the autonomous localization and navigation thchnology on urban roads, and the main research contents are as below:

(1) The sensor models of camera, IMU and solid-state LiDAR are studied respectively, and a joint calibration of camera/IMU and lidar is carried out. Then, a SLAM framework that combines visual, IMU and lidar is proposed and an incremental smoothing factor graph optimization method is adopted. The factor graph is saved in the Bayesian tree. When a new factor node is added, the affected variable nodes can be identified and optimized so as to maintain the fully optimized sparsity characteristics and achieve the goal of multi-sensor fusion localization and mapping. A global factor graph is constructed, and the starting factor, registration factor, IMU factor are inserted into the factor graph by each model. When a new factor node is inserted, it is optimized and updated through the factor graph optimization algorithm to achieve the coupling of more sensors and to provide reliable position, gestures and map information for the perception of autonomous vehicles. The algorithm is tested in the KITTI dataset, real urban road conditions and extreme tunnel conditions, and it’s also compared with algorithms such as A-LOAM, LOAM-Livox and VINS-Fusion. The experimental results show that the designed autonomous localization and map construction system that conbines fusion vision, IMU and laser radar can achieve high accuracy and high precision in complicated conditions, and it still has high real-time and robustness under the premise of ensuring accuracy.

(2) In order to realize the goal of the autonomous navigation of the vehicle, a dynamic model of the Ackerman vehicle is analysed at first, then the constraints that it needs to meet is calculated, and the map of obstacle in static state can be obtained through processing the constructed high-precision map.By detecting dynamic obstacles and estimating continuous time state, the information of localization, speed, and category of the obstacles are calculated, then the dynamic obstacle constraints are added to the TEB trajectory planner to achieve real-time and highly safe trajectory planning. After the targeted trajectory of autonomous vehicles is generated by the trajectory planner, the trajectory is tracked by the trajectory tracker based on the kinematics model, and the smooth time-varying feedback control is implemented on the vehicle to realize the point-to-point autonomous navigation of the vehicle. Finally, the autonomous navigation method is verified through an unmanned vehicle experimental platform. The experimental results show that the designed autonomous localization and navigation system is correct and effective.

[1] 杜明博. 基于人类驾驶行为的无人驾驶车辆行为决策与运动规划方法研究[D]. 合肥：中国科学技术大学, 2016.

[2] 王宏旭. 一种基于模糊神经网络的智能车横向控制方法[D]. 大连：大连理工大学, 2015.

[3] 陈佳佳. 城市环境下无人驾驶车辆决策系统研究[D]. 合肥：中国科学技术大学, 2014.

[4] 张高明. 室外无人车自主导航方法研究[D]. 杭州：浙江大学, 2017.

[5] 李勇. 基于激光雷达的智能车横向避障研究[D]. 长沙：湖南大学, 2017.

[6] 张向南. 智能车辆队列纵向控制系统的建模与实验研究[D]. 贵阳：贵州大学, 2015.

[7] Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE transactions on Robotics, 2007, 23(1): 34-46.

[8] Schon T, Gustafsson F, Nordlund P J. Marginalized particle filters for mixed linear/nonlinear state-space models[J]. IEEE Transactions on signal processing, 2005, 53(7): 2279-2289.

[9] Konolige K, Grisetti G, Kümmerle R, et al. Efficient sparse pose adjustment for 2D mapping[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010: 22-29.

[10] Hess W, Kohler D, Rapp H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016: 1271-1278.

[11] Zhang J, Singh S. Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2):401-416.

[12] 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.

[13] 李帅鑫, 李广云, 王力, 等. LiDAR/IMU紧耦合的实时定位方法[J]. 自动化学报, 2020.

[14] 闫利, 戴集成, 谭骏祥, 等. SLAM激光点云整体精配准位姿图技术[J]. 测绘学报, 2019,48(3):313-321.

[15] Besl P J, McKay N D. Method for registration of 3-D shapes[C]//Sensor fusion IV: control paradigms and data structures. International Society for Optics and Photonics, 1992, 1611: 586-606.

[16] 孙成. 三维激光点云特征提取及配准算法改进[D]. 西安：西安科技大学, 2019.

[17] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, D. Rus. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems, 2020.

[18] Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[C]//2011 International conference on computer vision. Ieee, 2011: 2564-2571.

[19] 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.

[20] Mur-Artal R, Tardós J D. Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[21] 张福斌, 刘宗伟. 一种单目相机/三轴陀螺仪/里程计紧组合导航算法[J]. 中国惯性技术学报, 2019, 27(1):36-42.

[22] V. Usenko, J. Engel, J. Stückler and D. Cremers. Direct visual-inertial odometry with stereo cameras[C]. IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, 2016, pp. 1885-1892.

[23] Qin T, Cao S, Pan J, et al. A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors[J]. 2019.

[24] Campos C, Elvira R, JJG Rodríguez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J]. 2020.

[25] 谷晓琳, 杨敏, 张燚, 等.一种基于半直接视觉里程计的RGB-D SLAM算法[J]. 机器人, 2020, 42(1): 39-48.

[26] Zhang J, Singh S. Visual-lidar odometry and mapping: Low-drift, robust, and fast[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015: 2174-2181.

[27] Lin J, Zhang F. Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 3126-3131.

[28] 谢浩. 结构化道路下无人驾驶汽车路径规划与运动控制算法研究[D]. 长沙：湖南大学, 2019.

[29] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[30] Stentz A. Optimal and efficient path planning for partially known environments[M]// Intelligent unmanned ground vehicles. Springer, Boston, MA, 1997: 203-220.

[31] Koenig S, Likhachev M. D^* lite[J]. Aaai/iaai, 2002, 15.

[32] Boor V, Overmars M H, Van Der Stappen A F. The Gaussian sampling strategy for probabilistic roadmap planners[C]//Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No. 99CH36288C). IEEE, 1999, 2: 1018-1023.

[33] Trees R E R. A New Tool for Path Planning[J]. Department of Computer Science. Iowa State University, 1998.

[34] Sprunk C, Lau B, Burgard W. Improved non-linear spline fitting for teaching trajectories to mobile robots[C]//2012 IEEE International Conference on Robotics and Automation. IEEE, 2012: 2068-2073.

[35] 修彩靖, 陈慧. 基于改进人工势场法的无人驾驶车辆局部路径规划的研究[J]. 汽车工程, 2013, 35(9):808-811.

[36] 王凡, 冯楠, 胡小鹏. 一种基于RRT-ConCon改进的路径规划算法[J]. 大连理工大学学报, 2014, (6):637-643.

[37] Rösmann C, Feiten W, Wösch T, et al. Trajectory modification considering dynamic constraints of autonomous robots[C]//ROBOTIK 2012; 7th German Conference on Robotics. VDE, 2012: 1-6.

[38] Liu Z, Zhang F, Hong X. Low-cost retina-like robotic lidars based on incommensurable scanning[J]. IEEE/ASME Transactions on Mechatronics, 2021.

[39] Chin E, Olafsson S, Nel D. Problems and Solutions in Mathematical Finance: Stochastic Calculus[M]. John Wiley & Sons, 2014.

[40] 杨红娟, 陈继文, 张运楚, 等. 基于SIFT特征的低区分度点云数据匹配[J]. 计算机辅助设计与图形学学报, 2016, 28(3): 498-504.

[41] Bay H, Ess A, Tuytelaars T, et al. Speeded-up robust features (SURF)[J]. Computer vision and image understanding, 2008, 110(3): 346-359.

[42] 李小红, 谢成明, 贾易臻, 等. 基于ORB特征的快速目标检测算法[J]. 电子测量与仪器学报, 2013, 27(5): 455-460.

[43] Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[C]//2011 International conference on computer vision. Ieee, 2011: 2564-2571.

[44] Harris C G, Stephens M. A combined corner and edge detector[C]//Alvey vision conference. 1988, 15(50): 10-5244.

[45] Bouguet J Y. Pyramidal implementation of the affine lucas kanade feature tracker description of the algorithm[J]. Intel corporation, 2001, 5(1-10): 4.

[46] Kümmerle R, Grisetti G, Strasdat H, et al. g 2 o: A general framework for graph optimization[C]//2011 IEEE International Conference on Robotics and Automation. IEEE, 2011: 3607-3613.

[47] 弗兰克·德尔阿特, 迈克尔·克斯, 刘富强. 机器人感知: 因子图在SLAM中的应用[M]. 北京：电子工业出版社, 2018.10:12-15.

[48] Kaess M, Johannsson H, Roberts R, et al. iSAM2: Incremental smoothing and mapping using the Bayes tree[J]. The International Journal of Robotics Research, 2012, 31(2): 216-235.

[49] 高翔，张涛，等.视觉SLAM十四讲[M].北京：电子工业出版社, 2017: 1-2.

[50] 陈常. 基于视觉和惯导融合的巡检机器人定位与建图技术研究[D]. 徐州：中国矿业大学, 2019.

[51] Zhang Z. A flexible new technique for camera calibration[J]. IEEE Transactions on pattern analysis and machine intelligence, 2000, 22(11): 1330-1334.

[52] Rehder J, Nikolic J, Schneider T, et al. Extending kalibr: Calibrating the extrinsics of multiple IMUs and of individual axes[C]//2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016: 4304-4311.

[53] Yang Z, Shen S. Monocular visual–inertial state estimation with online initialization and camera–IMU extrinsic calibration[J]. IEEE Transactions on Automation Science and Engineering, 2016, 14(1): 39-51.

[54] Furgale P, Rehder J, Siegwart R. Unified temporal and spatial calibration for multi-sensor systems[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013: 1280-1286.

[55] Maye J, Furgale P, Siegwart R. Self-supervised calibration for robotic systems[C]//2013 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2013: 473-480.

[56] Furgale P, Barfoot T D, Sibley G. Continuous-time batch estimation using temporal basis functions[C]//2012 IEEE International Conference on Robotics and Automation. IEEE, 2012: 2088-2095.

[57] Hughes P C. Spacecraft attitude dynamics[M]. Courier Corporation, 2012.

[58] Moré J J. The Levenberg-Marquardt algorithm: implementation and theory[M]//Numerical analysis. Springer, Berlin, Heidelberg, 1978: 105-116.

[59] Cui J, Niu J, Ouyang Z, et al. ACSC: Automatic Calibration for Non-repetitive Scanning Solid-State LiDAR and Camera Systems[J]. arXiv preprint arXiv:2011.08516, 2020.

[60] Ioannou Y, Taati B, Harrap R, et al. Difference of normals as a multi-scale operator in unorganized point clouds[C]//2012 Second International Conference on 3D Imaging, Modeling, Processing, Visualization & Transmission. IEEE, 2012: 501-508.

[61] Yan Y, Mao Y, Li B. Second: Sparsely embedded convolutional detection[J]. Sensors, 2018, 18(10): 3337.

[62] Zhou Y, Tuzel O. Voxelnet: End-to-end learning for point cloud based 3d object detection[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2018: 4490-4499.

[63] Long J, Shelhamer E, Darrell T. Fully convolutional networks for semantic segmentation[C]//Proceedings of the IEEE conference on computer vision and pattern recognition. 2015: 3431-3440.

[64] Vasudevan A, Anderson A, Gregg D. Parallel multi channel convolution using general matrix multiplication[C]//2017 IEEE 28th international conference on application-specific systems, architectures and processors (ASAP). IEEE, 2017: 19-24.

[65] Girshick R. Fast r-cnn[C]//Proceedings of the IEEE international conference on computer vision. 2015: 1440-1448.

[66] Liu W, Anguelov D, Erhan D, et al. Ssd: Single shot multibox detector[C]//European conference on computer vision. Springer, Cham, 2016: 21-37.

[67] Dellaert F, Kaess M. Square Root SAM: Simultaneous localization and mapping via square root information smoothing[J]. The International Journal of Robotics Research, 2006, 25(12): 1181-1203.

[68] Henein M, Zhang J, Mahony R, et al. Dynamic SLAM: The need for speed[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 2123-2129.

[69] Polok L, Solony M, Ila V, et al. Efficient implementation for block matrix operations for nonlinear least squares problems in robotic applications[C]//2013 IEEE International Conference on Robotics and Automation. IEEE, 2013: 2263-2269.

[70] Kim G, Kim A. Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4802-4809.

[71] Wang H, Wang C, Xie L. Intensity scan context: Coding intensity and geometry relations for loop closure detection[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 2095-2101.

[72] Calonder M, Lepetit V, Strecha C, et al. Brief: Binary robust independent elementary features[C]//European conference on computer vision. Springer, Berlin, Heidelberg, 2010: 778-792.

[73] Segal A, Haehnel D, Thrun S. Generalized-icp[C]//Robotics: science and systems. 2009, 2(4): 435.

[74] The Schur complement and its applications[M]. Springer Science & Business Media, 2006.

[75] Geiger A, Lenz P, Stiller C, et al. Vision meets robotics: The kitti dataset[J]. The International Journal of Robotics Research, 2013, 32(11): 1231-1237.

[76] Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012: 573-580.

[77] Horn B K P. Closed-form solution of absolute orientation using unit quaternions[J]. Josa a, 1987, 4(4): 629-642.

[78] Hornung A, Wurm K M, Bennewitz M, et al. OctoMap: An efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous robots, 2013, 34(3): 189-206.

[79] Wilhelms J, Van Gelder A. Octrees for faster isosurface generation[J]. ACM Transactions on Graphics (TOG), 1992, 11(3): 201-227.

[80] Meagher D. Geometric modeling using octree encoding[J]. Computer graphics and image processing, 1982, 19(2): 129-147.

[81] 孟祥天. 陌生环境下机器人自主建图的研究与实现[D]. 成都：电子科技大学,2020.

[82] Samson C. Control of chained systems application to path following and time-varying point-stabilization of mobile robots[J]. IEEE transactions on Automatic Control, 1995, 40(1): 64-77.

[83] De Luca A, Oriolo G, Samson C. Feedback control of a nonholonomic car-like robot[M]//Robot motion planning and control. Springer, Berlin, Heidelberg, 1998: 171-253.

[84] Rong G, Shin B H, Tabatabaee H, et al. Lgsvl simulator: A high fidelity simulator for autonomous driving[C]//2020 IEEE 23rd International Conference on Intelligent Transportation Systems (ITSC). IEEE, 2020: 1-6.