随着科技的不断进步，无人机技术正朝着智能化和自主化方向快速发展，特别是在复杂环境中的应用。为了确保任务的顺利完成，无人机需要依赖高精度的定位与控制系统、强大的环境感知能力以及高效的轨迹规划算法。在此需求下，本文研究了无人机在复杂环境中的自主导航技术，重点研究了抗干扰控制、多传感器融合定位以及基于深度图像的轨迹规划等关键技术，并通过自主搭建的硬件实物平台验证了所提算法的有效性和可行性。

首先，本文基于 Newton-Euler 方法建立无人机的动力学与运动学模型。针对复杂环境中的抗干扰问题，在传统反步法的基础上引入了自适应因子，并结合非线性自抗扰姿态控制技术，设计了一种BSP-ADRC（Backstepping-Active Disturbance Rejection Control）自抗扰控制算法。仿真结果表明，该算法能够有效抑制外界干扰，提高轨迹跟踪精度。

其次，本文设计了视觉-惯性紧耦合融合定位系统。在前端，采用基于Shi-Tomasi 角点检测和反向光流的方法对连续帧中的特征点进行精确跟踪，通过EPnP（Efficient Perspective-n-Point）方法实现帧间位姿的高效求解，同时引入IMU（Inertial Measurement Unit）预积分来求解惯性测量单元的位姿。为解决图优化中的规模爆炸问题，后端采用了滑动窗口优化技术；同时，引入回环检测并通过全局位姿优化消除长期定位漂移和累积误差。实验结果表明，该定位系统具有分米级的定位精度。

接着，本文研究了基于深度图像的环境感知与轨迹规划技术。重点分析了双目稠密点云重建原理，并基于此构建了环境的八叉树地图和ESDF（Euclidean Signed Distance Field）地图。在轨迹规划方面，提出了一种前端使用动力学A*路径搜索，后端基于ESDF软约束进行轨迹优化的框架，并通过实验分析了关键参数对轨迹优化效果的影响，以确保轨迹规划的安全性和平滑性。

最后，本文搭建了无人机导航系统硬件验证平台，对本文所提算法进行了验证，分别设计了定点悬停、圆形轨迹跟踪及复杂环境下的避障实验。实验结果表明，本文提出的算法能够提升无人机在复杂环境中的自主导航能力，具有重要的应用价值。

With the continuous advancement of science and technology, UAV technology is rapidly evolving toward greater intelligence and autonomy, particularly in complex environments. To ensure the successful completion of missions, UAVs must rely on high-precision localization and control systems, powerful environmental sensing capabilities, and efficient trajectory planning algorithms. In response to this need, this paper investigates the autonomous navigation technology of UAVs in complex environments, focusing on key technologies such as anti-jamming control, multi-sensor fusion localization, and trajectory planning based on depth images. The effectiveness and feasibility of the proposed algorithms are verified through an independently built hardware platform.

First, this paper establishes the dynamics and kinematics model of the UAV using the Newton-Euler method. To address the anti-jamming issue in complex environments, an adaptive factor is introduced into the traditional backstepping method, and a BSP-ADRC self-imposed control algorithm is designed by combining nonlinear self-imposed attitude control technology. Simulation results demonstrate that the algorithm can effectively suppress external interference and improve trajectory tracking accuracy.

Second, this paper designs a visual-inertial tightly coupled fusion localization system. In the front-end, feature points in consecutive frames are accurately tracked using the Shi-Tomasi corner point detection method and reverse optical flow. The efficient solution of inter-frame positions is achieved through the EPnP method, and IMU pre-integration is also introduced to solve the position of the inertial measurement unit. To address the scale explosion problem in graph optimization, the sliding window optimization technique is applied in the back-end. Meanwhile, loopback detection is introduced, and long-term positioning drift and cumulative errors are eliminated through global position optimization. Experimental results show that the positioning system achieves decimeter-level accuracy.

Next, this paper investigates environmental sensing and trajectory planning technologies based on depth images. It focuses on analyzing the principle of binocular dense point cloud reconstruction and constructing the octree map and ESDF map of the environment. In terms of trajectory planning, a framework is proposed that uses kinodynamic A* path search at the front end and trajectory optimization based on ESDF soft constraints at the back end. The influence of key parameters on the effectiveness of trajectory optimization is analyzed through experiments to ensure the safety and smoothness of trajectory planning.

Finally, this paper builds a hardware verification platform for the UAV navigation system and verifies the algorithms proposed herein. It also designs fixed-point hovering, circular trajectory tracking, and obstacle avoidance experiments in complex environments. The experimental results show that the algorithm proposed in this paper can enhance the autonomous navigation capabilities of UAVs in complex environments, demonstrating significant practical application value.

[1] Lai X, Yang F X. A robust Kalman filter for SINS/GPS integrated Navigation[C]//2021 IEEE 4th Advanced Information Management, Communicates, Electronic and Automation Control Conference (IMCEC), Chongqing, China: IEEE, 2021: 238-242.

[2] 董竹林. 无人旋翼飞行器光流/惯性组合导航系统的设计与实现[D]. 南京: 南京航空航天大学, 2020.

[3] 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-36.

[4] 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), Stockholm, Sweden: IEEE, 2016: 1271-1278.

[5] Zhang J, Singh S. LOAM: Lidar Odometry and Mapping in Real-time[C]//Robotics: Science and systems, 2014, 2(9): 1-9.

[6] Li J, Zhao J Q, Kang Y C, et al. DL-SLAM: Direct 2.5D LiDAR SLAM for Autonomous Driving[C]//2019 IEEE Intelligent Vehicles Symposium (IV), Paris, France: IEEE, 2019: 1205-1210.

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

[8] Shan T X, Englot B, Meyers D, et al. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA: IEEE, 2020: 5135-5142.

[9] Klein G, Murray D. Parallel Tracking and Mapping for Small AR Workspaces[C]//2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality, Nara, Japan: IEEE, 2007: 225-234.

[10] Davison A J, Reid I D, Molton N D, et al. MonoSLAM: Real-Time Single Camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067.

[11] Newcombe R A, Lovegrove S J, Davison A J. DTAM: Dense tracking and mapping in real-time[C]//2011 International Conference on Computer Vision, Barcelona, Spain: IEEE, 2011: 2320-2327.

[12] Engel J, Schöps T, Cremers D. LSD-SLAM: Large-Scale Direct Monocular SLAM[C]// Computer Vision – ECCV 2014, Zurich, Switzerland: Springer, Cham, 2014: 834-849.

[13] Mur-Artal R, Montiel J M M, Tardós J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

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

[15] Weiss S M. Vision based navigation for micro helicopters[D]. Zurich: ETH Zurich, 2012.

[16] Leutenegger S, Furgale P, Rabaud V, et al. Keyframe-Based Visual-Inertial SLAM using Nonlinear Optimization[J]. Proceedings of Robotis Science and Systems (RSS), 2013, 2013.

[17] Bloesch M, Burri M, Omari S, et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. The International Journal of Robotics Research, 2017, 36(10): 1053-1072.

[18] Sun K, Mohta K, Pfrommer B, et al. Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.

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

[20] Zou D, Wu Y, Pei L, et al. StructVIO: Visual-Inertial Odometry With Structural Regularity of Man-Made Environments[J]. IEEE Transactions on Robotics, 2019, 35(4): 999-1013.

[21] Campos C, Elvira R, Rodríguez J J G, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM[J]. IEEE Transactions on Robotics, 2021,

37(6): 1874-1890.

[22] Fu Q, Wang J, Yu H S, et al. PL-VINS: Real-Time Monocular Visual-Inertial SLAM with Point and Line Features[J]. arXiv preprint arXiv: 2009.07462, 2020.

[23] Lee J, Park S Y. PLF-VINS: Real-Time Monocular Visual-Inertial SLAM With Point-Line Fusion and Parallel-Line Fusion[J]. IEEE Robotics and Automation Letters, 2021, 6(4): 7033-7040.

[24] Sun Y H, Fang M, Su Y X. AGV Path Planning based on Improved Dijkstra Algorithm[J]. Journal of Physics: Conference Series, 2021, 1746(1): 012052.

[25] Sedeño-noda A, Colebrook M. A biobjective Dijkstra algorithm[J]. European Journal of Operational Research, 2019, 276(1): 106-118.

[26] Nie Z, Zhao H. Research on Robot Path Planning Based on Dijkstra and Ant Colony Optimization[C]//2019 International Conference on Intelligent Informatics and Biomedical Sciences (ICIIBMS), Shanghai, China: IEEE, 2019: 222-226.

[27] 张瑞鑫, 王伟, 田泽, 等. 基于模型约束A~*算法的无人机三维航迹规划[J]. 国外电子测量技术, 2022, 41(9): 163-169.

[28] Li D C, Yin W P, Wong W E, et al. Quality-Oriented Hybrid Path Planning Based on A* and Q-Learning for Unmanned Aerial Vehicle[J]. IEEE Access, 2022, 10: 7664-7674.

[29]Kong X D, Pan B, Cherkashin E, et al. Multi-constraint UAV Fast Path Planning Based on Improved A* Algorithm[J]. Journal of Physics: Conference Series, 2020, 1624(4): 042009.

[30] Qi B K, Li M Q, Yang Y, et al. Research on UAV path planning obstacle avoidance algorithm based on improved artificial potential field method[J]. Journal of Physics: Conference Series, 2021, 1948(1): 012060.

[31] Huang T P, Huang D Q, Qin N, et al. Path Planning and Control of a Quadrotor UAV Based on an Improved APF Using Parallel Search[J]. International Journal of Aerospace Engineering, 2021, 2021(1): 5524841.

[32] Wu E M, Sun Y D, Huang J Y, et al. Multi UAV Cluster Control Method Based on Virtual Core in Improved Artificial Potential Field[J]. IEEE Access, 2020, 8: 131647-131661.

[33] Yuan W J, Shi Y. UAV Path Planning Based on APF-Q-Learning[C]//6GN for Future Wireless Networks. Chengdu, China: Springer, Cham, 2023: 290-300.

[34] Feng J X, Zhang J Z, Zhang G, et al. UAV Dynamic Path Planning Based on Obstacle Position Prediction in an Unknown Environment[J]. IEEE Access, 2021, 9: 154679-154691.

[35] Li W M, Wang L, Zou A W, et al. Path Planning for UAV Based on Improved PRM[J]. Energies, 2022, 15(19): 7267.

[36] 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, Detroit, MI, USA: IEEE, 1999, 2: 1018-1023.

[37] Hsu D, Jiang T T, Reif J, et al. The bridge test for sampling narrow passages with probabilistic roadmap planners[C]//2003 IEEE International Conference on Robotics and Automation, Taipei, Taiwan: IEEE, 2003, 3: 4420-4426.

[38] Cao K, Cheng Q, Gao S, et al. Improved PRM for Path Planning in Narrow Passages[C]//2019 IEEE International Conference on Mechatronics and Automation (ICMA), Tianjin, China: IEEE, 2019: 45-50.

[39] Yuan F, Liang J H, Fu Y W, et al. A hybrid sampling strategy with optimized Probabilistic Roadmap Method[C]//2015 12th International Conference on Fuzzy Systems and Knowledge Discovery (FSKD), Zhangjiajie, China: IEEE, 2015: 2298-2302.

[40] Chen J B, Zhou Y M, Gong J, et al. An Improved Probabilistic Roadmap Algorithm with Potential Field Function for Path Planning of Quadrotor[C]//2019 Chinese Control Conference (CCC), Guangzhou, China: IEEE, 2019: 3248-3253.

[41] Ye L J, Chen J B, Zhou Y M. Real-Time Path Planning for Robot Using OP-PRM in Complex Dynamic Environment[J]. Frontiers in Neurorobotics, 2022, 16: 910859.

[42] Ravankar A A, Ravankar A, Emaru T, et al. HPPRM: Hybrid Potential Based Probabilistic Roadmap Algorithm for Improved Dynamic Path Planning of Mobile Robots[J]. IEEE Access, 2020, 8: 221743-221766.

[43] Liu C Y, Xie S B, Sui X, et al. PRM-D* Method for Mobile Robot Path Planning[J]. Sensors, 2023, 23(7): 3512.

[44] You H, Chen G, Jia Q X, et al. Path Planning for Robot in Multi-dimensional Environment Based on Dynamic PRM Blended Potential Field[C]//2021 IEEE 5th Information Technology, Networking, Electronic and Automation Control Conference (ITNEC), Xi'an, China: IEEE , 2021: 1157-1162.

[45] Todi V, Sengupta G, Bhattacharya S. Probabilistic Path Planning using Obstacle Trajectory Prediction[C]//Proceedings of the ACM India Joint International Conference on Data Science and Management of Data. New York, NY, USA: Association for Computing Machinery, 2019: 36-43.

[46] Liu H, Xiao F, Wang C. A predictive model for narrow passage path planner by using Support Vector Machine in changing environments[C]//2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA: IEEE, 2015: 2991-2996.

[47] Gammell J D, Srinivasa S S, Barfoot T D. Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA:IEEE, 2014: 2997-3004.

[48] Xu J X, Tian Z S, He W, et al. A Fast Path Planning Algorithm Fusing PRM and P-BiRRT[C]//2020 11th International Conference on Prognostics and System Health Management, Jinan, China: IEEE, 2020: 503-508.

[49] Yang F, Fang X, Gao F, et al. Obstacle Avoidance Path Planning for UAV Based on Improved RRT Algorithm[J]. Discrete Dynamics in Nature and Society, 2022, 2022(1): 4544499.

[50] 刘奎武. 基于改进RRT算法的无人机航迹规划方法研究[D]. 沈阳: 沈阳航空航天大学, 2022.

[51] Mechali O, Xu L M, Wei M Z, et al. A Rectified RRT* with Efficient Obstacles Avoidance Method for UAV in 3D Environment[C]//2019 IEEE 9th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems (CYBER), Suzhou, China: IEEE, 2019: 480-485.

[52] Mashayekhi R, Idris M Y I, Anisi M H, et al. Informed RRT*-Connect: An Asymptotically Optimal Single-Query Path Planning Method[J]. IEEE Access, 2020, 8: 19842-19852.

[53] Wang J K, Li B P, Meng M Q H. Kinematic Constrained Bi-directional RRT with Efficient Branch Pruning for robot path planning[J]. Expert Systems with Applications, 2021, 170: 114541.

[54] Yuan C R, Liu G F, Zhang W Q, et al. An efficient RRT cache method in dynamic environments for path planning[J]. Robotics and Autonomous Systems, 2020, 131: 103595.

[55] Grothe F, Hartmann V N, Orthey A, et al. ST-RRT*: Asymptotically-Optimal Bidirectional Motion Planning through Space-Time[C]//2022 International Conference on Robotics and Automation (ICRA), Philadelphia, PA, USA: IEEE, 2022: 3314-3320.

[56] Zhang J C, An Y Q, Cao J N, et al. UAV Trajectory Planning for Complex Open Storage Environments Based on an Improved RRT Algorithm[J]. IEEE Access, 2023, 11: 23189-23204.

[57] Jin H, Cui W, Fu H. Improved RRT-Connect Algorithm for Urban low-altitude UAV Route Planning[J]. Journal of Physics: Conference Series, 2021, 1948(1): 012048.

[58] Webb D J, Berg J van den. Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints[J]. arXiv preprint arXiv:1205.5088, 2012.

[59] Yin H, Li B, Zhu H, et al. Kinodynamic RRT* Based UAV Optimal State Motion Planning with Collision Risk Awareness[J]. Information Technology and Control, 2023, 52(3): 665-679.

[60] Cain B, Kalaitzakis M, Vitzilaios N. MK-RRT*: Multi-Robot Kinodynamic RRT Trajectory Planning[C]//2021 International Conference on Unmanned Aircraft Systems (ICUAS), Athens, Greece: IEEE, 2021: 868-876.

[61] Liu S K, Watterson M, Mohta Kartik, et al. Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments[J]. IEEE Robotics and Automation Letters, 2017, 2(3), 1688-1695.

[62] Chen J, Su K Y, Shen S J. Real-time safe trajectory generation for quadrotor flight in cluttered environments[C]//2015 IEEE International Conference on Robotics and Biomimetics (ROBIO), Zhuhai, China: IEEE, 2015: 1678-1685.

[63] Gao F, Shen S J. Online quadrotor trajectory generation and autonomous navigation on point clouds[C]//2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Lausanne, Switzerland: IEEE, 2016: 139-146.

[64] Gao F, Wu W, Lin Y, et al. Online Safe Trajectory Generation for Quadrotors Using Fast Marching Method and Bernstein Basis Polynomial[C]//2018 IEEE International Conference on Robotics and Automation (ICRA), QLD, Australia: IEEE, 2018: 344-351.

[65] Zucker M, Ratliff N, Dragan A D, et al. CHOMP: Covariant Hamiltonian optimization for motion planning[J]. The International Journal of Robotics Research, 2013, 32(9-10):11641193.

[66] Oleynikova H, Burri M, Taylor Z, et al. Continuous-time trajectory optimization for online UAV replanning[C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, Korea (South): IEEE, 2016: 5332-5339.

[67] 梁洪基, 李俊丽, 张元耀, 等. 基于自耦PD的四旋翼无人机轨迹跟踪控制[J]. 陕西理工大学学报(自然科学版), 2024, 40(1): 47-55+79.

[68] 焦环宇, 王春阳, 赵锦楠. 四旋翼无人机串级自抗扰控制及参数优化方法[J]. 国外电子测量技术, 2021, 40(11): 35-40.

[69] 刘春玲, 冯锦龙, 张瑾. 基于改进 PD 的 LESO 四旋翼控制算法[J]. 计算机仿真, 2023, 40(6): 42-46.

[70] 孙克文, 刘国宁, 杨允权, 等. 四旋翼飞行器抗干扰轨迹跟踪控制方法研究[J]. 机械设计与制造, 2023(12): 281-287+292.

[71] 孙伟杰, 万振刚. 面向小型四旋翼无人机的BPPID-LADRC 串级抗扰控制器[J]. 计算机与数字工程, 2023, 51(6): 1444-1449.

[72] 顾明琨, 钟小勇. 改进人工蜂群算法的四旋翼自抗扰控制参数优化[J]. 科学技术与工程, 2022, 22(14): 5693-5699.

[73] 唐志勇, 马福源, 裴忠才. 四旋翼的改进PSO-RBF神经网络自适应滑模控制[J]. 北京航空航天大学学报, 2023, 49(7): 1563-1572.

[74] 祝文星, 王立辉, 田栢苓. 基于有限时间观测器的四旋翼无人机积分滑模控制方法[J]. 中国惯性技术学报, 2023, 31(12): 1244-1253.

[75] 陆洪志, 段富海. 四旋翼无人机的高度和姿态容错控制研究[J]. 电光与控制, 2023, 30(12): 73-79.

[76] 刘祚时, 徐燕生, 穆鸿书. 飞行仿真器四旋翼姿态控制方法研究[J]. 传感器与微系统, 2023, 42(4): 59-62.

[77] 张鑫, 吴伊凡, 崔永琪. 基于改进MPC算法的四旋翼无人机轨迹跟踪控制[J]. 电子技术与软件工程, 2023(3): 148-153.

[78] 牛康. 编队无人机航路规划与轨迹跟踪技术研究[D].南京: 南京航空航天大学, 2019.

[79] 韩京清. 自抗扰控制技术[J]. 前沿科学, 2007 (1): 24-31.

[80] 厉宽宽, 陈允芳, 程敏, et al. MEMS-IMU 随机误差的Allan 方差分析[J]. 全球定位系统, 2016, 41(6): 102-106.

[81] 赖焕杰, 孟祥印, 肖世德, 等. 基于 RANSAC 的单应性矩阵估计优化算法[J]. 传感器与微系统, 2023, 42(8): 135-138.

[82] 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, Tokyo, Japan: IEEE, 2013: 1280-1286.

[83] 周松. 基于点线特征融合的VINS系统研究[D]. 成都: 电子科技大学, 2024.

[84] Lepetit V, Moreno-Noguer F, Fua P. EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of Computer Vision, 2009, 81(2): 155-166.

[85] Han L, Gao F, Zhou B, et al. FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China: IEEE, 2019: 4423-4430.