In the rapidly evolving field of unmanned aerial vehicles, achieving precise and stable indoor localization for quadrotor drones remains a significant challenge. Common issues such as low accuracy, instability, and excessive latency persist. Solutions that require pre-configuration of indoor environments are costly and economically inefficient. In this work, we address these limitations by proposing an improved Lucas-Kanade (LK) optical flow algorithm tailored for quadrotor drone navigation. Our approach integrates the Shi-Tomasi corner detection method and bilinear interpolation to enhance localization precision and robustness. We have implemented this algorithm on an onboard computing system for real-time operation. Our findings demonstrate that the refined LK optical flow method delivers high accuracy and stability, enabling a quadrotor drone to perform fixed-point flight under consistent lighting conditions with a precision defined by a circle of radius 30 cm.
The core motivation stems from the need for autonomous quadrotor drone operations in GPS-denied environments, such as indoor inspections, search-and-rescue missions in buildings, or hazardous area assessments. Traditional methods like GPS are ineffective indoors due to signal blockage. Vision-based approaches, particularly optical flow, offer a promising alternative by estimating motion from visual data. However, standard optical flow algorithms often struggle with computational efficiency and feature tracking consistency. Our contribution focuses on optimizing the LK method to meet the real-time demands and environmental constraints faced by a quadrotor drone in indoor settings.
The quadrotor drone platform used in our study is based on a common F450 frame, selected for its compact size, agility, and ability to carry sensor payloads. This quadrotor drone is equipped with a flight control board centered around an STM32F103ZET6 microcontroller, an MPU6050 inertial measurement unit for accelerometer and gyroscope data, an MS5611 barometric pressure sensor, an HC-SR04 ultrasonic module for altitude measurement, and an OV7725 camera sensor paired with a Raspberry Pi 3 for optical flow computation. The integration of these components allows the quadrotor drone to perform stable flight while processing visual data for localization. The system architecture ensures that the quadrotor drone can handle the computational load of our improved algorithm while maintaining responsive flight control.
Our improved optical flow algorithm begins with feature extraction using the Shi-Tomasi corner detection method. For an image with intensity $I(x, y)$, we consider a window $Q$ of size $m \times m$ centered at point $(x, y)$. The goal is to find corners by examining the matrix $N$ derived from image gradients:
$$ N = \sum_{(x,y) \in Q} W(x, y) \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix} $$
where $I_x$ and $I_y$ are the partial derivatives in the x and y directions, and $W$ is a window function. The Shi-Tomasi algorithm computes the eigenvalues $\lambda_1$ and $\lambda_2$ of $N$. A corner is identified if $\min(\lambda_1, \lambda_2)$ exceeds a threshold. This method provides robust feature points for tracking in a quadrotor drone’s video stream. To achieve sub-pixel accuracy, we apply bilinear interpolation to refine the corner coordinates.
Next, we employ the pyramid Lucas-Kanade optical flow method to track these features across frames. The LK algorithm is based on three key assumptions: brightness constancy, small motion between frames, and spatial coherence. The fundamental optical flow equation is:
$$ I_x u + I_y v + I_t = 0 $$
where $u = \frac{dx}{dt}$ and $v = \frac{dy}{dt}$ are the horizontal and vertical velocities of a pixel. Since this single equation has two unknowns, we solve for $u$ and $v$ by considering a local neighborhood of pixels, typically a $5 \times 5$ or $3 \times 3$ window. For a quadrotor drone, which may experience larger motions, we use an image pyramid. The pyramid approach processes images at multiple scales, starting from a coarse level to estimate initial motion, which is then refined at finer levels. The total displacement $D$ is computed as:
$$ D = g_0 + D_0 = \sum_{L=0}^{N-1} 2^L D_L $$
where $D_L$ is the optical flow result at pyramid level $L$, $g_0$ is the initial guess at the top level, and $N$ is the number of levels. This enables the quadrotor drone to track features even under significant motion.
The integration of Shi-Tomasi detection with pyramid LK optical flow forms our enhanced algorithm. We track the detected corners across consecutive frames, discard outliers that exhibit large drifts, and compute the optical flow vectors. These vectors are then converted into velocity estimates for the quadrotor drone. The use of an onboard Raspberry Pi 3 ensures real-time computation, critical for the dynamic control of a quadrotor drone. The algorithm’s steps are summarized in the table below:
| Step | Description | Key Equation/Technique |
|---|---|---|
| 1. Feature Detection | Extract corners using Shi-Tomasi method | $\min(\lambda_1, \lambda_2) > \text{threshold}$ |
| 2. Sub-pixel Refinement | Improve corner accuracy with bilinear interpolation | $I(x,y) \approx \text{interpolate from neighbors}$ |
| 3. Optical Flow Tracking | Track corners using pyramid LK algorithm | $I_x u + I_y v + I_t = 0$ over local window |
| 4. Outlier Rejection | Remove inconsistent feature points | Based on flow magnitude and direction variance |
| 5. Velocity Estimation | Convert optical flow to drone velocity | $v_{\text{drone}} = \frac{v_{\text{flow}} \cdot Z}{f}$ |
For the quadrotor drone’s attitude control, we use quaternions for orientation representation. The rotation from the geographic reference frame ($R$) to the body frame ($B$) is given by the direction cosine matrix:
$$ C_R^b = \begin{bmatrix} 1-2(q_2^2+q_3^2) & 2(q_1 q_2 + q_0 q_3) & 2(q_1 q_3 – q_0 q_2) \\ 2(q_1 q_2 – q_0 q_3) & 1-2(q_1^2+q_3^2) & 2(q_2 q_3 + q_0 q_1) \\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 – q_0 q_1) & 1-2(q_1^2+q_2^2) \end{bmatrix} $$
where $q = [q_0, q_1, q_2, q_3]$ is the unit quaternion. The Euler angles (roll $\phi$, pitch $\theta$, yaw $\psi$) are derived as:
$$ \phi = \arctan\left( \frac{2(q_2 q_3 + q_0 q_1)}{1 – 2(q_1^2 + q_2^2)} \right) $$
$$ \theta = \arcsin\left( 2(q_0 q_2 – q_1 q_3) \right) $$
$$ \psi = \arctan\left( \frac{2(q_1 q_2 + q_0 q_3)}{1 – 2(q_2^2 + q_3^2)} \right) $$
Control of the quadrotor drone is achieved through a cascaded PID controller. The outer loop regulates attitude angles, while the inner loop manages angular rates. The PID control law is:
$$ u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt} $$
where $e(t)$ is the error signal. For a quadrotor drone, this ensures stable hover and responsive maneuverability.
Integrating optical flow data with attitude control requires compensation for the quadrotor drone’s own rotations. The optical flow measured by the onboard camera includes components due to the drone’s rotation. The rotational contributions in the image plane are:
$$ D_{x_{\text{roll}}} = -\frac{T f}{Z} – \omega_y f + \omega_z y $$
$$ D_{y_{\text{pitch}}} = -\frac{T f}{Z} – \omega_x f + \omega_z x $$
where $f$ is the focal length, $Z$ is the altitude, $\omega_x, \omega_y, \omega_z$ are the angular rates from the IMU, and $T$ is the translation. The pure translational optical flow components used for localization are:
$$ D_{x_p} = D_x – D_{x_{\text{roll}}} $$
$$ D_{y_p} = D_y – D_{y_{\text{pitch}}} $$
This compensation is vital for accurate velocity estimation of the quadrotor drone. The final control signals for the quadrotor drone’s motors are generated by combining the PID outputs from altitude, attitude, and optical flow-based velocity control. The PWM outputs for motors $M_1$ to $M_4$ are:
$$ \begin{bmatrix} M_1 \\ M_2 \\ M_3 \\ M_4 \end{bmatrix} = \begin{bmatrix} u_h – u_{\psi} – u_x – u_y \\ u_h + u_{\psi} + u_x – u_y \\ u_h – u_{\psi} + u_x + u_y \\ u_h + u_{\psi} – u_x + u_y \end{bmatrix} $$
where $u_h$ is the altitude control PWM, $u_{\psi}$ is the yaw control PWM, and $u_x, u_y$ are the velocity control PWMs derived from optical flow.
We conducted extensive experiments to validate our improved algorithm for the quadrotor drone. The feature point tracking performance was evaluated under various indoor lighting and texture conditions. Our algorithm consistently detected and tracked a stable number of feature points, even in environments with low texture contrast, which is common for a quadrotor drone operating indoors. The table below summarizes the feature tracking performance:
| Environment Type | Average Feature Points Tracked | Tracking Stability (Time before loss) |
|---|---|---|
| High-contrast texture | 45-55 points | > 30 seconds |
| Low-contrast texture | 25-35 points | 15-25 seconds |
PID tuning for the quadrotor drone was performed using an enumerative approach within feasible ranges. The final PID parameters that yielded stable flight are:
| Control Loop | $K_p$ | $K_i$ | $K_d$ |
|---|---|---|---|
| Roll Angle | 3.5 | 0.05 | 0.8 |
| Pitch Angle | 3.5 | 0.05 | 0.8 |
| Yaw Angle | 4.0 | 0.1 | 0.5 |
| Altitude | 6.0 | 0.2 | 1.2 |
With these settings, the quadrotor drone exhibited fast response to disturbances without overshoot, maintaining attitude errors near zero. The altitude holding performance was tested, and the quadrotor drone maintained a hover at 35 cm with a vertical error of ±5 cm. Stable altitude control is essential for reliable optical flow computation on a quadrotor drone, as it ensures a consistent camera-to-ground distance.
The core localization accuracy of the quadrotor drone was assessed in fixed-point flight mode. We activated the optical flow localization and recorded the drone’s position deviations over time by analyzing video footage against a checkerboard pattern on the ground. The results indicate that the quadrotor drone could maintain position within a circular region of radius 30 cm for extended periods. The horizontal deviations in X and Y directions are summarized below:
| Time Interval (s) | Max X Deviation (cm) | Max Y Deviation (cm) | RMS Error (cm) |
|---|---|---|---|
| 0-10 | 12 | 15 | 9.8 |
| 10-20 | 18 | 20 | 13.2 |
| 20-30 | 22 | 25 | 16.1 |
The optical flow algorithm’s output, after compensation, provided velocity estimates that enabled the quadrotor drone to correct its position autonomously. The improved LK method demonstrated higher robustness compared to the standard pyramid LK, as feature points were rarely completely lost, ensuring continuous localization for the quadrotor drone.
In discussion, our approach highlights several advantages for quadrotor drone applications. The use of Shi-Tomasi corners ensures that tracked features are distinctive, reducing mismatches. Bilinear interpolation enhances sub-pixel accuracy, which is critical for precise velocity estimation of a quadrotor drone. The pyramid LK handles the motion scale variations typical in quadrotor drone flights. However, challenges remain, such as performance under varying lighting conditions or on featureless surfaces. Future work could integrate additional sensors like infrared cameras or machine learning techniques to further improve the quadrotor drone’s localization in diverse indoor environments.
In conclusion, we have presented and implemented an improved LK optical flow algorithm for indoor localization of a quadrotor drone. The method combines Shi-Tomasi corner detection, sub-pixel refinement, and pyramid-based optical flow tracking to achieve stable and accurate position estimation. Our experimental results confirm that the quadrotor drone can perform fixed-point flight with a precision of 30 cm radius under consistent lighting. The algorithm operates in real-time on onboard hardware, making it suitable for autonomous quadrotor drone missions in GPS-denied areas. This work contributes to the advancement of vision-based navigation for quadrotor drones, paving the way for more reliable indoor operations.