Improving Optical Flow Sensor Using a Gimbal for Quadrotor Navigation in GPS-Denied Environment

This paper proposes a new sensor using optical flow to stabilize a quadrotor when a GPS signal is not available. Normally, optical flow varies with the attitude of the aerial vehicle. This produces positive feedback on the attitude control that destabilizes the orientation of the vehicle. To avoid this, we propose a novel sensor using an optical flow camera with a 6DoF IMU (Inertial Measurement Unit) mounted on a two-axis anti-shake stabilizer mobile aerial gimbal. We also propose a robust algorithm based on Sliding Mode Control for stabilizing the optical flow sensor downwards independently of the aerial vehicle attitude. This method improves the estimation of the position and velocity of the quadrotor. We present experimental results to show the performance of the proposed sensor and algorithms.


Introduction
Research around Unmanned Aerial Vehicles (UAVs) has increased in recent decades due to their capacity for experimentation in different disciplines, low cost, and ease of repair compared to classical helicopter configurations.Sensor improvements and robust control algorithm developments allow for accuracy tasks in vehicles of different configurations such as fixed-wing aircrafts, rotorcrafts, and hybrids.Sensor-based navigation approaches are developed in specific environments and conditions for civil, military, or scientific applications.
In the early stages of development, optical flow-based navigation was limited by the range and field of view of the sensors.The frame-fixed optical flow module uses a downwardfacing camera and a distance sensor for velocity estimation.Optical flow detection is affected due to angle changes caused by UAV maneuvers.In hover flights the maneuvers are reduced, and the velocity estimation is reliable.
Motion capture can be used for high-precision indoor flight.In [1], the position of the multirotor aircraft is estimated by a set of cameras in a room; the tracking of the movement of the UAV was controlled offline to follow a ground vehicle, but the cost of the motion capture system is highlighted.The works [2,3] present open-air flights using GPS in precise long-range missions, analyze the sensitivity of GPS to interference, and propose a return-tohome flight mode using the heading angle to home.In [4], vision-based navigation is used for indoor and outdoor flights with high-cost computational requirements and lighting changes sensitivity.Moreover, navigation using optical flow and distance sensors requires low-cost computational and provides good accuracy in local environments.
In [5], the authors present an eight-rotor UAV using optical flow, and this rotorcraft uses the main four rotors for altitude and attitude displacement and another four rotors for horizontal maneuvers.This prototype uses the optical flow for estimating the horizontal position avoiding angular tilt.In [6] in the authors proposed improved motion compensation using feature block selection, look-ahead rotation, fault case detection and filtering using PX4FLOW hardware by tunning the camera resolution, the interval of adaptive boxes and advanced search algorithm.In [7] presents a UAV position estimation using an optical flow approach using a Gated Recurrent Unit (GRU) network-based pointing angles and Magnetic, Angular Rate, and Gravity (MARG) sensors improving robustness and performance in real-time experimentation.
Optical flow sensors used for position control in quadrotor aircraft are facing downwards and fixed to the airframe.In a flight based on the optical flow sensor, the roll and pitch maneuvers are restricted to ±15 • , while maneuvers in flights without optical flow sensors are up to ±35 • .The bounded angles reduce the horizontal displacement velocity of the vehicle.Quadrotor rotorcraft in an ideal hover flight would maintain the Euler angles of pitch and roll aligned to the horizontal and with a linear displacement velocity near zero.However, in practice (even in hover flight), a quadrotor rotorcraft vehicle is sensitive to disturbances and the responses of the attitude controller can be considered as optical noise.Furthermore, it is well known that tilting an optical flow module produces positive feedback on vertical position estimates and linear velocity estimates.
The rotorcraft in Figure 1a shows an optical flow module mounted directly on the quadrotor's frame.When the quadrotor tilts it causes a distortion in the estimation of the altitude and horizontal velocity.Figure 1b shows an optical flow module stabilized by a gimbal.The gimbal feedback control stabilizes pitch and roll angles on the horizontal plane, providing reliable altitude and horizontal velocity estimations with respect to the ground and avoiding optical distortions.The main contribution of the present paper is improving the optical flow sensing to obtain a reliable set of image data using a gimbal that in general is used to stabilize camera videos for recording or First-Person View (FPV) flights.The optical flow sensor is improved for trajectory control in GPS-Denied environments.This approach allows for flight maneuvers with large changes in the attitude angles; in other cases this produces positive feedback that destabilizes the orientation of the vehicle.Furthermore, we use a robust control technique to enable hover flights and trajectory following with tracking velocity without affecting the sensed images.
The paper is organized as follows: Section 2 describes the system mathematical models.Section 3 is devoted to the control algorithms.Materials and Methods are given in Section 4. The experimental results are shown in Section 5. A brief discussion about our approach is presented in section 6. Final remarks are given in the conclusions.

Mathematical Models
Mathematical models are approximations of the real systems.The subsystems include the dynamics, estimations, control inputs, and degrees of freedom.In the quadrotor aircraft, the forces and angular moments are induced by four actuators to move in the inertial reference frame (x, y, z) and rotate in their Euler angles (φ, θ, ψ).In the gimbal, the angular moments are induced by its three actuators that allow it to tilt around its Euler angles (φ g , θ g , ψ g ).
The optical flow module estimates the lateral and longitudinal displacement velocity and altitude of the complete system ( ẋ, ẏ, z).The following subsections present the mathematical models of the quadrotor aircraft, gimbal, and optical flow module to describe the complete system.

Quadrotor Aircraft Model
Figure 2 shows the representation of the quadrotor aircraft.Let us define the total thrust u as follows: where f i = kω 2 i is the force produced by i-rotor where i = 1, 2, 3, 4. k represents a set of aerodynamic constants, and ω i is the angular velocity.We obtain the Euler angles rates as The rotor torque τ (φ,θ,ψ) is defined as where l is the distance from the rotors to the center of the gravity and τ i is the torque produced by the i-rotor.Therefore, according to [8] the quadrotor aircraft model can be defined as where m is the mass, g is the acceleration due to the gravity, I represents the inertia matrix, C is the Coriolis matrix, η = (φ, θ, ψ) T is the attitude vector, and τ is the control input vector.

Gimbal Model
Figure 3 shows the gimbal diagram, where the G = (x g , y g , z g ) represents the frame fixed to the optical flow sensor.In [9,10], the authors propose the gimbal mathematical model as follows: where J = D(J x , J y , J z ) is the gimbal rotational inertia matrix, and η g = (φ g , θ g , ψ g ) represents the torques in the pitch, roll, and yaw gimbal angles.ξ = (ξ 1 , ξ 2 , ξ 3 ) are the friction coefficients for each angle.Moreover, υ = (υ φ , υ θ , υ ψ ) is the control input, and is the nonlinear dynamic effect of the couplings that are considered as a disturbance.

Optical Flow Model
Optical flow is a monocular vision method that uses images of a moving object to estimate the displacement velocity.
Figure 4 shows the optical flow diagram, where the O = (x o , y o , z o ) represents the frame fixed of the optical flow sensor.P is a direct point on the ground from the optical axis, f is the focal length from the centre of projection O to the image plane, and h is the altitude from the image plane to the ground.θ o is the optical axis angle and the z i axis.The authors of [11] present the approximate pixel displacements as follows: where (V x , V y ) are the estimate optical flow translation velocities, (ω x , ω y , ω z ) are the angular velocities, and (T x , T y , T z ) are the linear velocities.

Control Strategies
This section presents a feedback control algorithm implemented to stabilize the quadrotor aircraft altitude and the robust control algorithms that guarantee finite-time convergence for the quadrotor aircraft pitch and roll angles and for the gimbal pitch and roll angles.The main goal of the algorithms based on Sliding Mode Control is to stabilize the quadrotor aircraft attitude and the optical flow module attitude independently.The quadrotor aircraft altitude and attitude are controlled by the aircraft's four rotors angular velocity, and the gimbal is controlled by its two rotor torques that stabilize the optical flow module on the horizontal line.

Quadrotor Aircraft Control
The quadrotor aircraft robust control approach enables stable flight by damping external disturbances or attitude tilts for horizontal vehicle displacement.This behavior is very important for the entire aerial vehicle system.

Vertical Displacement and Yaw Control (z, ψ)
A feedback control algorithm has been selected to stabilize the vertical position, and it is obtained using the following input: where c 1 is defined as where k p , k d > 0 and z d is the desired altitude.Introducing ( 6) into (3) When the quadrotor aircraft reaches the desired altitude z → z d then c 1 → 0. The yaw angle ψ is controlled by where k a , k b > 0 and ψ d means the desired yaw angle.

Lateral Displacement and Roll Control (y, φ)
By applying (6) into Equation (3), after a convergence time, the lateral dynamics become ÿ ≈ mg tan φ (10) In order to guarantee the robust tracking in y-axis, differentiating (10) twice gives Introducing (3) into (11) and applying a variable change τ = τ + C(η, η) η and inertia are considered the identity matrix Let us define the following f a and f b as where f a (•), f b (•) represents terms obtained after a second derivative.We have considered Then, the y dynamics are In order to reach the desired trajectory, we propose the robust control based on the Sliding Mode Control algorithm.Let us define the following variables: and the following errors as The sliding surface is defined as a function of error where k 1 , k 2 , k 3 > 0. The objective is to reach the surface s = 0. We propose the following positive function: By choosing τ φ like where ν φ is defined as where ρ φ > 0 and ν φ denote the Sliding Mode Controller, then V = −ρ φ s sign(s).Then, s → 0 in a finite time.Then, from (19) this is an error linear system, where the Hurwitz dynamics characteristic polynomial is Consider it diagonal and compensate the torque.Considering a small angle φ, (22) becomes approximately ẍ ≈ −mg tan θ.Indeed, since the quadrotor aircraft configuration is a symmetrical frame, it is also possible to apply the Sliding Mode Controller to the pitch angle (θ) and to the longitudinal displacement in the x-axis.

Gimbal Control
The gimbal robust control approach ensures that the attitude of the optical flow module is stable and independent of the quadrotor aircraft attitude.This performance is very important for optical flow sensing.

Gimbal Pitch Control (θ g )
An algorithm based on Nonsingular Terminal Sliding Mode Control (NTSMC) is selected to stabilize the gimbal pitch angle (θ g ) due to the robustness to disturbances.
Since the tracking error converges to zero, we propose the following sliding surface: where sign γ (θ g ) = signθ g |θ g | γ , θ g is the gimbal pitch angle, and the variables satisfying α, β > 0, 1 > γ 1 > 2, γ 1 > γ 2 and sign(•) are the signum function.Then, s g guarantees the convergence to zero in finite time.The algorithm control law υ θ is defined as where K 1 , K 2 > 0. The control law υ θ for the pitch (θ g ) angle guarantees the finite-time convergence of the tracking error to zero.The stability proof can be found in [12].

Gimbal Roll Control (φ g )
Indeed, since the gimbal configuration is symmetrical and orthogonal in xy-axes, we applied the same control strategy to the angular displacement roll control (φ).
The algorithm control law υ θ is defined as The control law υ θ for the roll (φ g ) angle guarantees the finite-time convergence of the tracking error to zero.

Chattering Avoidance
It is well known that the implementation of the control approach based on sliding modes brings with it the disadvantage of the vibration effect present in the real-time experimentation of the sign function.In implemented control systems, it is important to ensure the performance of the system in the face of high working frequency and that the electronic controller can command in real time considering the electronic and mechanical wear of its actuators.However, in [8] the authors propose replacing the sign function with a high slope saturation function sign(•) ≈ sat(•) to solve this issue.
In our approach, in order to avoid the chattering effect caused by the signum function in robust control algorithms used in the quadotor attitude and gimbal attitude, the approximation function sign(•) ≈ tanh (•) is used to smooth the signal.The signum function and hyperbolic tangent function are shown in Figure 5.

Materials and Methods
In this section, we will describe the different hardware parts of the embedded control system, namely, sensors, actuators, pilot communication, and the flight controller board that links them together.The total weight of the Unmanned Aerial Vehicle is 2.5 kg; the weight of the gimbal is clearly considered as the payload.The setup presented can be scaled to a smaller quadrotor aircraft by using a smaller gimbal or using servomotors instead of brushless motors for stabilization purposes.However, as in the case of Unmanned Aerial Vehicles, the gains must be adjusted in flight tests.
In order to guarantee the stability of the aerial vehicle for the development of the experiments through the optical flow module, an advanced autopilot called: Pixhawk 6X is used, which provides the corresponding measurements (attitude, position) for the high-performance control of our quadrotor aircraft.
Finally, the optical flow sensor presented in this work uses a stabilized gimbal to avoid the positive feedback produced when the vehicle tilts.The vehicle platform was built with the components shown in Table 1. Figure 6 shows the system diagram.A quadrotor aircraft and optical flow module are stabilized by gimbal, where I = (x i , y i , z i ) represents the inertial frame, B = (x b , y b , z b ) represents the body frame fixed to the center of mass of the quadrotor aircraft, and O = (x o , y o , z o ) is the frame fixed to the optical flow module.Finally, h is the altitude and (V x , V y ) are the horizontal velocity estimations.
The flight controller and custom PX4 firmware are used to control the quadrotor aircraft and stabilized gimbal.The modular flight controller has redundant embedded sensors (IMU's), communication modules, and peripherals sensors.Figure 7 shows the experimental platform using an optical flow module stabilized by a gimbal.In this configuration, the experimental setup does not include GPS or MoCam.
The gimbal system configuration consists of two tilting joints around the x o and y o axis to control θ o and φ o close to zero.The gimbal mathematical model and robust control algorithm are important to ensure the performance through the torque stability of the motors.The gimbal robust control algorithm is implemented on a PX4 compatible controller board.In order to avoid the chattering effect caused by the signum function in robust control algorithms, we use the approximation function sign(•) ≈ tanh (•).The improved optical flow module placed on the gimbal ensures that the estimation of the vertical position and the horizontal displacement velocity are reliably estimated regardless of changes in pitch-roll angles caused by the attitude disturbances or by the quadrotor aircraft displacement.Therefore, the optical flow angular velocities are close to zero ω x , ω y ≈ 0; Equation ( 5) can be rewritten as Therefore, the distance and optical flow sensor estimates can be used to control the position of the quadrotor aircraft considering the altitude h ≈ z and the horizontal displacement as

Experimental Results
To validate the proposed control strategies for robust attitude control implemented in the quadrotor aircraft and the stabilized gimbal, we will summarize the various real-time experimental tracking tests at different stages of development, see Figure 8.The tracking performance can be observed as it reaches the desired position.The controller parameters were adjusted by trial and error until the best performance was obtained.The parameters used are shown in Table 2.The attitude angles performance of the quadrotor aircraft during real time flight are presented below.Figure 11 shows the robust control law algorithm (20) behavior applied on the pitch angle stability of the quadrotor aircraft with disturbances added over the whole trajectory.
Figure 12 shows the robust control law algorithm (20) behavior applied on the roll angle stability of the quadrotor aircraft, with disturbances added over the whole trajectory.
The quadrotor aircraft attitude angles (θ, φ) back to the reference in a short period of time, achieving normal operation after disturbances, with some oscillatory behavior around the horizontal reference values.Therefore, the results obtained are satisfactory.Figure 13 shows the robust control law algorithm (24) behavior applied on the pitch angle stability of the gimbal, with disturbances added over the fight.The gimbal attitude angles (θ g , φ g ) back to the reference in a short period of time to normal operation after rejecting the angular disturbances induced by the quadrotor aircraft, with some oscillatory behavior around the horizontal reference values.Therefore, the results obtained are satisfactory.

Optical Flow Sensing
The altitude and displacement velocity sensing in real time flight are presented below.Figure 15 shows the altitude estimate by the distance sensor to reach the desired altitude (z d = 2 m).
Figure 16 shows the optical flow estimation of lateral velocity during indoor flight.An increase in speed of almost 0.04 m/s is observed during its lateral path of 7 m.It is possible to observe a peak in the horizontal displacement velocity at the start and at the end of the flight.This occurs when the quadrotor aircraft does not take off or land with its full landing gear (due to disturbances, like the ground effect, or because the terrain is not flat and horizontal); the optical flow can detect a small horizontal displacement that increases with short focal length.

Discussion
There are many approaches around GPS-Denied navigation; some are mentioned in Section 1 of this work.The stabilized optical flow avoids the positive feedback of the velocity and position estimation.Regardless of the inclination rate and attitude change of the quadrotor, the stabilized optical flow will always maintain the downward orientation; this is the main goal of this work.However, a handicap of using a stabilized optical flow is the extra weight of the gimbal itself.Normally, the use of conventional optical flow sensors considers limitations in the translational velocity and the altitude of the sensor with respect to the ground as this altitude increases the estimation of the velocity, which is more sensitive to changes in sensor orientation.In our proposal, we are avoiding these attitude changes due to the stabilization of the optical flow.To carry out safe flight tests, it is required to know the operating limits of the sensors to produce reliable local estimates of altitude and horizontal velocity.For example, the maximum distance sensed by the optical flow module is 2 m, so we use a distance sensor with a longer operating range (40 m).

Conclusions
This paper has proposed a novel technique for navigating in a GPS-Denied environment using an optical flow module stabilized by a gimbal system.The proposed approach solves the problem of having a positive feedback in the pitch angle when the optical module is attached to the body of the aircraft.The technique consists of a control algorithm that keeps the optical module facing downwards so that the optical flow is independent of the pitch angle of the aircraft.We have successfully tested the proposed approach in an experimental platform in outdoor and indoor environments.The control algorithms of both the aircraft and gimbal were designed robustly to reduce the effect of external perturbations.The experiments showed that in spite of disturbances, the aircraft Euler angles remained close to the origin.

Figure 6 .
Figure 6.Navigation system: A quadrotor aircraft and a stabilized optical flow module.

Figure 8 .
Figure 8. Stabilized gimbal in hover flight test with external perturbations.

Figure 9
Figure 9 shows an indoor flight tracking control test, and Figure 10 shows the trajectory in three dimensions of the quadrotor aircraft, which demonstrates the correct execution of the indoor flight mission using the optical flow sensor to track the programmed trajectory in the quadrotor aircraft at an altitude of 2 m and with a distance of 7 m.

Figure 9 .
Figure 9. Indoor flight tracking control test with stabilized optical flow.In the following link, we share a video to show the disturbance rejection tests of the unmanned aerial system to demonstrate the quadrotor aircraft performance and the stabilized gimbal performance.Additionally, hover stability and a good performance in optical flow-based trajectory tracking control in a GPS-Denied environment are ensured.

5. 2 .
Gimbal Pitch and Roll Attitude (θ g , φ g )The attitude angles performance of the stabilized gimbal during real time flight are presented below.

Figure 14
Figure14shows the robust control law algorithm (25) behavior applied on the roll angle stability of the gimbal, with disturbances added over the fight.

Figure 17
Figure 17 shows the optical flow estimates of the lateral and longitudinal velocities, respectively, during the indoor flight.A lateral velocity of 0.01 m/s during a longitudinal path of 1 m is observed.