Research on Autonomous Navigation of Unmanned Aerial Vehicle Based on Visual SLAM and Path Planning

1. Introduction

1.1 Research Background and Significance

With the rapid advancement of contemporary technology, intelligent unmanned systems are progressively replacing human beings in executing high-risk, precision-demanding, or monotonous operational tasks. Among these systems, the unmanned aerial vehicle (UAV), particularly the quadrotor configuration, stands out due to its exceptional maneuverability and operational flexibility. Capable of both remote control and intelligent autonomous flight, UAVs have demonstrated tremendous application value across various sectors of society.

From a practical perspective, these aerial platforms have formed a comprehensive industrial system in civilian applications, primarily serving professional domains such as ecological environment observation, geographic information acquisition, power grid facility inspection, aerial imagery recording, industrial equipment diagnosis, precision agriculture management, and emergency disaster relief command. In the realm of national defense, application priorities include border security, battlefield reconnaissance, and precision guidance for military operations. During the COVID-19 pandemic, UAVs innovatively undertook critical missions including epidemic prevention knowledge dissemination, public space disinfection, traffic order management, population health screening, and material transport in quarantine zones, providing essential technical support for public health safety.

The realization of autonomous flight without human assistance constitutes the core prerequisite for UAVs to accomplish diverse missions. Within this context, autonomous positioning and pose estimation technology emerges as the key technical pillar enabling autonomous flight. This technology empowers UAVs to operate autonomously in unstructured environments and complete specific tasks without human intervention, making it a focal research direction in the current UAV field. During the process of safely and efficiently navigating to target positions through autonomous navigation systems, UAV performance predominantly depends on the accuracy of environmental information acquisition and the precision of self-localization.

However, under complex environmental conditions, traditional sensors exhibit significant limitations in communication capability and environmental perception, impeding UAVs from accurately sensing surrounding environments and making appropriate decisions. Although substantial research efforts have been devoted to addressing these technical bottlenecks, more effective solutions still require exploration. Therefore, enhancing the high-performance autonomous navigation capability of UAVs holds significant theoretical and practical value for technological development and application expansion.

Currently, Simultaneous Localization and Mapping (SLAM) technology for UAVs has achieved remarkable progress. Based on the type of sensors employed, it can be primarily categorized into: vision-based SLAM, LiDAR SLAM, optical reflection positioning, Inertial Measurement Unit (IMU) systems, satellite navigation systems, ultrasonic positioning, spatial beacon navigation, and WiFi/Bluetooth-based positioning technologies. Each technology possesses distinct characteristics, providing diversified solutions for UAV navigation in different application scenarios.

The Global Navigation Satellite System (GNSS), as a passive positioning technology, has been widely applied in both civilian and military domains. By analyzing space satellite signals, it can provide centimeter-level absolute position information and relative displacement data. However, its performance exhibits significant environmental dependence: on one hand, urban canyons, dense vegetation, or severe weather conditions can cause signal attenuation or even interruption; on the other hand, system positioning reliability directly depends on the number and quality of visible satellites, and in scenarios with limited satellite signals such as indoor environments, navigation performance deteriorates sharply. More notably, in military countermeasure environments, the system faces security threats including deliberate jamming and spoofing attacks, severely constraining its operational effectiveness.

Vision-based navigation technology, as an important branch in the field of autonomous systems, has gained widespread attention in recent years thanks to breakthroughs in computer vision. These methods, leveraging their unique perceptual advantages, demonstrate tremendous development potential and application value in autonomous navigation research for unmanned systems. The extensive attention received by vision-based navigation technology from academia and industry primarily stems from several notable advantages: first, vision sensors provide high-resolution environmental information with rich texture and geometric features, offering more precise environmental perception and positioning support for navigation systems. Second, in complex environments, particularly dynamic scenes, vision sensors exhibit strong anti-interference capabilities, effectively addressing challenges such as illumination changes and target motion, with unique advantages in dynamic environment perception and real-time decision-making. Furthermore, vision sensors possess high concealment and security; their passive sensing characteristics make them less susceptible to external detection or interference, enhancing system reliability and applicability. Finally, the camera equipment relied upon by visual SLAM technology offers advantages of low cost, light weight, and easy availability—for instance, smartphone built-in cameras and high-pixel, miniaturized action cameras all provide convenient conditions for the popularization and widespread application of vision-based navigation technology.

Nevertheless, despite the notable advantages of vision-based navigation methods, their practical application still presents certain limitations, primarily reflected in two aspects: first, vision navigation systems require real-time processing of substantial image information, imposing high demands on computational resources. This typically necessitates high-performance processors or specialized computing units such as GPUs or FPGAs. Meanwhile, complex image processing algorithms may lead to prolonged computation times, affecting system real-time performance. Second, under specific environmental conditions (such as large white walls, glass curtain walls, and other scenes lacking significant texture features), vision systems struggle to extract sufficient effective feature points, leading to decreased depth information computation accuracy or even failure, subsequently affecting navigation system stability and reliability. These technical bottlenecks not only restrict the application scope of vision-based navigation methods but also provide important improvement directions for subsequent research, such as enhancing system robustness and environmental adaptability through multi-sensor fusion and deep learning-assisted feature extraction.

Current navigation systems, although performing well under specific conditions, still face numerous technical bottlenecks. With the expansion of UAV application domains and increasing environmental uncertainty, single-sensor-mode navigation solutions can no longer meet the demands of complex scenarios. Consequently, integrated navigation technology fusing multi-source information is increasingly becoming a research focus due to its robustness and environmental adaptability advantages. Typical integrated schemes include: satellite-inertial navigation integration, optical flow-IMU fusion, and vision-inertial integrated systems. Among these, satellite-inertial integration performs excellently in open outdoor environments but cannot address indoor navigation problems; optical flow-inertial schemes are suitable for indoor scenarios but require the UAV to maintain a horizontal attitude, and image-sequence-based displacement integration introduces cumulative errors; while vision-inertial integrated systems, leveraging their excellent indoor navigation performance, have become a focus of attention in both academia and industry.

1.2 Research Status

In indoor and special environments where GPS signals are absent, quadrotor UAVs face two major technical challenges: achieving precise pose estimation through multi-sensor fusion technology in the absence of prior location information, and designing efficient trajectory planning algorithms to ensure mission execution reliability in complex indoor environments.

For positioning and mapping technology in denied environments, researchers generally adopt multi-sensor fusion strategies that integrate observations from heterogeneous sensors including vision sensors, inertial measurement units, and LiDAR to achieve autonomous positioning. In the field of unknown environment navigation, the problem is typically modeled within the SLAM framework. Among the technical approaches, Visual-Inertial SLAM (VI-SLAM) systems have gained particular prominence, with front-end pose estimation modules referred to as Visual-Inertial Odometry (VIO).

According to differences in back-end optimization strategies, existing VI-SLAM systems are primarily divided into two technical routes: probability filtering-based estimation methods and nonlinear graph optimization-based solving methods.

For filtering-based visual-inertial SLAM, Mourikis proposed a vision-aided inertial navigation algorithm based on Extended Kalman Filter (EKF), implementing real-time high-precision pose estimation through a Multi-State Constraint Kalman Filter (MSCKF). This approach designs a measurement model that exploits geometric constraints from static features observed across multiple camera poses without adding 3D feature points to the state vector, thereby reducing computational complexity. Bloesch developed ROVIO, a robust visual-inertial odometry based on direct EKF, which utilizes pixel intensity errors of image patches directly as filter innovation terms, achieving high-precision pose estimation with strong robustness and real-time performance. Su proposed a robust stereo visual-inertial odometry algorithm based on MSCKF suitable for fast autonomous flight. OpenVINS, an open-source visual-inertial odometry system built upon the MSCKF framework, supports online calibration of camera intrinsic and extrinsic parameters and implements consistent first-order estimation Jacobian computation.

For nonlinear optimization-based visual-inertial SLAM, Leutenegger proposed a keyframe-based nonlinear optimization method tightly integrating IMU data, achieving real-time visual-inertial SLAM by establishing a joint probability model containing IMU error terms and reprojection errors. Qin proposed VINS-Mono, a robust and versatile monocular visual-inertial state estimator that achieves precise 6-DOF state estimation through tightly-coupled nonlinear optimization combining IMU pre-integration and feature observations. Campos further proposed ORB-SLAM3, an open-source visual, visual-inertial, and multi-map SLAM system supporting monocular, stereo, and RGB-D cameras. This algorithm performs tightly-coupled visual-inertial fusion based on maximum a posteriori estimation, implements improved location recognition methods with geometric consistency verification, and supports cross-session map reuse through an Atlas system. To further improve accuracy and robustness, some researchers have proposed indoor visual SLAM systems combining ORB-SLAM3 with object detection networks, maintaining robust performance in dynamic environments while achieving high-precision positioning, with IMU auxiliary information significantly enhancing system robustness and accuracy.

Category Algorithm Core Technique Characteristic
Filtering-based MSCKF Multi-state constraint Kalman filter Linear complexity, high efficiency
ROVIO Direct EKF with pixel intensity Robust, no initialization required
S-MSCKF Stereo MSCKF Suitable for fast flight
OpenVINS Sliding window Kalman filter Modular, online calibration
Optimization-based Keyframe VI-SLAM Nonlinear optimization with IMU High accuracy, robust
VINS-Mono Tightly-coupled nonlinear optimization Versatile, 6-DOF estimation
ORB-SLAM3 MAP-based visual-inertial fusion Multi-map, high robustness

For dense mapping, current mainstream representation methods include dense point cloud maps, probabilistic grid maps, octree maps, and Signed Distance Field (SDF) representations. Dense point cloud maps, constructed using depth sensors, precisely characterize environmental geometric structures. However, they face significant limitations in UAV navigation applications due to substantial data volume containing redundant information, imposing severe challenges on onboard computing resources. Additionally, raw point cloud data is difficult to directly utilize for path planning algorithms. Therefore, converting raw point cloud data into lightweight map representations better suited for real-time processing is typically necessary.

Occupancy grid maps, derived by discretizing dense point cloud maps into uniform voxel units, are widely adopted in UAV path planning due to their fast and simple accessibility. However, three-dimensional grid maps require substantial storage resources, and memory requirements multiply with increasing resolution. To address storage efficiency shortcomings of traditional grid maps, octree-based environmental representation methods have been proposed. This approach employs hierarchical recursive spatial partitioning strategies, dynamically adjusting spatial resolution to achieve data compression. Since obstacle distribution in real environments typically exhibits spatial continuity, this adaptive resolution mechanism significantly reduces memory consumption while maintaining environmental representation accuracy, effectively improving UAV map construction and autonomous navigation efficiency.

Current UAV SLAM technology, despite significant progress in environmental modeling and autonomous positioning, still faces several critical issues requiring resolution. In terms of sensor configuration, existing systems commonly rely on expensive sensing devices such as high-precision depth cameras or LiDAR, increasing system costs and posing severe challenges to the payload capacity of micro-miniature UAVs. Regarding environmental adaptability, limited by onboard sensors’ restricted field of view and measurement range, coupled with dynamic obstacle interference in complex scenes, map completeness in unstructured environments cannot be guaranteed.

1.3 Main Contributions

This paper designs a complete autonomous navigation scheme based on binocular vision, aiming to achieve high-maneuverability flight control in complex indoor spaces. A binocular visual SLAM framework based on ORB-SLAM3 with line feature fusion is proposed for high-precision autonomous positioning and octree map construction for autonomous navigation. Based on the three-dimensional environmental map, a fast path planning algorithm is employed to plan routes from start to goal, with the UAV controlled to follow the planned trajectory, ultimately completing the autonomous navigation mission from start to goal.

The main research contents are organized as follows:

Aiming at the high-precision autonomous positioning problem of quadrotor UAVs, this research improves upon the ORB-SLAM3 framework to enhance tracking stability and environmental modeling robustness in low-texture environments. By introducing point-line feature fusion technology, ORB feature points and line segment features are collaboratively optimized, significantly enhancing the environmental adaptability of the visual SLAM system. For the three-dimensional environmental perception problem in denied environments, an octree-based three-dimensional occupancy grid mapping method utilizing depth information is analyzed. This method first employs a binocular camera for real-time scene depth data acquisition, then calculates the probability of grid cells being occupied by obstacles for dynamic environmental representation updates, and finally efficiently maintains and updates the three-dimensional environmental model through octree data structures.

To address real-time and safety issues in complex three-dimensional spatial environments, a path planning method combining improved Informed-RRT* algorithm with Artificial Potential Field (APF) algorithm is proposed. Compared with traditional Informed-RRT* algorithms, the proposed fusion algorithm reduces path length and planning time, eliminates numerous redundant nodes, and demonstrates effectiveness and superiority. The Informed-RRT* algorithm generates a collision-free path from start to goal in global space as the initial reference path for the Dynamic Window Approach (DWA) algorithm. In actual operation, the DWA algorithm dynamically adjusts control inputs based on the UAV’s current position, velocity, and reference path, ensuring real-time avoidance of dynamic obstacles and unknown static obstacles while staying as close to the reference path as possible.

The proposed algorithms are systematically validated using the EuRoC public dataset. Through quantitative comparison with ORB-SLAM3 regarding pose estimation accuracy, experimental data demonstrates significant advantages in positioning accuracy. Large-scale indoor environment data from the dataset is utilized to successfully construct octree-based three-dimensional grid maps, verifying mapping algorithm effectiveness. Path planning comparison experiments in three-dimensional environments show that the proposed algorithm outperforms comparison methods in path search efficiency and trajectory optimization quality. Finally, a complete UAV experimental platform based on the Robot Operating System (ROS) is constructed, including hardware and software frameworks, with actual flight tests verifying the reliability and practicality of the binocular vision navigation system.

2. UAV Autonomous Navigation System Design

2.1 System Overview

To address the autonomous navigation requirements of UAVs in denied environments, this research designs a complete system architecture comprising four core functional units: a multi-sensor fusion environmental perception unit integrating visual and inertial measurement data for environmental modeling; a high-precision positioning unit based on visual-inertial odometry ensuring reliable pose estimation in indoor environments; a motion planning unit with real-time response capability responsible for generating optimal flight paths; and a control execution unit considering UAV dynamic constraints for precise trajectory tracking. These modules work collaboratively to form a complete autonomous navigation solution effectively addressing flight control challenges in complex environments.

The perception system’s core function is constructing a dynamic interaction model between the UAV and its environment. Its technical path primarily encompasses two levels: first, utilizing various environmental sensing devices to collect raw environmental data, then extracting key environmental features through algorithms to ultimately achieve precise spatial positioning of the UAV. For different application scenarios, environmental perception algorithm research focuses differ significantly. For instance, in mapping and augmented reality applications, algorithms need to fully present environmental details, typically accepting higher computational resource consumption but with relatively relaxed real-time requirements. UAV navigation applications place greater emphasis on algorithm operational efficiency, requiring simplified navigation maps that occupy small storage space, offer fast construction speed, provide high positioning accuracy, and effectively identify dynamic obstacles.

Motion planning and control systems constitute the key technical link in UAV autonomous navigation, with their core function establishing an intelligent connection between environmental perception and flight execution. System design must satisfy multiple optimization objectives: pursuing geometric optimality in path quality, requiring real-time response capability in computational performance, and considering economic factors such as energy efficiency.

2.2 Visual SLAM Model

Visual SLAM utilizes visual information to complete probabilistic estimation of the UAV’s pose. With recent developments, visual SLAM has gradually divided into front-end and back-end modules, evolving into a complete framework. According to information processing flow, the visual SLAM framework can be broadly divided into five modules: sensor acquisition, visual odometry, back-end optimization, loop closure detection, and mapping.

Sensor input encompasses multiple data sources, with visual sensors being the primary data source. Additionally, the system integrates IMU data including acceleration and angular velocity information for fusion with visual odometry data to improve positioning accuracy. In multi-UAV cooperative navigation scenarios, the system also collects positioning data and ranging information from neighboring UAVs.

Visual odometry processes images captured by visual sensors to compute pose transformations between adjacent frames, providing initial pose estimation for the UAV. Specifically, the system first extracts feature points from images and encodes them using descriptors. Subsequently, by matching feature descriptors between adjacent keyframes, the pose transformation between two frames is computed. When using monocular cameras, since pixels contain only planar coordinate information, the system typically employs the eight-point method for relative pose transformation computation, though this method cannot recover scale information. When using binocular or RGB-D cameras, the system can obtain pixel depth information, enabling more accurate relative pose estimation through methods such as Iterative Closest Point (ICP).

The back-end optimization module further optimizes visual odometry outputs. Early visual SLAM systems primarily employed filtering-based optimization methods, treating pose data as state variables for optimization. However, filtering-based methods assume current frame states relate only to previous frames, causing errors to accumulate over time. To address this issue, subsequent research proposed nonlinear optimization-based methods that save all historical states as keyframes and perform global optimization on all historical states when new keyframes are added. During nonlinear optimization, the system uses reprojection errors between positioning results and pixel points as the objective function, leveraging the sparsity of the reprojection error Jacobian matrix to significantly improve computational efficiency.

Loop closure detection eliminates cumulative errors from long-term operation. This module annotates and records keyframes using the bag-of-words model. When two keyframes with high bag-of-words similarity are detected, the system adds constraints to these frames, effectively reducing error accumulation from prolonged operation.

The mapping module visualizes keyframe trajectories and map feature points. To meet real-time requirements, visual SLAM typically constructs sparse point cloud maps. While suitable for positioning, such maps cannot provide sufficient environmental information for navigation. Therefore, when UAVs plan paths based on visual SLAM, specialized dense maps must be constructed according to the actual physical environment to support more complex navigation tasks.

2.3 UAV Autonomous Positioning Model

When executing autonomous navigation tasks in unknown environments, UAVs need to determine their pose in real-time as the foundation for environmental perception and motion planning. Since UAV motion exhibits continuity while onboard sensors sample environmental information at discrete intervals, the core of the autonomous positioning problem lies in: based on sensor observation data at discrete time instants, continuously calculating the UAV’s position and attitude information in the global coordinate system through state estimation algorithms. This process requires addressing the temporal matching problem between continuous motion and discrete observations, providing precise pose reference for subsequent real-time mapping and path planning.

Different perception devices have significantly different measurement principles: inertial measurement units and encoders directly output motion parameters, while visual cameras, LiDAR, and other devices require indirect computation to obtain motion information. In the UAV autonomous positioning problem, the system’s state estimation framework comprises two core components: a dynamic model representing UAV motion characteristics and an observation model establishing the mapping relationship between sensor data and system states. The UAV’s motion behavior can be mathematically described through a nonlinear state transition equation:

$$
x_k = f(x_{k-1}, u_k, w_k)
$$

where $x_k$ represents the flight state vector at time $k$, $u_k$ represents internal motion sensor readings, and $w_k$ represents sensor random noise. The sensor observation model describing the relationship between sensors and actual UAV states can be expressed as:

$$
z_{k,j} = h(y_j, x_k, v_{k,j})
$$

where $z_{k,j}$ represents the observation value of environmental landmark $y_j$ when the UAV is at state $x_k$. The term $v_{k,j}$ characterizes measurement noise introduced during the measurement process. Based on these two models, the UAV autonomous positioning problem can be formalized as a state estimation problem: given control inputs $u$ and observations $z$, solve for the optimal estimate of the UAV state.

2.4 Binocular Camera Model

The binocular camera model is typically based on a stereo imaging device composed of two monocular cameras. During modeling, each camera can first be idealized to obtain the classic pinhole imaging model. The geometric relationship of the binocular camera can be equivalently treated as a combination of two pinhole models. By introducing nonlinear distortion parameters, the actual optical system’s imaging deviations can be more accurately characterized, thereby improving three-dimensional reconstruction accuracy.

Binocular cameras typically consist of two horizontally arranged cameras: the left camera and right camera. In this horizontal arrangement, both camera optical centers are on the same x-axis, with the horizontal distance between the two optical centers termed the baseline. Suppose a three-dimensional point $P$ in space projects onto the imaging planes of the left and right cameras, denoted as $P_L$ and $P_R$ respectively. Due to the baseline distance in the dual-camera system, these two projection points exhibit positional differences. In the standard binocular vision model, since the left and right cameras only undergo displacement along the x-axis, the imaging of point $P$ only differs along the x-axis. Let its left coordinate be $u_L$ and right coordinate be $u_R$. Through geometric similarity:

$$
\frac{z – b}{z} = \frac{u_R – u_L + b}{b}
$$

After rearrangement:

$$
z = \frac{fb}{d}
$$

$$
d = u_L – u_R
$$

where $f$ is the focal length, $b$ is the baseline, and $d$ is the disparity, defined as the horizontal coordinate difference of the same spatial point on the left and right camera imaging planes. Based on geometric optics principles, disparity is inversely related to target distance: larger disparity indicates smaller object distance. Notably, constrained by the minimum sampling unit of digital images, the minimum detectable disparity is one pixel, which directly determines the maximum ranging distance of binocular vision. Camera baseline length is positively correlated with maximum detection distance; increasing baseline dimensions effectively extends the ranging range.

Camera distortion primarily results from lens characteristics and the relative position between the lens and imaging plane. Radial distortion, one of the most typical types, arises from the nonlinear refraction effects caused by the lens’s curved surface structure. Radial distortion can be modeled using polynomial functions, with the Brown model being commonly employed:

$$
y = y_0(1 + k_1 r^2 + k_2 r^4 + k_3 r^6)
$$

where $k_1$, $k_2$, $k_3$ are distortion coefficients, $(x_0, y_0)$ are the distorted pixel coordinates, and $r^2 = x^2 + y^2$. Tangential distortion, another common geometric distortion, results from non-parallel relationships between the lens plane and image sensor surface. The distortion model is:

$$
x = x_0 + 2p_1 y_0 + p_2(r^2 + 2x_0^2)
$$

$$
y = y_0 + 2p_2 x_0 + p_1(r^2 + 2y_0^2)
$$

All distortion calculations are performed in the camera coordinate system. Each lens’s distortion coefficients $[k_1, k_2, k_3, p_1, p_2]$ can be determined through calibration. Images directly captured by cameras typically contain optical distortion, requiring distortion correction before visual processing.

2.5 Quadrotor UAV Hardware Platform

The Q250 frame was selected as the UAV’s airframe platform. The frame body is constructed from high-strength composite materials, with four arms made of nylon fiber composite material combining excellent impact resistance and structural strength. The center carrier board adopts gold-plated composite PCB design integrating power system wiring functions. The frame features compact dimensions with a symmetric motor-to-motor distance of 250mm and a frame weight of 121g.

For the power system, TMOTOR V2306-2400KV brushless motors (35g, supporting 5-6 inch propellers, compatible with 2-4S lithium batteries) were selected. GEMFAN 51477 5-inch propellers provide thrust generation. HSKRC 45A electronic speed controllers (6.4g, continuous output current 45A, supporting 2-6S lithium batteries) regulate motor speed. TATTU 4S lithium batteries (236g, capacity 2300mAh, voltage 14.8V) provide system energy support.

The CUAV V5 nano flight controller, based on Pixhawk FMUv5 design standards and fully compatible with PX4 and ArduPilot firmware, handles low-level control of the UAV. The main processor is STM32F765, with rich expansion capability for various sensor types. The R12DSE 2.4G 12-channel receiver receives remote control signals and transmits them to the control center.

The NVIDIA Jetson NX serves as the core computing unit, equipped with a 6-core ARM architecture Carmel CPU and 384 CUDA cores Volta GPU, delivering up to 21 TOPS artificial intelligence computing capability to meet real-time processing requirements. This processing module not only undertakes sensor data fusion computation but also provides necessary computational power for various intelligent algorithms.

The Orbbec DaBai DCW binocular camera (dimensions 89.82×25.10×25.10mm, working distance 0.2m-2.5m, maximum resolution 1920×1080, 30Hz FPS) provides real-time environmental perception. The IMU used is the built-in ICM-20689 six-axis motion tracking sensor within the CUAV V5 nano flight controller.

3. UAV Autonomous Positioning and 3D Mapping Based on Point-Line Feature Fusion

3.1 Visual-Inertial SLAM System Framework

In denied environments such as indoor, underground, or complex urban areas where GNSS signals are absent, the stable autonomous flight of quadrotor UAVs fundamentally depends on continuously acquiring precise pose information and executing flexible three-dimensional maneuvers in dynamically constructed environmental models. This chapter designs a visual SLAM algorithm suitable for high-precision, high-real-time autonomous positioning and mapping of quadrotor UAVs in denied environments characterized by indoor or outdoor no-GPS, no-prior-map, and obstacle-laden conditions.

ORB-SLAM3, as the first system to achieve short-term, medium-term, and long-term data association with tightly-coupled vision-IMU fusion, supports monocular, stereo, and RGB-D cameras. Therefore, this paper selects ORB-SLAM3 as the basic framework. Through point-line feature fusion, the extracted ORB feature points are combined with line segment features to further enhance tracking thread stability and environmental modeling robustness.

The improved system architecture comprises the following four core threads:

The visual odometry front-end processing flow primarily includes feature detection, data association, and initial pose estimation modules. This thread first performs feature analysis on input images, simultaneously extracting keypoints and edge features. After feature enhancement and matching, the system completes initial environmental map construction using point and line features from the first two frames of observation data. As new image frames continuously arrive, the algorithm persistently tracks feature correspondences between adjacent frames. Based on these matched features, the system computes relative pose for camera motion through nonlinear optimization, simultaneously utilizing geometric constraints from both point and line features. Subsequently, this thread combines local map information to refine camera pose. After completing the above processing, the system determines whether to mark the current frame as a keyframe according to preset strategies. These screened keyframes serve as the foundation for back-end optimization and global mapping.

The local mapping thread’s core processing flow encompasses three key links: dynamic keyframe management, environmental feature maintenance and updates, and local bundle adjustment optimization. When new keyframes are generated by the visual front-end, the system dynamically maintains and optimizes topological connections between keyframes by analyzing the point and line features extracted from keyframes, continuously refining the environment’s three-dimensional representation and outputting a map fusing point and line features. Simultaneously, appropriate strategies are employed for keyframe selection and culling, and local map optimization is performed on keyframes, points, line segments, and line structures centered around the current image frame.

The loop closure thread’s primary function is detecting new keyframes. When new keyframes are generated, the bag-of-words model performs similarity retrieval in the pre-constructed visual dictionary to identify the most likely matching historical keyframes. After confirming valid environmental feature matches, pose graph optimization corrects cumulative deviations in the camera trajectory. This process effectively addresses drift errors in the SLAM system, significantly improving positioning accuracy. Subsequently, global bundle adjustment optimization is performed on selected keyframes, synchronously updating environmental model data, enhancing global consistency of three-dimensional environmental reconstruction while making camera trajectory estimation more accurate and reliable.

The octree map construction thread addresses the inherent optical distortion issues of binocular cameras by employing Zhang’s calibration method to obtain camera intrinsic parameters and applying polynomial distortion models for image correction, effectively eliminating radial and tangential distortion. Stereo matching completes feature point matching through disparity analysis of left and right images, subsequently computing depth information for each pixel to generate depth maps. Depth measurement data is then converted into hierarchical octree map representations. This three-dimensional environmental modeling approach adopts multi-resolution voxel structures to significantly reduce memory consumption, supports rapid updates in dynamic environments, and accurately characterizes obstacle distribution in flight space.

3.2 Point-Line Feature Fusion

In vision-based SLAM technology, traditional point feature matching methods are commonly used for camera motion estimation. However, when facing texture-poor environments such as empty corridors or uniformly colored walls, these methods often fail to extract sufficient effective feature points, causing pose estimation failures. Notably, such scenes typically contain abundant geometric structure information, particularly prominent straight line features. Compared with point features, line features not only provide stronger spatial constraints but also exhibit better illumination robustness and feature persistence. Therefore, introducing line feature assistance in pose calculation for low-texture scenes can significantly improve the operational stability and anti-interference capability of SLAM systems in complex environments.

The Line Segment Detector (LSD) algorithm has become the preferred line feature extraction method for numerous advanced systems in current visual SLAM research. LSD achieves sub-pixel detection accuracy while avoiding complex parameter settings, particularly suitable for line segment identification in noisy images. However, its higher computational complexity remains a critical issue requiring resolution.

For each support region, the gradient direction and magnitude are computed. For pixel $P_i(x,y)$, the pixel gradient in the x-direction $g_x(x,y)$ and y-direction $g_y(x,y)$ are:

$$
g_x(x,y) = \frac{i(x+1,y) + i(x+1,y-1) – i(x-1,y) – i(x-1,y+1)}{2}
$$

$$
g_y(x,y) = \frac{i(x,y+1) + i(x-1,y+1) – i(x,y-1) – i(x+1,y-1)}{2}
$$

Based on $g_x(x,y)$ and $g_y(x,y)$, the gradient magnitude $G(x,y)$ and gradient direction $LLA$ at point $P_i(x,y)$ are:

$$
G(x,y) = \sqrt{g_x^2(x,y) + g_y^2(x,y)}
$$

$$
LLA = \arctan\frac{g_x(x,y)}{-g_y(x,y)}
$$

To address ORB-SLAM3’s insufficient feature extraction in low-texture environments, this research introduces line feature constraints to enhance system scenario recognition capability. Compared with point features, line features effectively characterize geometric structure information in scenes. By capturing continuity features of object edges, they remain more stable during rapid camera motion or excessive rotation.

The standard LSD algorithm detects pixel sets with significant gradient changes but lacks capabilities for merging line features and eliminating redundant ones. Excessive low-quality line segments not only increase detection and tracking costs but also significantly impact visual odometry accuracy. Therefore, line segment filtering and approximate line segment merging methods are proposed:

1) Line segment sorting: The line segment sequence extracted by the LSD algorithm is sorted in descending order by length:

$$
\mathbf{l} = \{l_1, l_2, \ldots, l_m\}
$$

2) Short line filtering: Long line features are more stable and easier to detect repeatedly across multiple image frames. Short line segments contribute little to positioning accuracy and are susceptible to image noise and detection algorithm effects. Threshold constraints are used to eliminate some short line segment features:

$$
l_{min} = \delta \cdot \min(M, N)
$$

where $l_{min}$ is the minimum length threshold, $M$ and $N$ are the input image height and width, and $\delta$ is the scaling coefficient. Through experimentation, setting $\delta$ to 0.07 effectively eliminates most useless short line segments while preserving valuable long ones.

3) Line segment filtering: Horizontal and vertical distance filtering is adopted to reduce computational load. The vertical distance filtering process targets the pre-screened subset $\mathbf{l}_{\theta}$:

$$
\mathbf{l}_{\varepsilon} = \{l_m \mid |\theta_m – \theta_1| < \theta_{min} \rho, l_m \in \mathbf{l}\}
$$

$$
\rho = \frac{4a^5 + 1}{e^{L_m} – 1}
$$

where $l_m$ is the sequence in the line segment sequence $\mathbf{l}$ with length $L_m$, $\theta_m$ is the angle between the line segment and the horizontal direction, $\theta_1$ is the angle of $l_1$ with the horizontal direction, $\theta_{min}$ is the angle filtering threshold for line features, and $\rho$ is the proportional factor.

4) Approximate line segment merging: After filtering, approximate line segment groups are obtained and merged. The improved LSD algorithm stably extracts long line features primarily oriented in horizontal and vertical directions. These long lines with significant geometric characteristics not only enhance pose estimation constraints but also demonstrate better stability during inter-frame tracking due to their structural continuity.

For the key problem of 3D line feature reconstruction in binocular vision, traditional line feature matching methods based on LBD descriptors have two main limitations: high descriptor computation complexity causing insufficient real-time performance, and significantly degraded matching performance in low-texture environments. To address these issues, this paper proposes a geometric constraint-based binocular line feature matching and triangulation method that significantly improves matching efficiency and robustness.

In the line segment matching process, the line segment features captured by the left camera are defined as reference line segments $l_i^m$, and the corresponding features captured by the right camera are defined as candidate line segments $l_j^m$. Due to the geometric constraint characteristics of parallel binocular cameras, line correspondences satisfy parallel constraints. Therefore, the matching degree can be evaluated by calculating the direction cosine value between two line segment vectors:

$$
\cos(l_i^m, l_j^m) = \frac{l_i^m \cdot l_j^m}{|l_i^m| \cdot |l_j^m|}
$$

The parallel error term $E_{ij}^p$ for line pairs $\{l_i^m, l_j^m\}$ whose cosine value is close to 1 is:

$$
E_{ij}^p = 1 – \cos(l_i^m, l_j^m)
$$

After stereo rectification and lens distortion compensation, image pairs captured by the binocular camera satisfy strict coplanar row alignment characteristics. Based on epipolar geometry principles, matching feature points in left and right views are constrained to the same horizontal line. By introducing spatial region constraints, numerous irrelevant line feature matching candidates can be eliminated. The region constraint parameter $D_{ij}$ for candidate line segment $\{s_k, e_k\}$ is defined as:

$$
D_{ij} = |s_i – s_j| + |e_i – e_j|
$$

To address the limitation of existing parallel and region constraint methods that only consider line features themselves while ignoring local neighborhood information, a neighborhood constraint-based line feature matching method is proposed. A 3×3 pixel matching window $W_p$ centered at the line segment’s center point in the left image is constructed, with the objective function for correlation measurement defined as:

$$
NCC_{ij} = \frac{\sum_{(x,y) \in W_p} (I_i(x,y) – \overline{I_i})(I_j(x+d,y) – \overline{I_j})}{\sqrt{\sum_{(x,y) \in W_p} (I_i(x,y) – \overline{I_i})^2 \sum_{(x,y) \in W_p} (I_j(x+d,y) – \overline{I_j})^2}}
$$

where $d$ is the horizontal coordinate difference between the matching windows, $I_i(x,y)$ is the pixel gray value in the left neighborhood window $W_p$, and $I_j(x+d,y)$ is the pixel gray value in the right matching window. The matching cost vector $Q_{ij}$ for left-right line pairs is defined as:

$$
Q_{ij} = \begin{bmatrix} E_{ij}^p \\ D_{ij} \\ N_{ij} \\ U_{ij} \end{bmatrix}
$$

where $N_{ij} = (1 – NCC_{ij})/2$, and the line segment length constraint term $U_{ij} = |u – 1|$ with $u$ representing the length ratio of left and right view line segments. The line segment matching problem is transformed into an optimization problem with L1 norm sparse solution:

$$
\hat{x}_i = \arg\min_{x_i} \frac{1}{2}\|A_i x_i – b\|^2 + \lambda\|x_i\|_1
$$

For matched left-right line segment features, the two endpoints of the left line segment are projected along horizontal epipolar lines onto the right image plane, then the geometric intersections between projection lines and right matched line segments are solved to accurately determine endpoint correspondences, ultimately achieving three-dimensional reconstruction of line features based on the camera geometric model.

Experimental comparison of line feature detection and matching algorithms on the EuRoC dataset demonstrates that the traditional LSD algorithm has significant limitations in feature extraction, with extracted line segments commonly exhibiting insufficient length and gradual gradient changes. In contrast, the proposed improved algorithm significantly enhances feature quality through multi-scale gradient constraints and geometric consistency verification. In matching performance, the LBD descriptor-based method exhibits multiple mismatches, while the proposed method dramatically reduces mismatches by fusing geometric and appearance features.

3.3 Line Segment Reprojection Error and Point-Line Fusion Pose Optimization

In pose estimation, the system simultaneously establishes observation models using both point and line features, optimizing camera motion states by minimizing reprojection errors. Point feature residuals adopt the same approach as ORB-SLAM3, measuring pixel offsets between projected three-dimensional spatial points and their matched feature points on the 2D image plane. However, line feature error modeling faces greater challenges: since line endpoints may drift or temporarily disappear in consecutive frames, directly applying point feature error calculation methods leads to system instability.

Traditional methods use endpoint distance-based projection error models, which have limitations including high computational complexity and sensitivity to line segment breakage. The improved line feature reprojection error model uses the vertical distance between the projected line segment midpoint and the pixel plane line segment midpoint, plus the angle between the two line segments, as optimization variables. The vertical distance is expressed using Manhattan distance, reducing optimization variables from traditional 4 dimensions (endpoint coordinates) to 3 dimensions (midpoint distance + angle), decreasing computational load.

The projection process first converts Plücker coordinates $L^w$ to camera coordinate system $L^c$:

$$
L^c = \begin{bmatrix} R_{cw} & [t_{cw}]_\times R_{cw} \\ 0 & R_{cw} \end{bmatrix} L^w
$$

Then the projection line segment on the image plane is computed based on camera model:

$$
l = K_c n_c = \begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ -f_x c_x & -f_y c_y & f_x f_y \end{bmatrix} n_c = \begin{bmatrix} l_1 \\ l_2 \\ l_3 \end{bmatrix}
$$

The residual is calculated as:

$$
e_{le} = [d, \theta]
$$

$$
d = |x_1 – x_2| + |y_1 – y_2|
$$

$$
\theta = \arctan\left|\frac{k_{l1} – k_{l2}}{1 + k_{l1} \cdot k_{l2}}\right|
$$

where $d$ is the Manhattan distance from the line segment midpoint to the projected line, and $\theta$ is the angle difference between the two line segments.

For the point features and line features observed at time $k$, the projection errors are defined as:

$$
e_{k,i}^p = p_i’ – K(R_k^{k-1} P_i + t_k^{k-1})
$$

$$
e_{k,j}^l = d(l_j’, K_l Q_k^{k-1} L_j)
$$

where $K$ and $K_l$ represent the projection matrices for point and line features, respectively, and $p_i’$ and $l_j’$ represent the point and line observations obtained through feature matching.

To address the issue where certain outlier error terms dominate the optimization direction when feature mismatches exist, the Huber robust kernel function is introduced:

$$
H(e) = \begin{cases} 0.5e^2 & |e| \leq \delta \\ \delta(|e| – 0.5\delta) & \text{otherwise} \end{cases}
$$

Based on the defined point and line feature projection errors, a unified point-line fusion reprojection error objective function is established:

$$
\{R_k^{k-1}, t_k^{k-1}\} = \arg\min_{R_k^{k-1}, t_k^{k-1}} \sum_{i} \rho_p(e_{k,i}^{pT} \Omega_p e_{k,i}^p) + \sum_{j} \rho_l(e_{k,j}^{lT} \Omega_l e_{k,j}^l)
$$

where $\Omega_p$ and $\Omega_l$ are the covariance matrices of point and line features, and $\rho_p$ and $\rho_l$ are the Huber kernel functions. The Levenberg-Marquardt optimization algorithm iteratively computes the pose parameters for the $k$-th frame image. The rotation matrix obtained through optimization is denoted as $R_k^{k-1*}$. Based on motion continuity between adjacent frames, the rotation matrix of the $k$-th frame can also be derived from the previous frame’s pose, denoted as $R_k^{k-1′}$:

$$
R_k^{k-1′} = R_{k-1}^{k-2} R_{k-2}^{k-1}
$$

Spherical linear interpolation is employed to fuse the two rotation matrix estimates:

$$
R_k^{k-1} = R_k^{k-1′} ((R_k^{k-1*})^{-1} R_k^{k-1′})^{\frac{1}{2}}
$$

Through these optimization algorithms, the system filters rotation matrix estimates, suppresses abnormal error interference, and obtains the optimal spatial rotation matrix and translation vector, ultimately achieving high-precision motion attitude estimation.

When combining point and line features for pose estimation, the visual SLAM system demonstrates significant advantages over point-only approaches, particularly in low-texture environments where line features provide additional geometric constraints. The table below summarizes the key differences between point-only and point-line fusion strategies for the unmanned aerial vehicle visual SLAM system.

Characteristic Point-only SLAM Point-line Fusion SLAM
Feature extraction in low-texture scenes Poor — insufficient features Good — line features supplement
Pose estimation accuracy Degraded in repetitive scenes Improved with geometric constraints
Computational complexity Lower Moderate increase
Robustness to illumination changes Sensitive Line features are more robust
Suitability for UAV navigation May fail in corridors/warehouses Reliable for indoor environments

3.4 Real-Time 3D Map Construction

Quadrotor UAVs perceive surrounding environments in real-time through onboard binocular vision systems and construct flight trajectories based on environmental information. Traditional ORB-SLAM3 lacks dense mapping functionality. While the improved visual-inertial odometry system outputs high-precision pose estimates and sparse feature point clouds, sparse point clouds cannot represent obstacle occupancy in three-dimensional space, making them insufficient to support autonomous navigation. UAVs planning trajectories in unknown environments must accurately determine passable regions between arbitrary two points.

The octree is a three-dimensional environment representation method based on recursive spatial partitioning, employing multi-level data organization for discretized expression of three-dimensional space. The fundamental principle is: using the bounding cube containing the target scene as the initial node, when obstacles are detected within a voxel unit, the system automatically evenly divides that unit into eight sub-cubes, forming a tree-like hierarchical structure from parent to child nodes. Each terminal node stores obstacle information for the corresponding spatial region.

Using binocular camera observation data at different time points, the three-dimensional map is updated in real-time to obtain obstacle position information. The occupancy probability of each child node is represented in the range of 0 to 1: when $p$ approaches 1, the probability of obstacles existing in that voxel is high; conversely, when $p$ approaches 0, the spatial region is likely empty. The probability log-odds value $y$ expresses the occupancy probability $x$:

$$
y = \log it(x) = \ln\left(\frac{x}{1-x}\right)
$$

$$
x = \log it^{-1}(y) = \frac{e^y}{1 + e^y}
$$

During octree map construction, each spatial unit’s occupancy probability $p \in [0,1]$ can be described by its log-odds value $y$. The system dynamically adjusts: detecting occupied status increases the log-odds value; observing free status correspondingly decreases it. For multiple observations of a node, the expression from initial time to time $t$ with observations $z_{1:t}$ is:

$$
L(n|z_{1:t+1}) = L(n|z_{1:t}) + L(n|z_t)
$$

Clamping is applied to limit the update rate, and the probability update follows:

$$
P(n|z_{1:t+1}) = \left[1 + \frac{1 – P(n|z_t)}{P(n|z_t)} \cdot \frac{1 – P(n|z_{1:t-1})}{P(n|z_{1:t-1})} \cdot \frac{P(n)}{1 – P(n)}\right]^{-1}
$$

This approach infers whether data exists within node $n$ and updates the global map based on observation results for different nodes. As octree depth increases, three-dimensional space partitioning becomes progressively finer. This hierarchical characteristic enables the system to flexibly control map precision by dynamically adjusting traversal depth. Reducing traversal of deeper-level octree nodes allows the overall visual-inertial SLAM system to lower computational resource consumption, effectively improving system operational efficiency from both spatial and temporal perspectives.

4. UAV Fast Path Planning Algorithm

4.1 Problem Definition

Mathematical notation describes the UAV trajectory planning problem. Path planning algorithms plan collision-free paths from start to goal for the UAV. This research primarily addresses path planning problems for UAVs confronting static and dynamic obstacles in complex environments.

Three problems are defined: Let $X$ represent the entire space, $X_{obs}$ the region where obstacles exist, and $X_{free} = X / X_{obs}$ the free space where the UAV can move freely and generate feasible paths. $X_{start} \in X_{free}$ is the UAV starting point, and $X_{goal} \in X_{free}$ is the goal point.

Definition 1 (Feasible path planning): Feasible path planning finds solutions for a path planning problem $(X, X_{start}, X_{goal})$ that makes $\phi(0) = X_{start}$, $\phi(1) = X_{goal}$ a collision-free path, where $\phi: [0,1] \rightarrow X$ is a continuous function of bounded variables.

Definition 2 (Optimal path planning): Optimal path planning finds, among all feasible path solution sets, one such that $c(\phi^*) = \min\{c(\phi) : \phi \in \text{feasible paths}\}$, where $c(\phi)$ is the path cost.

Definition 3 (Dynamic and static obstacle avoidance): After finding the global optimal path, the UAV safely and efficiently completes the path from $X_{start}$ to $X_{goal}$, guaranteeing avoidance of newly appearing dynamic obstacles and obstacles not previously present on the planned global path.

4.2 UAV Path Planning Methods

4.2.1 RRT* Algorithm

The RRT algorithm, proposed by Steven M. LaValle in 1998, is a tree-structure-based random sampling path planning algorithm. Its core idea utilizes random sampling and rapid exploration to build a tree, thereby rapidly searching for a path in high-dimensional state space. This algorithm is not only simple and easy to implement but also excels in adapting to high-dimensional spaces and non-convex obstacles.

In each algorithm iteration, RRT generates a random point $x_{rand}$ in $X_{search}$ space, finds the nearest node $x_{near}$ in the tree, extends a fixed step size from $x_{near}$ toward $x_{rand}$ to obtain the new node $x_{new}$; if there are no obstacles between $x_{near}$ and $x_{new}$, $x_{new}$ is added to the tree; otherwise, iteration repeats until a feasible path is found.

The RRT* algorithm, first proposed in 2010, retains RRT’s probabilistic completeness while adding asymptotic optimality, meaning RRT* converges to the optimal solution given sufficient iterations. The algorithm performs two key processes when generating new nodes: reselecting the parent node and path rewiring. Parent node reselection aims to minimize trajectory cost from the new node to the start point, while path rewiring eliminates unnecessary connections in the random tree structure.

The pseudocode for RRT* is as follows:

**Algorithm 1: RRT***($x_{start}$, $x_{goal}$, $X_{obs}$, $iter$)
1: $V \leftarrow \{x_{start}\}$; $E \leftarrow \emptyset$;
2: **for** each $iter \in [1, Max_{iter}]$ **do**
3: $x_{rand} \leftarrow Sample(iter)$;
4: $x_{near} \leftarrow Nearest(V, x_{rand})$;
5: $x_{new} \leftarrow Steer(x_{rand}, x_{near})$;
6: **if** $CollisionFree(x_{near}, x_{new}, X_{obs})$ **then**
7: $X_{near} \leftarrow Near(V, x_{new})$;
8: $x_{parent} \leftarrow ChooseParent(X_{near}, x_{new}, x_{nearest})$;
9: $V \leftarrow V \cup \{x_{new}\}$;
10: $E \leftarrow E \cup \{(x_{new}, x_{parent})\}$;
11: $G \leftarrow Rewire(G, X_{near}, x_{new})$;
12: **end if**
13: **end for**
14: **Return** $G = (V, E)$;

The ChooseParent function reselects the optimal parent node for the new node, and the Rewire function updates child nodes, making selected tree node quality higher. The path cost of RRT* is significantly lower than that of the RRT algorithm.

4.2.2 Informed-RRT*

Informed-RRT* represents a further improvement and extension of the RRT* algorithm. Before finding an initial solution, both algorithms are identical. Once Informed-RRT* obtains an initial solution, it begins utilizing the optimization cost of the current best path $C_{best}$ to guide subsequent exploration. The algorithm creates an elliptical sampling space, performing further sampling and exploration only in this specific elliptical configuration subset region:

$$
\|x_{rand} – x_{start}\| + \|x_{rand} – x_{goal}\| \leq C_{best}
$$

where $x_{start}$ and $x_{goal}$ are the foci of the hyperellipsoid region, $C_{min}$ is the distance between the two foci, and $C_{best}$ is the semi-major axis of the hyperellipsoid region. Since this region reduces the algorithm’s sampling area and increases the probability of sampling valid nodes, algorithm efficiency improves. Furthermore, as paths progressively optimize, $C_{best}$ shortens, the hyperellipsoid region gradually shrinks, and algorithm convergence speed accelerates, thus speeding up convergence to the optimal solution.

**Algorithm 2: Informed-RRT***
1: $V \leftarrow \{q_{start}\}$;
2: $E \leftarrow \emptyset$;
3: $X_{soln} \leftarrow \emptyset$;
4: **for** each $iter \in [1, Max_{iter}]$ **do**
5: $C_{best} \leftarrow \min_{q_{soln} \in X_{soln}} \{Cost(q_{soln})\}$;
6: $x_{rand} \leftarrow SampleInformed(x_{start}, x_{goal}, C_{best})$;
7: $q_{nearest} \leftarrow Nearest(V, x_{rand})$;
8: $q_{new} \leftarrow Steer(q_{nearest}, x_{rand})$;
9: **if** $CollisionFree(q_{nearest}, q_{new}, X_{obs})$ **then**
10: $X_{near} \leftarrow Near(V, q_{new}, R_{near})$;
11: $x_{min} \leftarrow ChooseParent(X_{near}, q_{new}, q_{nearest})$;
12: $q_{near} \leftarrow q_{min}$;
13: $V \leftarrow V \cup \{q_{new}\}$;
14: $E \leftarrow E \cup \{(q_{near}, q_{new})\}$;
15: $G \leftarrow Rewire(G, x_{new}, q_{near})$;
16: **end if**
17: **if** $InGoalRegion(q_{new})$ **then**
18: $X_{soln} \leftarrow X_{soln} \cup \{x_{new}\}$;
19: **end if**
20: **end for**
21: **Return** $G = (V, E)$;

4.2.3 APF Algorithm

In complex environments, the UAV moves toward the target position while maintaining distance from obstacles. The APF method’s basic idea is that the target point produces attraction, while obstacles produce repulsion, constructing repulsive and attractive potential fields. Superimposing these two potential fields yields the potential energy at any point. UAV path planning is accomplished by searching and moving toward directions of decreasing potential energy.

The common attractive potential field function is:

$$
U_{att}(P) = \frac{1}{2} k_a d^2(P, P_{goal})
$$

where $P$ is the UAV’s current coordinate, $k_a$ is the attractive gain coefficient, and $P_{goal}$ is the target point coordinate.

The common repulsive potential field function is:

$$
U_{rep}(P) = \begin{cases} \frac{1}{2} k_r \left(\frac{1}{d(P, P_{ob})} – \frac{1}{Q}\right)^2 & \text{if } d(P, P_{ob}) \leq Q \\ 0 & \text{if } d(P, P_{ob}) > Q \end{cases}
$$

where $k_r$ is the repulsive gain coefficient, $Q$ is the obstacle influence range, and $P_{ob}$ is the obstacle coordinate.

The total potential field is: $U = U_{att} + U_{rep}$.

The attractive force function is:

$$
F_{att}(P) = k_a d(P, P_{goal})
$$

The repulsive force function is:

$$
F_{rep}(P) = \begin{cases} k_r \left(\frac{1}{d(P, P_{ob})} – \frac{1}{Q}\right) \frac{1}{d^2(P, P_{ob})} & \text{if } d(P, P_{ob}) \leq Q \\ 0 & \text{if } d(P, P_{ob}) > Q \end{cases}
$$

The resultant force acting on the UAV is: $F = F_{att} + F_{rep}$.

4.3 UAV Path Planning Fusion Optimization Algorithm

Although the Informed-RRT* algorithm possesses strong search capabilities, it often fails to generate safe paths when confronting unknown static obstacles and dynamic obstacles. Conversely, the DWA algorithm offers good real-time performance and compatibility with UAV motion characteristics, but frequently faces issues such as overly long routes, falling into local optima, and inability to reach target points.

To address these problems, a fusion algorithm based on improved Informed-RRT* is proposed. First, the improved Informed-RRT* algorithm plans the global optimal path, extracting key points of this path as intermediate nodes for the DWA algorithm. The path generated by Informed-RRT* serves as the DWA algorithm’s input, guiding the UAV to perform local path planning and optimization along this path. When encountering dynamic obstacles or environmental changes, the DWA algorithm optimizes the UAV’s real-time flight parameters to ensure flight safety while staying as close as possible to the originally planned path. Then, the dynamic window method performs local path planning between every two adjacent nodes to dynamically avoid obstacles, achieving dynamic path adjustment and optimization.

The specific algorithm improvements and fusion are as follows:

High-quality sampling strategy: An adaptive factor is introduced, adjusting the sampling strategy based on Euclidean distance to the goal point. During the sampling process, randomness is enhanced at the beginning, and convergence accelerates as the path approaches its end, avoiding premature convergence to local optima while improving the efficiency of finding valid paths:

$$
\text{adaptive} = 1 – \frac{\text{norm}(X_{goal} – X_{start})}{n}
$$

where $n$ is the maximum considered distance (1000 unit lengths in this paper). When the adaptive value is small (i.e., start and goal are relatively close), there is a higher probability of selecting a strategy biased toward the goal direction during sampling:

$$
x_{rand} = \begin{cases} \text{Random} & \text{if } \text{rand}() < m \cdot \text{adaptive} \\ X_{goal} & \text{otherwise} \end{cases}
$$

Efficient search strategy: The APF concept is introduced into the new node generation process, making the goal point and random sampling point each exert attraction on the nearest node, while obstacles exert repulsion on the nearest node. The combined force direction serves as the growth direction for the new node, improving trajectory search and obstacle avoidance efficiency. The force schematic is: $F_{att1}$ and $F_{att2}$ are the attractive forces of $x_{rand}$ and $x_{goal}$ on $x_{near}$, respectively, and $F_{rep}$ is the repulsive force of obstacles on $x_{near}$. The resulting combined force direction is the expansion direction of $x_{new}$.

By introducing an attractive force component, the growth trend of the random tree is adjusted to be more directional toward the goal region. The target attractive force function is:

$$
F_{att} = k_a \frac{x_{goal} – x_{near}}{|x_{goal} – x_{near}|}
$$

The random node growth function is:

$$
R_a(x) = \frac{x_{rand} – x_{near}}{|x_{rand} – x_{near}|}
$$

The obstacle repulsive force function is:

$$
F_{rep}(x) = \begin{cases} k_r \left(\frac{1}{d(x, P_{obs})} – \frac{1}{Q}\right) \frac{1}{d^2(x, P_{obs})} \frac{x_{near} – x_{obs}}{|x_{near} – x_{obs}|} & \text{if } d(x, P_{obs}) \leq Q \\ 0 & \text{if } d(x, P_{obs}) > Q \end{cases}
$$

Path optimization: Despite improvements in sampling quality and operational speed, paths remain tortuous. Cubic spline interpolation is used for further optimization, considering the curvature at each point and imposing restrictions. The curvature describes the degree of bending of the path at a certain point:

$$
k(t) = \frac{x'(t)y”(t) – y'(t)x”(t)}{((x'(t))^2 + (y'(t))^2)^{3/2}}
$$

The maximum curvature constraint is:

$$
R_{min} = \frac{v^2}{F_{max} \cdot \sin\phi_{max} / m}
$$

where $v$ is the UAV speed, $F_{max}$ is the maximum thrust, $\phi_{max}$ is the maximum tilt angle, and $m$ is the UAV mass. The scaling factor $G$ ensures the adjusted interpolation points satisfy curvature requirements:

$$
x_i = x(i) + dx(i) \cdot G
$$

$$
y_i = y(i) + dy(i) \cdot G
$$

4.4 Dynamic Window Approach (DWA) Fusion

The Dynamic Window Approach (DWA) is a local path planning strategy based on model predictive control. The algorithm samples multiple sets of velocity values in velocity space, simulates the next set of motion trajectories based on the obtained velocity values, and performs reasonable evaluation of predicted trajectories to obtain the optimal trajectory for path planning. The complete algorithm process mainly includes three parts: establishment of the motion model, collection of velocity samples, and trajectory evaluation functions.

The motion trajectory simulation principle assumes that during computation, due to the short duration between adjacent moments, the motion trajectory is approximately treated as a straight line, projecting the UAV’s motion distance onto the x and y axes of the ground coordinate system:

$$
x_{t+1} = x_t + v_t \Delta t + \frac{1}{2} a_x \Delta t^2
$$

$$
y_{t+1} = y_t + v_t \Delta t + \frac{1}{2} a_y \Delta t^2
$$

$$
\theta_{t+1} = \theta_t + w_t \Delta t + \frac{1}{2} \alpha \Delta t^2
$$

Based on the UAV’s dynamic characteristics, a motion prediction model is established, and candidate motion trajectory sets are generated through velocity sampling. A multi-trajectory evaluation strategy determines the optimal flight path. Considering flight platform mechanical constraints and actual environmental limitations, feasible velocity sampling space has corresponding constraints:

Velocity constraints based on maximum and minimum values:

$$
V = \{v \in [v_{min}, v_{max}], w \in [w_{min}, w_{max}]\}
$$

Constraints based on acceleration and deceleration:

$$
V_d = \{(v, w) \mid v \in [v_c – a_v \Delta t, v_c + a_v \Delta t], w \in [w_c – a_w \Delta t, w_c + a_w \Delta t]\}
$$

Constraints based on safe distance:

$$
V_a = \{(v, w) \mid v \leq \sqrt{2 \cdot dist(v, w) \cdot \dot{v}}, w \leq \sqrt{2 \cdot dist(v, w) \cdot \dot{w}}\}
$$

where $dist(v, w)$ is the distance to the nearest obstacle on the trajectory predicted by the velocity.

The trajectory evaluation function:

$$
C(v, w) = \sigma[\alpha \cdot heading(v, w) + \beta \cdot obsdist(v, w) + \gamma \cdot velocity(v, w) + \delta \cdot smoothness(a)]
$$

where $heading(v, w)$ is the azimuth evaluation function measuring the angle difference between the trajectory end and the goal, $obsdist(v, w)$ is the distance evaluation function measuring distance to the nearest obstacle, $velocity(v, w)$ is the velocity evaluation function encouraging high-speed motion, and $smoothness(a)$ is the path smoothness evaluation function based on acceleration:

$$
smoothness(a) = -(a_x^2 + a_y^2 + a_z^2)
$$

4.5 Simulation Results and Analysis

To evaluate the performance advantages of the proposed fusion optimization algorithm, experiments were designed containing three typical scenarios. The improved Informed-RRT* algorithm was systematically compared with the classic RRT* and Informed-RRT* algorithms. Four key performance indicators were used for quantitative evaluation. Each test case underwent 30 independent repeated experiments with a maximum iteration limit of 800 times. Dynamic obstacles and obstacles not present in the global path were added to the original map to test the real-time obstacle avoidance effect after DWA fusion.

Three test environments were designed: general environment, narrow environment, and cluttered environment. These environments were designed based on common working environments of UAVs in real life, each 100m×100m in size, with red circles representing starting points, green circles representing goal points, and grass-green rectangles and circles representing static obstacles.

The simulation results are summarized in the following tables for the three different environments.

Environment Algorithm Average Nodes Time (s) Average Path Length (m) Success Rate (%)
Environment A (General) RRT* 566 8.9856 127.3167 80
Informed-RRT* 524 7.3331 119.7200 83
Improved-Informed-RRT* 126 1.3452 115.0886 93
Environment B (Narrow) RRT* 572 8.2394 125.5162 76
Informed-RRT* 506 7.4856 123.7842 83
Improved-Informed-RRT* 167 1.8220 115.4792 90
Environment C (Cluttered) RRT* 486 6.3901 128.7113 86
Informed-RRT* 437 5.9795 127.1665 90
Improved-Informed-RRT* 155 1.6915 117.8026 93

The experimental results demonstrate significant improvements of the proposed algorithm. In environment A, compared with the Informed-RRT* algorithm, the proposed algorithm shortened the trajectory by 3.86%, reduced node count by 75.95%, and decreased average runtime by 81.65%. In environment B, the average trajectory length was reduced by 6.71%, average runtime by 75.65%, and average node count by 66.99%. In environment C, average path length decreased by 7.36%, average runtime by 71.71%, and average node count by 64.53%. The success rate also significantly improved, demonstrating that the algorithm is more stable and efficient in complex environments.

Path optimization results show that the original path generated by the algorithm, while feasible, contains numerous trajectory points and long path length, making it difficult to adapt to actual UAV flight requirements. The proposed method employs cubic spline interpolation considering the UAV’s turning radius, optimizing the path generated by the improved algorithm. The optimized path has fewer redundant points, smoother trajectory, and better conforms to actual UAV flight requirements.

Dynamic obstacle avoidance experiments were conducted by adding dynamic obstacles and obstacles not on the initially planned path to the three environments. The light-green dynamic obstacles maintained uniform motion in different directions. The UAV formed appropriate obstacle avoidance velocities relative to the dynamic obstacles, maintaining proximity to the initial path while avoiding obstacles. The improved fusion algorithm demonstrated better performance in complex dynamic environments.

The simulation results in the three environments demonstrate the effectiveness of the trajectory planning algorithm, accelerating algorithm convergence, significantly reducing redundant points, and improving stability and success rates.

5. Experimental Verification of UAV Autonomous Navigation System

5.1 UAV Visual-Inertial SLAM System Experiment Analysis

5.1.1 UAV Autonomous Positioning Experiment Analysis

To evaluate the performance of the proposed UAV navigation system, this study selected the public EuRoC standard dataset for experimental verification. The dataset was collected by a hexacopter flight platform equipped with binocular vision sensors (20Hz) and inertial measurement units (200Hz), with a millimeter-level precision motion capture system recording ground truth trajectories. The data collection platform, shown in the experimental setup, completed multiple experiments in industrial workshops and narrow indoor spaces with typical denied environment characteristics, providing rich scene data for algorithm testing. These synchronized visual-inertial data can verify positioning accuracy and comprehensively assess trajectory tracking performance in complex environments, providing a reliable experimental basis for navigation algorithm evaluation.

In the experiments, pose estimation performance was compared between the ORB-SLAM3 algorithm and the improved method. ORB-SLAM3, as a benchmark algorithm in the SLAM field, had its pose estimation results used as reference. The proposed method adds line feature constraints to enhance accuracy and stability based on ORB-SLAM3’s feature detection and tracking framework.

The Machine Hall (MH03) sequence from the EuRoC dataset was selected for verification, with VICON system-provided precise motion trajectories as ground truth. The estimation results of ORB-SLAM3 and the proposed method were compared against ground truth in three-dimensional space, with further comparisons on x, y, z axis translations and rotation attitudes.

The Absolute Pose Error (APE) metric was used for systematic comparison of pose estimation accuracy in three-dimensional space. The APE metric effectively reflects algorithm consistency in global coordinate system pose estimation by calculating differences between estimated trajectories and high-precision motion capture system ground truth trajectories. Experimental results show that compared with the ORB-SLAM3 benchmark algorithm, the proposed improved method achieved an improvement in position estimation accuracy and corresponding reduction in attitude estimation error.

Algorithm Max (m) Min (m) Std (m) Median (m) Mean (m) RMSE (m)
ORB-SLAM3 0.091 0.004 0.015 0.028 0.031 0.035
Proposed Method 0.081 0.002 0.014 0.020 0.025 0.029

The comparative data indicates that compared with the ORB-SLAM3 benchmark algorithm, the proposed method achieved approximately 17.14% reduction in the RMSE metric, verifying the significant improvement in position estimation accuracy.

5.1.2 UAV 3D Mapping Experiment Verification

Systematic experimental research was conducted on three-dimensional environmental modeling for quadrotor UAVs. Using the Machine Hall 03 (MH_03) standard dataset from the EuRoC MAV Dataset for verification, stereo matching was performed on geometrically corrected binocular images, disparity information was calculated and converted to depth maps. Dense three-dimensional point clouds were then constructed through point cloud registration algorithms, ultimately converted to octree-based occupancy grid representations. This experimental scheme ensures both verification reliability of real scenes and repeatability advantages of standard datasets.

The depth maps acquired by the binocular camera were used for three-dimensional environmental reconstruction. Under the challenging MachineHall environment of the EuRoC dataset, feature point clouds were mapped to dense three-dimensional point clouds, and octree principles were used to construct three-dimensional grid maps, efficiently and accurately constructing three-dimensional environmental maps suitable for UAV flight trajectory planning.

5.2 UAV Path Planning Experiment Analysis

To apply the improved algorithm to actual UAV systems, it was integrated into the Robot Operating System (ROS). A Gazebo simulation platform based on a physics engine was used to construct complex three-dimensional environments for systematic verification of the UAV path planning algorithm. The platform ensures high realism of the simulation environment through accurate simulation of gravity, air resistance, and collision detection effects. During experiments, different environment complexity parameters and sensor parameters were configured, combined with rviz visualization tools for real-time monitoring and analysis of UAV pose, environmental perception, and path planning results.

The ROS communication framework implements data interaction in the simulation system, based on the publish/subscribe messaging model. In the Gazebo simulation environment, various sensor data and aircraft state information are transmitted through specific topics. The rviz visualization platform subscribed to related topics for real-time monitoring, including environmental perception data and aircraft pose information.

Path planning simulation results show that the RRT* algorithm generated paths with more turning points and closer proximity to obstacles, easily causing collisions. The Informed-RRT* path planning result, while better in route quality, produced longer paths. The Informed-RRT* optimized path result was shorter with fewer turning points, better conforming to UAV dynamic flight requirements. This experiment verified the feasibility and effectiveness of the proposed method.

5.3 UAV Autonomous Navigation System Verification

5.3.1 Navigation System Software Framework

The proposed UAV autonomous navigation system uses Ubuntu 20.04 as the basic operating environment, built upon the ROS Noetic framework. The system adopts modular design, with functional components interacting through ROS publish-subscribe mechanisms. The hardware platform is equipped with the Orbbec DaBai DCW RGB stereo vision binocular camera, synchronously acquiring color images, depth maps, and inertial measurement data at 30Hz, with raw perception information distributed through standard ROS topics.

In the data processing flow, cv_bridge tools convert OpenCV-format visual data to ROS standard message formats. After format conversion, depth information is transmitted to subsequent processing units through dedicated topics. The environmental modeling module uses this data for three-dimensional scene reconstruction, while the positioning module completes motion state estimation based on multi-source perception information. The system continuously fuses sensor observation data, computes and publishes UAV pose changes in real-time. Additionally, the map construction node achieves real-time environmental modeling by integrating pose estimation results with depth information.

The mapping node receives UAV pose information and depth image data, first completing depth-to-point-cloud conversion. Based on camera intrinsic parameter matrix, depth values in the pixel coordinate system are converted to the camera coordinate system, then point cloud data is converted to the world coordinate system combined with current UAV pose. After coordinate transformation, the system updates the environmental map in real-time and publishes the latest map information through grid map topics. The planner node, upon subscribing to grid map and goal point information, uses the path planning algorithm described in Chapter 4 for real-time trajectory computation and publishes generated trajectories through dedicated topics.

To achieve ROS system and UAV flight controller communication, MAVROS middleware converts ROS messages to MAVLink protocol, converting planned trajectories to flight-controller-recognizable commands. The flight controller receives commands and computes motor speed control values through solving, driving the UAV along the planned trajectory to the goal position. For debugging convenience, Nomachine remote desktop tools connect to the UAV onboard computer through SSH protocol, supporting remote startup of sensor drivers, sending goal point commands, and real-time visualization of system operational status.

5.3.2 UAV Autonomous Navigation Simulation

An indoor complex environment was prepared in Gazebo as the test environment, integrating all modules. The PX4 Gazebo simulation environment was launched with the bookstore scene, followed by launching the ORB-SLAM3 visual-inertial SLAM node, the XTDrone communication module connecting ROS and the UAV, and the keyboard control node for UAV control. The Planning path planning node was launched to find safe and feasible paths, ultimately completing autonomous UAV navigation in the complex environment. The visual SLAM operational results and path planning simulation results confirmed the effectiveness of the complete navigation pipeline.

5.3.3 UAV Autonomous Navigation Real-World Verification

During experiments, only start and goal points needed to be set in the environmental map; the UAV could complete autonomous flight from start to goal without satellite positioning and without human intervention. An obstacle environment was constructed in the campus square, with the physical UAV having no external positioning modules installed to simulate GNSS-denied testing. Flight data was recorded in real-time through the ROS system, including all sensor data and status information, saved in bag file format for subsequent detailed analysis and verification.

After experiments, rviz visualization tools processed the collected data, plotting the UAV’s motion trajectory in three-dimensional space. The results clearly show that the UAV in complex environments could perceive obstacle information in real-time, make reasonable obstacle avoidance decisions, and ultimately achieve smooth, safe flight, accurately reaching preset goal points. This successful obstacle avoidance experiment fully demonstrates the excellent real-time obstacle avoidance capability of the proposed algorithm.

Reliable obstacle avoidance capability requires precise navigation as a prerequisite. In this experiment, the UAV demonstrated excellent positioning accuracy and trajectory tracking performance, maintaining stable flight states even in dynamic environments with obstacles. Experimental results indicate that the algorithm not only accurately estimates UAV position and attitude but also adjusts flight paths in real-time according to environmental changes, fully verifying the superiority of the algorithm in navigation applications.

The table below summarizes the experimental results of the complete autonomous navigation system.

Experimental Module Validation Method Key Result
Visual-Inertial SLAM positioning EuRoC MH03 dataset comparison with ORB-SLAM3 RMSE reduced by ~17.14%
3D mapping MH03 dataset dense reconstruction Successful octree map construction
Path planning Three environments, 30 runs each Node reduction up to 75.95%, time reduction up to 81.65%
Dynamic obstacle avoidance DWA fusion in dynamic environments Real-time obstacle avoidance achieved
Autonomous navigation Real-world GNSS-denied flight tests Stable autonomous flight with obstacle avoidance

6. Conclusion and Future Work

6.1 Conclusion

In complex environments such as deep mountain canyons, dense forests, urban canyons, indoor spaces, and electronic countermeasure battlefields, satellite navigation signals are susceptible to physical occlusion, electromagnetic interference, or spoofing attacks, causing serious degradation or complete failure of positioning performance. Meanwhile, the lack of prior environmental information poses great challenges to autonomous navigation of unmanned systems. This research addresses the core issue of autonomous navigation capability for quadrotor UAVs in satellite-denied environments through the following contributions:

First, for the autonomous navigation requirements of UAVs in complex scenarios without GPS signals and unknown environments, a complete autonomous navigation system was designed, comprising data acquisition, positioning and mapping, path planning, and low-level control modules. An integrated hardware platform was developed consisting of UAV airframe, power modules, flight controller, onboard computing device, and sensors, providing hardware support for subsequent visual-inertial fusion positioning and three-dimensional environmental modeling research. To address navigation problems in areas without prior information, the front-end data processing modules of the classic ORB-SLAM3 framework were improved, adopting a point-line hybrid feature detection strategy with efficient line segment matching algorithms. In back-end optimization, an improved line feature reprojection error computation model was designed, realizing joint pose optimization of point and line features. Additionally, depth computation models were established using binocular stereo vision principles, enabling precise acquisition of three-dimensional environmental information and dense point cloud generation. An octree structure was adopted for efficient map storage and updating, ultimately constructing a higher-performance visual SLAM solution.

Second, a UAV path planning method combining improved Informed-RRT* algorithm with enhanced DWA algorithm was proposed for path planning problems in dynamic or unknown static obstacle environments. Compared with traditional Informed-RRT* algorithms, the improved algorithm shortened path length and planning time while eliminating numerous redundant nodes. The method first uses Informed-RRT* to generate a collision-free path from start to goal in global space as the initial reference path for the DWA algorithm. In actual operation, DWA dynamically adjusts control inputs based on the UAV’s current position, velocity, and reference path, enabling real-time avoidance of dynamic obstacles and unknown static obstacles while maintaining proximity to the reference path. This combined method not only improves path planning efficiency but also enhances UAV adaptability in complex dynamic environments, better conforming to real UAV flight scenarios.

Third, systematic experimental verification was conducted for the binocular vision-based quadrotor UAV autonomous navigation system in denied environments. In visual-inertial SLAM system verification, the optimized visual-inertial odometry scheme was compared with ORB-SLAM3 using the EuRoC public dataset. Experimental results show significant improvement in position estimation accuracy. The proposed method effectively completes three-dimensional environmental modeling, successfully constructing navigation-valuable octree three-dimensional grid maps. In trajectory planning algorithm verification, simulation experiments confirmed that the improved Informed-RRT* algorithm significantly outperforms traditional methods in search nodes, path length, and computational efficiency. Based on the ROS framework, system integration and real-world verification were completed, achieving complete autonomous UAV navigation in real denied environments, successfully verifying reliable system operation without GPS, including autonomous waypoint arrival and obstacle avoidance functions.

6.2 Future Work

The current binocular vision and IMU fusion-based autonomous navigation system for UAVs still faces numerous technical challenges. Due to research period constraints, this study focuses on designing a vision-based quadrotor UAV autonomous navigation system for denied environments, with emphasis on core architecture design and key algorithm research. Several limitations remain:

In visual-inertial fusion positioning and mapping, the current improvements have enhanced positioning accuracy and mapping effects. However, drastic environmental illumination changes can cause positioning deviations in feature-matching-based visual odometry, affecting real-time pose estimation accuracy and environmental modeling effectiveness. Experimental results show that traditional feature extraction algorithms significantly degrade in low-light or low-texture scenes. Future research directions can focus on deep learning technology applications, constructing end-to-end neural network models to improve feature extraction and matching capabilities in complex environments. Combined with self-supervised learning methods, reliance on labeled data can be reduced. Multi-modal heterogeneous sensor tightly-coupled models could be constructed with adaptive weight mechanisms to enhance environmental perception anti-interference capabilities. In mapping, implicit mapping methods based on Neural Radiance Fields (NeRF) could be explored to enhance three-dimensional reconstruction completeness in unstructured scenes.

In UAV trajectory planning research based on three-dimensional maps, accurately describing quadrotor UAV dynamic constraints remains challenging. Regarding nonlinear optimization solving, current research is mostly limited to directly invoking existing nonlinear optimization solvers in the ROS framework, lacking customized optimization strategies for UAV motion characteristics. Future research should focus on establishing more complete parameterized expressions to accurately describe multi-dimensional constraint conditions and systematically comparing various nonlinear optimization algorithms to improve trajectory planning problem-solving efficiency and robustness.

Scroll to Top