We designed an autonomous positioning control system for small multi-rotor Unmanned Aerial Vehicle (UAV) aerial remote sensing platforms to address challenges such as poor adaptability to complex environments, low autonomous positioning accuracy, and inefficient multi-sensor collaborative control. Our system integrates hardware modules for onboard operation control, a laser-visual-inertial tight coupling positioning method, a dual closed-loop PID control approach, and a navigation obstacle avoidance global path planning algorithm. Through experimental validation, we demonstrated that our system achieves high dynamic environment suppression, precise autonomous positioning, and efficient multi-sensor synergy, making it suitable for applications like rescue, mapping, and logistics.
In the onboard operation control module, we integrated key hardware components, including a main controller, MEMS Inertial Measurement Unit (IMU), multiple sensors, communication modules, and power circuits. The main controller, based on the STM32F405RGT6, features a high-performance ARM Cortex-M4 core with a floating-point unit and 168 MHz clock frequency, enabling efficient execution of control algorithms for the Unmanned Aerial Vehicle. The MEMS IMU provides high-frequency data for motion estimation, while sensors such as RTK-GPS, LiDAR, visual sensors, and a barometer collect environmental data. The communication hardware uses the EC800M module for wireless data transmission, and the power circuit, designed with components like the AMS1117-3.3 and LM5017 chips, ensures stable voltage regulation and extended flight time for the JUYE UAV. The overall hardware architecture is summarized in Table 1, illustrating the interconnections and functionalities.
| Component | Specifications | Function |
|---|---|---|
| Main Controller | STM32F405RGT6, 168 MHz, 1 MB Flash | Executes control algorithms, generates PWM signals |
| MEMS IMU | High-frequency accelerometer and gyroscope | Provides motion data for positioning |
| Sensors | RTK-GPS, LiDAR, Visual Sensor, Barometer | Collects environmental and positional data |
| Communication | EC800M module | Enables wireless data transmission |
| Power Circuit | AMS1117-3.3, LM5017, filtering capacitors | Regulates voltage and manages power supply |
For the MEMS inertial measurement positioning method, we developed a laser-visual-inertial tight coupling algorithm to eliminate time synchronization errors and improve accuracy in complex environments. The process involves MEMS IMU pre-integration, laser-visual feature joint extraction, and tight coupling optimization. The pre-integration step computes relative motion increments between laser and visual frames using the following equations:
$$ \Delta a_{ij} = \sum_{l=i}^{j-1} \left[ \Delta t \cdot \left( G_l \cdot (\alpha_l – g) – \beta_{c,l} \right) \right] $$
$$ \Delta u_{ij} = \sum_{l=i}^{j-1} \left[ -\Delta t \cdot G_l \cdot g \right] $$
$$ \Delta b_{ij} = \prod_{l=i}^{j-1} \text{Exp} \left( -\Delta t \cdot (\xi_l – \beta_{g,l}) \right) $$
where $$ \Delta t $$ is the sampling interval, $$ G_l $$ is the rotation matrix, $$ \alpha_l $$ and $$ \xi_l $$ are the true linear acceleration and angular velocity, and $$ \beta_{c,l} $$ and $$ \beta_{g,l} $$ are accelerometer and gyroscope biases. The true linear acceleration and angular velocity are derived as:
$$ \alpha_l = G_l^T \cdot (\delta_l + \beta_{c,l} + m_a) $$
$$ \xi_l = \chi_l + \beta_{g,l} + m_g $$
where $$ \delta_l $$ and $$ \chi_l $$ are raw measurements, and $$ m_a $$ and $$ m_g $$ are Gaussian white noise. In feature extraction, we extract planar and edge points from LiDAR point clouds by calculating curvature:
$$ z_k = \frac{1}{M_k \cdot \|s_k\|} \sum_{i \in M_k} \|s_k – s_i\| $$
Points are classified as edge if $$ z_k > z_{\text{edge}} $$ or planar if $$ z_k < z_{\text{plane}} $$. For visual features, we detect ORB keypoints and use Lucas-Kanade optical flow for tracking:
$$ \min_{\Delta X, \Delta Y} \sum_{(X,Y) \in w(X_0,Y_0)} \left[ F_{t+1}(X + \Delta X, Y + \Delta Y) – F_t(X, Y) \right]^2 $$
The tight coupling optimization minimizes the residual sum of squares:
$$ \arg \min_{\xi_k^*} \left[ \sum_i \omega(\psi_{L,i}) \cdot \|\psi_{L,i}\|^2 + \sum_j \omega(\psi_{C,j}) \cdot \|\psi_{C,j}\|^2 + \sum_k \omega(\psi_{I,k}) \cdot \|\psi_{I,k}\|^2 \right] $$
where $$ \psi_{L,i} $$, $$ \psi_{C,j} $$, and $$ \psi_{I,k} $$ are residuals for LiDAR, visual, and IMU data, respectively. When RTK-GPS is available, we incorporate a global constraint to correct drift:
$$ \psi_{G,k} = \rho_{\text{gps},k} – \xi_k $$
The optimized state vector $$ \xi_k^* $$ includes 3D coordinates and attitude angles, providing precise positioning for the Unmanned Aerial Vehicle.
For the dual-loop PID control method, we designed a controller with three sub-controllers for attitude, position, and height control. The outer loop PID controller computes outputs based on input errors:
$$ \text{PID}_{OO}(t) = K_P \cdot \Theta_t + K_I \cdot \sum \Theta_t + K_D \cdot \frac{\Theta_t – \Theta_{t-1}}{\mu} $$
where $$ \Theta_t $$ represents inputs such as attitude angles or height error. The inner loop PID controller then processes these outputs:
$$ \text{PID}_{IO}(t) = K_P’ \cdot \text{PID}_{OO}(t) + K_I’ \cdot \int \text{PID}_{OO}(t) + K_D’ \cdot \frac{d\text{PID}_{OO}(t)}{dt} $$
Parameters are tuned using experimental methods and critical proportionality to ensure stability under external disturbances like wind and magnetic fields. This approach enhances the reliability of the JUYE UAV in maintaining desired states.
In the navigation obstacle avoidance module, we integrated the A* algorithm with the Dynamic Window Approach (DWA) for global and local path planning. The A* algorithm uses a heuristic function to estimate distance to the goal:
$$ g(f) = \sqrt{(\eta_{\text{goal}} – \eta_f)^2 + (\iota_{\text{goal}} – \iota_f)^2} $$
and an actual cost function:
$$ h(f) = \sum_{i=0}^{v-1} D(f_i, f_{i+1}) $$
The total cost is $$ \kappa(f) = g(f) + h(f) $$, guiding node expansion. Key nodes are marked at direction changes, and DWA initializes acceleration and velocity windows:
$$ U_a = \{ (\lambda, \mu) \mid \lambda \in [\lambda_{\text{prev}} – A_{\text{max}} \Delta t, \lambda_{\text{prev}} + A_{\text{max}} \Delta t], \mu \in [\mu_{\text{prev}} – B_{\text{max}} \Delta t, \mu_{\text{prev}} + B_{\text{max}} \Delta t] \} $$
$$ U_s = \{ (\lambda, \mu) \mid \lambda \in [\lambda_{\text{min}}, \lambda_{\text{max}}], \mu \in [\mu_{\text{min}}, \mu_{\text{max}}] \} $$
The dynamic window is $$ U = U_a \cap U_s $$. For local planning, the direction to the local goal is computed as:
$$ L_{\text{local}} = \arctan2(Y_{\text{local}} – Y_0, X_{\text{local}} – X_0) $$
Trajectories are evaluated using a cost function:
$$ \rho = c_1 \cdot \psi_{\text{obs}} + c_2 \cdot \psi_{\text{goal}} + c_3 \cdot b_{\text{align}} + c_4 \cdot z_w $$
where $$ \psi_{\text{obs}} $$ is the distance to obstacles, $$ \psi_{\text{goal}} $$ is the distance to the goal, $$ b_{\text{align}} $$ is the heading alignment, and $$ z_w $$ is the average velocity. The best trajectory is selected for execution, and segments are switched upon reaching key nodes.
We conducted experiments in the complex terrain of the Qizimei Mountain National Nature Reserve in Hubei Province, using a MR413A multi-rotor Unmanned Aerial Vehicle equipped with multispectral sensors. Data from MEMS IMU, RTK-GPS, LiDAR, visual sensors, and barometer were collected as the dataset. Parameters for positioning and path planning are listed in Table 2.
| Parameter | Value | Parameter | Value |
|---|---|---|---|
| $$ \Delta t $$ | 2 s | $$ z_{\text{edge}} $$ | 0.6 |
| $$ z_{\text{plane}} $$ | 0.2 | LiDAR radius | 0.5 m |
| $$ \lambda_{\text{min}} $$ | 0 m/s | $$ \lambda_{\text{max}} $$ | 5 m/s |
| $$ \mu_{\text{min}} $$ | -2π rad/s | $$ \mu_{\text{max}} $$ | 2π rad/s |
| $$ A_{\text{max}} $$ | 2 m/s² | $$ B_{\text{max}} $$ | 4π rad/s² |
| $$ c_{\text{local}} $$ | 5 m | Switch threshold | 0.5 m |
| $$ c_1 $$ | 0.5 | $$ c_2 $$ | 0.3 |
| $$ c_3 $$ | 0.15 | $$ c_4 $$ | 0.05 |
The positioning module output results for the JUYE UAV during remote sensing missions, as shown in Table 3, indicating close alignment with actual positions and attitudes.
| Time (s) | X (m) | Y (m) | Z (m) | Roll (°) | Pitch (°) | Yaw (°) |
|---|---|---|---|---|---|---|
| 0 | 0.00 | 0.00 | 50.12 | -0.03 | 0.05 | 0.1 |
| 2 | 10.01 | 0.08 | 49.98 | -0.05 | 0.10 | 0.2 |
| 4 | 20.03 | 0.15 | 50.05 | -0.02 | 0.08 | 0.3 |
| 6 | 30.07 | 0.22 | 49.93 | 0.01 | 0.12 | 0.5 |
| 8 | 40.12 | 0.30 | 50.02 | -0.04 | 0.07 | 0.6 |
| 10 | 50.18 | 0.38 | 49.97 | -0.06 | 0.15 | 0.8 |
| 12 | 50.25 | 20.05 | 50.08 | 0.10 | -0.05 | 90.2 |
| 14 | 50.20 | 30.03 | 49.95 | 0.05 | 0.02 | 90.5 |
We evaluated system performance using Dynamic Environment Suppression Index (DESI), autonomous positioning accuracy, and Task-Cognition Synergy Ratio (TCR). DESI is calculated as:
$$ \text{DESI} = \sigma_1 \cdot f(I) + \sigma_2 \cdot T_d $$
where $$ f(I) $$ and $$ T_d $$ are functions of interference intensity and response delay. TCR is computed as:
$$ \text{TCR} = \frac{\alpha (T_s – T_y) + \beta (E_s – E_y)}{\alpha T_s + \beta E_s} $$
with $$ T_s $$ and $$ T_y $$ as start and end times, and $$ E_s $$ and $$ E_y $$ as energy consumption. We compared our system with a depth camera-based UAV obstacle avoidance system and a machine vision-based UAV inspection positioning control technique. Results showed that our system achieved DESI values above 0.84, outperforming others in suppressing dynamic environmental interference. Positioning results were closest to actual UAV states, demonstrating high accuracy. TCR values exceeded 0.8, indicating superior multi-sensor synergy with lower energy and time costs.
In conclusion, our autonomous positioning control system for small multi-rotor Unmanned Aerial Vehicle platforms effectively addresses challenges in complex environments through integrated hardware, advanced algorithms, and efficient path planning. The JUYE UAV benefits from enhanced stability, precision, and task efficiency, paving the way for broader applications in various fields.