Visual Soft Landing Of An Autonomous Quadrotor On A Moving

Pad Using A Combined Fuzzy Velocity Control with MPC

Oussama Bouaiss1,∗, Raihane Mechgouga , Abdelmalik Taleb-Ahmedb

a Department of Electrical Engineering, LESIA Laboratory, University of Biskra, Algeria
b IEMN UMR CNRS 8520, Polytechnic University of Hauts-de-France, Valenciennes

Abstract
This paper demonstrates a sophisticated solution for the soft landing application of a fully
autonomous quadrotor on a moving pad considering external disturbance, model uncertainties &
actuators noise. The special designed landing pad is initially detected by an onboard vision sys-
tem with a robust Algorithm to estimate its coordinates precisely by a fusion of the the camera
pose estimation with Inertial Navigation System (INS) filters. Different sizes and designs of the
ArUco markers were attentively chosen to ensure best detection at different altitudes and angles of
approach.
The desired trajectory for way point and landing is dynamically generated based on minimal
jerk optimization, integrating a bio-inspired velocity profile by Fuzzy Logic Controller (FLC) with
a position loop estimator to smoothen the landing in an accordance with an ultrasonic sensor data.
Kalman filtering was used for best relative pose estimation and fusion of the asynchronous sensors
data.
Model Predictive Control (MPC) was opted for quadrotor control to track the generated tra-
jectory in time reference with the rejection of disturbance.
This solution presents a soft mechanism for flat surfaces landing that is similar to human decision
concept and control, as this method will ensure the absorption of impact shock and dynamic optimal
tracking of the moving landing pad.
Matlab simulation with comparison and data fusion experimentation results validate the effec-
tiveness and reliability of the proposed landing task solution.
Keywords: Unmanned Aerial Vehicles, Quadrotor, underactuated systems, Soft landing, Fuzzy
logic, MPC, pose estimation, ArUco

1. Introduction

Quadrotor is one of the most interesting types of the UAVs, it received a large volume of papers
and researches because its flexibility, maneuverability, and its hovering mode ability. Its use was

∗ Corresponding author
Email addresses: oussama.bouaiss@univ-biskra.dz (Oussama Bouaiss), raihane.mechgoug@univ-biskra.dz
(Raihane Mechgoug), Abdelmalik.Taleb-Ahmed@uphf.fr (Abdelmalik Taleb-Ahmed )

Preprint submitted to Journal of Automation, Mobile Robotics and Intelligent Systems November 6, 2021
effective in detection, civil & military surveillance, and shipping of commercial & medical services.
The Quadrotor is a very complex non linear system with under-actuation of control.
Because of accurate Inertial Navigation System (INS) and algorithms, Quadrotors have become
able to perform complicated manoeuvres and autonomous tasks even in the harsh conditions or
hazardous environment. Thus comes the option of Autonomous landing as a vital task for many
missions and as a fail safe in case of some technical issue during the flight such as GPS signal lost.
Several papers tackled the quadrotor landing, Zhe et al. in [1] developed a vision based au-
tonomous landing on a moving surface vessel with PID tracking and control, the relative pose was
based on the visual detection and estimation of the landing area. In [2] Danjun et al. considered the
ground effect based on the Cheeseman’s model for more precision of the quadrotor Vertical Taking
Off and Landing (VTOL) with a robust PID control. An efficient 3-DOF time optimal trajectory
tracking by a quadrotor model on a moving pad was addressed in [3]. Yuhua et al. proposed
a complete solution for autonomous landing of a low-cost quadrotor on a moving new designed
landing pad with Lyapunov stability demonstration.[4]. Other papers studied the landing control
of the UAV on an oscillating pad with sea characteristics [5] considering an introduction of non
linear KF [6]. In [7] Carlos et al. proposed an uncertain quadrotor smooth landing on a moving
pad without a relative state vector availability using a LQR to track the trajectory in the presence
of the unknown uncertainties.
Computer vision is the field that includes methods for acquiring, processing, analyzing, and un-
derstanding images and high dimensional data from the real world in order to produce numerical or
symbolic information. To comply with autonomous landing of the quadrotor, the vision processing
is crucial to determine the relative pose in order to generate the trajectory for the landing. Many
algorithms of vision subject to detection and estimation for autonomous landing have been devel-
oped. ArUco markers were widely used because of the easiness of the detection and the efficiency of
the identification [6]-[4]-[1]. Whereas the pose estimation varies from direct methods that use PnP
algorithm with a calibrated camera [8]-[4], or sophisticated & intelligent methods based on neural
networking [9].
It is critical to consider the noise and disturbance model in the control and estimation during
the robust landing process, thus the estimation of quadrotor state vector was based on Madgwick
data fusion of asynchronous sensors filter [10] enhanced by an Extended Kalman Filter, another
filter was set for best estimation of markers localization based on visual estimation. The visual
processing algorithm is so sensitive in matter of data extraction (size, pattern and pose), to camera
type and its calibration, to the shutter effect, to the velocity of drone’s landing and the external
light and initial field of view. Image noise and accuracy is also affected by the type of camera, as
a mirror effect can deteriorate the performance [8].
Based on literature, some researches used pursuit guidance while simultaneously descending with
the UAV in altitude [11], where in [12], a fusion of Monocular SLAM and INS was utilized in landing
of the quadrotor in bio-inspired guidance method which is based on Tau theory. In [13] complex
manoeuvres were illustrated by a robust trucking control that can be constrained for landing. In
this paper, to enhance the bio-inspired soft landing, a novel Fuzzy decision maker process is used
to estimate the landing time via the average possible speed, the Fuzzy sets use the observed state
vectors of both the quadrotor and moving pad to make a decision about the velocity of landing.
This method controls the velocity limits to design the trajectory to track for the landing. The
landing profile will be similar to an expert guidance control.
MPC is chosen for better performance of tracking than the widely used PID, MPC is adaptive
and can handle the considered disturbance and Gaussian noise.

2
 The proposed soft landing approach separates the process into two interconnected parts:
• Detection of markers, identification of at least one visible marker depending on the height,
and then determine the relative pose of the pad center with consideration to the motion of
both the quadrotor and the landing pad.

• Generation of minimal Jerk trajectory to ensure smoothness during landing, where a Model
Predictive Control is responsible of tracking iteratively the desired trajectory to land. the
velocity profile is controlled by the FLC decision maker.
The opted approach’s philosophy is demonstrated in Fig.1.

Figure 1: Autonomous Landing control frame work

The aim during this project was to develop a complete application for the smooth autonomous
Landing of a quadrotor on a moving pad based on a vision algorithm using a monocular camera
with a robust position control. the iterative and adaptive Generation of trajectory was based on
optimal jerk.
This paper has valued the following contributions:
• Design a robust application of the pose estimation based on a special landing pad by multiple
ArUco markers with different patters at different sizes to ease the detection task at a wide
range of altitudes.
• MPC control is adopted to design an optimal position tracker which can not only track the
reference state but also attenuate the influence of the external disturbance with a comparaison
to PID results.

• Planning a mimic of natural decision making as expert human processing for landing by
adding FLC decision making of average velocity and estimation time of landing.
• Data fusion of asynchronous sensors with an EKF to obtain accurate estimation for the
quadrotor and the pad.

3
 The rest of this paper is organized as follows: section 2, presents the landing pad design, the
vision processing approach for detection, the identification of the patterns and the pose estimation,
in Section 3 the dynamics of the quadrotor are modeled. In the section 4, the EKF and data fusion
were presented with sensor communication and magnetometer calibration for Attitude and Heading
Reference system (AHRS). Section 5 develops the generation of the optimum Jerk trajectory, with
the fuzzy velocity control. The MPC for the tracking of the desired trajectory for the landing
is detailed in Section 6. Results of the estimation of the relative pose by vision and control are
presented in Section 7, Finaly, Section 8 concludes the paper.

2. The detection of the pad and its relative pose estimation

Prior engaging the landing process, the detection and the estimation of the relative pose of
the moving landing pad is firstly processed, this section investigates this problem with the aim to
increase the detection probability and to avoid miss-estimation samples and the limitation of the
Field Of View (FOV).

2.1. Concept of detection

Many techniques for the pad detection were investigated, such as color based, or form based.
In this paper, we opted for the ArUco markers. The use of binary square fiducial markers provides
enough data for the detection by the four corners, the identification from the binary codification
and the relative pose thru a projection processing. All those features are easy to be acquired by
different types of camera and from relative higher altitudes. In addition, choosing different markers
designs at different sizes will ease the task of close detection in same time as far ones, which ensure
a robust detection all long the trajectory of landing.
The quadrotor has the ability via its on-board camera to detect the markers at different altitudes
to fulfil the detection requirements. The design of the landing pad is based on providing a multitude
of markers in shapes and sizes to enhance their detection, as the quadrotor got close to the landing
pad, some markers leave slowly the Field Of View (FOV), at this level, the detection will be
based only on the relatively smaller markers for the close to pad landing process. Every pattern
is recognizable and identifiable and can lead the quadrotor to estimate the pad center relative
coordinates for a robust landing while minimizing the error and noise by redundancy of detection.
An added filter is used for better estimation of the relative pose of landing area.

2.2. Landing pad design

Yuhua Qi et al. proposed a similar aspect of detection for a low cost camera in [4]. The
redundancy of markers will enhance the total probability of detection and recall, precision as well
will be augmented as we use small patters at low altitude. probability of non detection can be
written as: X
PN D = 1 − (1 − Pi )
i

Where Pi represents the non detection probability of marker ’i’.

Note: Tables 1. 2 and 3. were made during hovering mode of the quadrotor to optimize the
pad design considering the real aspect of vibration, flight disturbance and high frequency noise.
For that many real time images were taken and analyzed. Based on using one vs multiple markers
and varying their sizes, experimental tests of detection probability and precision are explicit in the
tables. 1. 2 and 3.

4
 Table 1: Multiple markers vs single markers detectability and precision

Markers Detectability (recall) Pose Estimate precision

Single Marker 97.1 4.89 cm
Multiple markers 99.3 2.55 cm

Table 2: Detection rate for different sizes of markers from different heights with Non Applicable cases (NA)

0.3 m 0.6 m 1m 3m 9m
5 cm 100 % 99.2 % 91.1 % 8.1 % (NA) 0%
10 cm 95.2% 99.2 % 97.4 % 88.9 % 31 % (NA)
18 cm 0 % (NA) 87.7 % 97.9 % 99.8 % 88.6 %

Table 3: Precision of pose estimation at different sizes of markers from different heights with the Non Applicable
values (NA)

0.3 m 0.6 m 1m 3m 9m
5 cm 3.35 cm 5.11 cm 9.41 cm (NA) (NA)
10 cm 2.71 (NA) 3.46 cm 6.99 cm 19.66 cm (NA)
18 cm (NA) (NA) 4.97 cm 12.93 cm 28.9 cm

Besides the detection accuracy, the precision of the pose will significantly increase as we enable
multi measurement which will attenuate the noise of calculation to a tolerated error margin of the
general position determination.
From the previous tables, we conclude that multiple markers increases the detection probability
of the pad, and the estimation precision of the relative pose got better. In addition, the different
sizes of markers plays an important role of pose estimation and detection from different heights, low
altitudes uses small marker as they are more adequate for detection and estimation where larger
ones are out of the FOV. Contrary to the high altitude detection, where we rely completely on
larger markers as the smaller ones are less detectable with bad precision.
The ArUco library was used as a base of markers design [14]. At every iteration image, the
pose estimation process is initially based on the markers detection algorithm that recognizes the
detected markers and identifies them thru binary codification. then ran an estimation process for
relative pose of the landing pad. A special designed filter decides which markers to be considered
based on the actual relative distance between the quadrotor and the landing pad, also the filter
extracts out the bad estimation based on the normalized variance to the mean.
Extraction(Boolean) = 1

if and only if:

X
(Estimation(i))2 − ( estimation(i)/N )2 )/M ax(Abs(V ariance(i))) > 0.3 (1)
i

The small markers are not a good reference for pose estimation from a far distance, where they
are the favorite to be considered at closer ranges. The final relative pose data are exploited in
Kalman filtering that considers the relative motion between the quadrotor and the moving pad. In
other words, process is stepped as following:

5
 • Detection of markers: Detecting different square contour shapes.
• Recognition: Analyze inner codification & Attribute the marker in a canonical form by dif-
ferentiating between black and white surfaces using simple Otsu thresholding technique . and
exclude wrong detection and identify markers from the list.
• Relative pose estimation: using an algorithm based on Open CV library for pose estimation
and KF.
• Filter to decide which markers to be considered with a mean value calculation and exclusion
of bad estimate.
Inner codification choice of ArUco markers was based on simple detection and reading from far
distance and more dense for smaller ones to improve close estimation.

2.3. Relative pose estimation

Perspective-n-Point (PnP) was widely tackeled to estimate the 3D pose of the moving object
using ArUco markers, PnP is based on estimating the pose of a calibrated 6 DOF camera given a
set of n points in the 3D environment and their corresponding 2D relative image projections.
Pinhole Model is used To treat depth perception with Open CV library for the estimation
process. The Calibration of the camera was done ahead by getting the Camera Matrix and the
Distortion Vector. A special calibration code that uses Open CV for camera calibration tools and
Charuco image was used to extract the needed data for calibration.
The Calibration of the camera is building the following matrix of 3x3 elements with the focal
lengths and the camera optical center coordinates and the distorsion vector.
 
fx 0 C x
Camera matrix =  0 fy Cy  (2)
0 0 1
 
Distortion V ector = K1 K2 P1 P2 P3 (3)
In [15], The homography method for planar targets followed by non-linear optimization using
the well-known Levenberg–Marquardt (LM) algorithm to minimize reprojection error was opted to
estimate the transformation matrix [4].
Relative pose estimate algorithm provide position of camera on board to the landing pad ref-
erence. The relative attitude of the quadrotor in the same refence can be calculated with matrix
transformation formula as follows.
   
XM XC
 YM   YC 
 ZM  = T M C  ZC  (4)
   

1 1
Where C denotes camera frame and M denotes marker frame.
During flight vibration of the quadrotor and shuttering effect of the camera both introduce
measurement noise. KF has mainly a correction effect of the direct estimation of relative pose from
image processing.
Thru a multiple pose estimations using many markers, an average estimation is fed to the KF
for final estimation.

6
3. Dynamics modeling

3.1. Dynamics
The Quadrotor dynamics have been discussed in many papers. The validated model is derived
based on the consideration of two frames as shown in Fig.2. [16].
B frame: that represents the Body related reference, B(Ob , Xb , Yb , Zb ).
E frame: which is the Earth related reference: E(Oe , Xe , Ye , Ze ).
The E frame will coincide with the landing pad in motion.

Figure 2: Quadrotor structure with different control inputs

Table 4: Symbols definition

Symbols Defintions Symbols Defintions

φ Roll angle
ψ Yaw angle
JT P Total rotors inertia
b Thrust coefficient
d Drag coefficient
m Quadrotor mass
I3∗3 3 by 3 Identity ma-
trix
Ti Torque of motor ”i”
θ Pitch angle
Ω Rotor speed
l Lever
g Gravity
Ω Rotor speed
I Inertia matrix
03∗3 3 by 3 Zeros matrix
Ixx , Iyy , Izz Inertia matrix diag-
onal in B-frame

Considering Ui as thrust torques, we may write:

   
0 0
0  0 
  
U1   P4 2

b i=1 Ωi 
UB =   = 
  
2 2
 (5)
U2   b l (Ω4 − Ω2 ) 

U3   2 2
b l (Ω3 − Ω1 ) 
U4 2 2 2 2
d (Ω4 + Ω2 − Ω3 − Ω1 )

Where Ω = −Ω1 + Ω2 − Ω3 + Ω4 .
Finally, Considering all applied forces and torques contributions on the quadrotor body, the
dynamics may be simplified in the following equations:

7
 

 Ẍ = (sψsφ + cψsθcφ) U1 /m




 Ÿ = (−cψsφ + sψsθcφ) U1 /m
Z̈ = −g + cθcφ U1 /m

(6)

 φ̈ = 1/Ixx ((Iyy − Izz ) φ̇ψ̇ + JT P φ̇Ω + U2 )
 = 1/Iyy ((Izz − Ixx )θ̇ψ̇ + JT P θ̇Ω + U3 )



 θ̈
 ψ̈ = 1/I ((I − I )θ̇φ̇ + U )
zz xx yy 4

3.2. State space representation

Quadrotor Dynamic can be represented in state space as follow:

Ẋ = F (X, U ) (7)

Considering state vector X = (x1 x2 ... x12 )T in R12 , Dynamics can be written as follows:
   
x1 = x x2
 x2 = ẋ1   (cos x7 sin x9 cos x11 + sin x7 sin x11 ) U1 /m 
   
 x3 = y   x4 
   
 x4 = ẋ3   (cos x7 sin x9 sin x11 − sin x7 cos x11 ) U1 /m 
   
 x5 = z   x6 
   
 x6 = ẋ5   −g + (cos x7 cos x9 ) U1 /m 
  ; F (X, U ) =   (8)
 x7 = φ   x8 
   
 x8 = ẋ7   1/Ixx ((Iyy − Izz ) x12 x10 + JT P x10 Ω + U2 ) 
   
 x9 = θ   x10 
   
 x10 = ẋ9   1/Iyy ((Izz − Ixx )x12 x8 + JT P x8 Ω + U3 ) 
   
 x11 = ψ   x12 
x12 = ẋ11 1/Izz ((Ixx − Iyy )x10 x8 + U4 )

From the system dynamics, the Cartesian position vector depends on the attitude states, how-
ever, the attitude of the quadrotor is independent of linear positions. nested loop control is based
on this relative dependency [16].

3.3. Linearization
Linearized model was approximated by Jacobian matrices developed at ”X0 , uo ”:

Ẋ ≈ AX + BU (9)
 ∂f ∂f1

1
∂x1 ... ∂xn
 . .. 
 
A =  .. .. (10)
. . 
 
∂fn ∂fn
∂x1 ... ∂xn X0 ,u0

8
And  ∂f 
1 ∂f1
∂U1 ... ∂Um
 . .. 
 
B =  .. .. (11)
. . 
 
∂fn ∂fn
∂U1 ... ∂Um X0 ,u0
This linear equivalent system will be used for the EKF and the control.

4. Madgwick / EKF and Data fusion

In order to be able to land autonomously on the moving pad, the Quadrotor should localize it
self first during the flight using multiple asynchronous sensors, then estimate the relative pose to
the landing pad and move upon a minimal jerk trajectory.
Initially, an Inertial Measurement Unit (IMU) is used to track all rotations and translations with
a relative errors and noise of measurements [17]. a magnetometer is used to enhance the Attitude
and Heading Reference system (AHRS) filter, then the EKF based on the Jacobian of the Linear
Time Variant (LTV) model will use the Madgwick filter of quatornian orientation and the GPS
data to provide the optimal orientation and position estimate.

Figure 3: Landing philosophy references of motion

Secondly, the pad motion model will be considered in a second KF as an observer with visual
estimation inputs.

4.1. AHRS Filter and EKF

AHRS is a Continuous Discreet method for the estimation of the orientation and heading of the
quadrotor UAV via asynchronous sensors data fusion and even GPS when available. The EKF later
considers the internal disturbances including the white Gaussian noises of the plant and sensors
noises[18].
Quaternion orientation is chosen to avoid the gimbal lock that may occur while using the Euler
rotation angles.
Accelerometer and magnetometer are susceptible to cancel gyro integrative bias drift [19].

9
4.2. Angular rate Orientation filter
Quaternion orientation based on angular rate ws can be established thru the Gyroscopic filter
mentioned in Fig.4.

Figure 4: Gyroscopic angular rate quaternion filter

The dead reckoning of angles based on gyroscopic reading is too sensitive to noise and bias drift.

4.3. Magnetic/accelerometer filter

The accelerometer and the magnetometer sense the earth gravity and magnetic fields and all
linear accelerations of the quadrotor as well, which lead to fuse with Gyro filter for an optimal
estimation of the quatornian orientations. However the magnetometer should be calibrated first
from both soft and hard iron distortions as shown in Fig.5, where calibration was established using
Matlab and the sensor MPU9250.The blue sphere shows the nominal magnetic field whereas the
red ellipsoid proves the distortion of the used sensor.

10
 Figure 5: Hard and soft iron magnetic distortion of MPU 9250

The Calibration of the magnetometer is to determine the Ci coefficients of the following equation
Eq.12.
    
c1 c2 c3 Hx c10
M agreadings = c4 c5 c6  Hy  − c11  (12)
c7 c8 c9 Hz c12
The accelerometer/magnetometer combination can simply provide the orientation of the quadro-
tor by the following equations:
θ = arcsin(Accx /g)
φ = arctan(−Accy /accz ) (13)
ψ = arctan(magz /magx ) + D
Where D is the declination between the magnetic north and true north.
Madgkwick formulated an optimization function to get the quaternion orientation from the
combination of accelerometer and magnetometer [10]. however his method doesn’t consider the
dynamics of the model.

4.4. Data fusion for Orientation

For the synchronous quatornian estimations of the quadrotor, a complementary filter was pro-
ceeded as follows: [17].
qest,i = γi qa−est,i + (1 − γi ) qw−est,i , 0<γ<1 (14)
Where qw−est,i is the angular rate sensor quaternion and qa−est,i is the accelerometer/magnetometer
quaternion orientation estimate.
γi is an optimal variable based on bad estimation method penalty, and it is calculated by:
γi erra,m = (1 − γi )errw (15)
With errw is the magnitude of quaternion derivative corresponding to the gyroscope measure-
ment error and erra,m is the magnitude of quaternion error of the accelerometer/magnetometer
method.

11
 γ = errw /(errw + erra,m ) (16)

4.5. Linear position update by GPS

The GPS data of the 5 Hz NEO-6M are added to the EKF as well but not used for the landing,
general position is used in proactive way-points application [20].

4.6. Design of the Kalman Filter gains for the quadrotor

The optimal EKF that observes the states vector including the orientations and the sensors bias
was considered based on The six DOF linear model [21].
A Linear Time Variant (LTV) dynamic model of the quadrotor using the Jacobin lineariza-
tion which is combined with the sensors filter was found better than the Madgwick filter for the
quatornian orientation as it does integrate the prediction data [17].
The EKF uses orientation filter data to observe the augmented state vector of 31 elements as
shown in the table.5.
Table 5: State vector elements

States Units Index

Orientation (quaternion ) 1:4
Orientation (Euler angles) rad 5:7
Angular Velocity (wx , wy , wz ) rad/s 8:10
Position (NED) m 11:13
Velocity (NED) m/s 14:16
Acceleration (NED) m/s2 17:19
Accelerometer Bias (XYZ) m/s 2 20:22
Gyroscope Bias (XYZ) m/s 2 23:25
Geomagnetic Field Vector (NED) uT 26:28
Magnetometer Bias (XYZ) uT 29:31

Quaternion orientation and linear positions are are updated by EKF, the design is explicit in
the Fig.6

4.6.1. EKF equations

The iterative equations of the EKF will be as follow [22]:

Ai = ∂F/∂x|xi ,ui


Bi = ∂F/∂u|xi ,ui




xˆ0 − = x̂− (0)




−
P0 = E[x˜0 − x˜0 − ]




Ki = Pi− CiT [Ci Pi− CiT + Ri ]−1 (17)


 x̂i + = x̂i − + Ki [yi − Ci x̂i ]
Pi+ = (I − Ki Ci )Pi− (I − Ki Ci )T + Ki Ri KiT




ˆ − = Ai x̂i + + Bi ui



 xi+1
ˆ− = A P + AT + G Q GT


Pi+1 i i i i i i

12
 Figure 6: EKF Design with pre-filtering and data fusion

5. Trajectory generation

Several method for trajectory generation have been examined in literature. Direct time or effort
optimal trajectory generation is not within the scope of this paper, The geometric treatment of
the problem greatly simplifies both the modeling and control law for the system [23], min Jerk was
considered in this work, although, Vijay et al proved that a min snap is equivalent to a min effort
for quadrotor trajectory generation [24].

5.1. Minimum Jerk trajectory

Neville Hogan developed the notion of the jerk optimization which is the minimization of the
position third time derivative, aiming for the smoothness of the position X(t), the jerk can be
written as: ...
JerkX (t) = X (t) = ∂ 3 X(t)/∂t3
To ensure the smoothness of motion of the quadrotor, it is judicious to minimize the jerk along
the trajectory with satisfaction of Boundary conditions:
tf
... 2
Z
J= X (t) dt (18)
t0
...
Euler Lagrange Equation is used to solve the optimization considering L = X (t)2 , Necessary
condition satisfied by the “optimal” function.

13
 ...
∂L/∂X − d/dt(∂L/∂ Ẋ) + d2 /dt2 (∂L/∂ Ẍ) − d3 /dt3 (∂L/∂ X ) = 0 (19)
(6)
The solution of such an optimization is: X (t) = 0 Hence

X(t) = c5 t5 + c4 t4 + c3 t3 + c2 t2 + c1 t + c0 (20)
Where ci are found by satisfying the boundary conditions of initial and final states. as we aim
to land on a moving landing pad, a control loop of the vision and estimation updates the target
position, thus, the final state changes such the optimization as well.
For every iteration of the estimation, the optimization process calculate the parameters online
using the initial dynamic state and the final pose estimate.
For 3D jerk optimization in our quadrotor landing case, we can write:
tf
... 2 ...
Z
...
J= x (t) + y (t)2 + z (t)2 dt (21)
t0
Which define a minimum jerk 3D trajectory:
 5 4 3 2
x(t) = cx5 t + cx4 t + cx3 t + cx2 t + cx1 t + cx0

y(t) = cy5 t5 + cy4 t4 + cy3 t3 + cy2 t2 + cy1 t + cy0 (22)

z(t) = cz5 t5 + cz4 t4 + cz3 t3 + cz2 t2 + cz1 t + cz0


In order to find those parameters we need to solve three systems of six algebraic equations each,
between the initial and final states for every dimension. the problem of the solution is that dynamic
resolution, requires a good estimate of quadrotor position, velocity and acceleration. Where the
need of the KF with sensors data fusion detailed in section 4. Additionally, final time of each
iteration should be calculated based on the required speed for that position. This latter is decided
by a fuzzy logic controller.
The minimum jerk 3D trajectory mentioned by Eq.(22) describes system smoothness between
initial and final times. for the aimed case, the final time is unknown, so the problem is refered to
be a feed forward control. based on this approach, the optimization will be divided to segments at
which we apply to find a new smooth trajectory depending on the quadrotor estimated states and
the relative pose to the landing pad in motion.
A simple reformulation of this problem lays on normalizing the time between to & tf :

τ = (t − t0 )/T (23)

T = tf − t0 (24)
The 3D minimum jerk Trajectory will be generated by:
 5 4 3 2
x(t) = cx5 τ + cx4 τ + cx3 τ + cx2 τ + cx1 τ + cx0

y(t) = cy5 τ 5 + cy4 τ 4 + cy3 τ 3 + cy2 τ 2 + cy1 τ + cy0 (25)

z(t) = cz5 τ 5 + cz4 τ 4 + cz3 τ 3 + cz2 τ 2 + cz1 τ + cz0


where
dτ /dt = 1/T (26)
Hence, the velocity in time reference is:

14
  4 3 2
ẋ(t) = 5cx5 τ /T + 4cx4 τ /T + 3cx3 τ /T + 2cx2 τ /T + cx1 /T

ẏ(t) = 5cy5 τ 4 /T + 4cy4 τ 3 /T + 3cy3 τ 2 /T + 2cy2 τ /T + cy1 /T (27)

ż(t) = 5cz5 τ 4 /T + 4cz4 τ 3 /T + 3cz3 τ 2 /T + 2cz2 τ /T + cz1 /T


And,the Acceleration:
 3 2 2 2 2 2
ẍ(t) = 20cx5 τ /T + 12cx4 τ /T + 6cx3 τ /T + 2cx2 /T

ÿ(t) = 20cy5 τ 3 /T 2 + 12cy4 τ 2 /T 2 + 6cy3 τ /T 2 + 2cy2 /T 2 (28)

z̈(t) = 20cz5 τ 3 /T 2 + 12cz4 τ 2 /T 2 + 6cz3 τ /T 2 + 2cz2 /T 2


The system of Equations.28 is used to form the algebraic equations between initial state which
is given by the KF estimation at initial instant of the current iteration and final time state define
by the desired landing on pose estimate of the pad.
Initial state:
x(t0 ) = x0 vx (t0 ) = vx0 ax (t0 ) = ax0
y(t0 ) = y0 vy (t0 ) = vy0 ay (t0 ) = ay0
z(t0 ) = z0 vz (t0 ) = vz0 az (t0 ) = az0
Where final state is trivial described by:

x(tf ) = xp ad vx (tf ) = 0 ax (tf ) = 0

y(tf ) = yp ad vy (tf ) = 0 ay (tf ) = 0
z(tf ) = zp ad vz (tf ) = 0 az (tf ) = 0
Resolution of the system should be in function of T :
At t = t0
cx0 = x0 cx1 = T vx0 cx2 = T 2 ax0 /2
cy0 = y0 cy1 = T vy0 cy2 = T 2 ay0 /2 (29)
2
cz0 = z0 cz1 = T vz0 cz2 = T az0 /2
at t = tf , normalized time τ will be τ =1, thus:

cx3 = 10(xf − x0 ) − 6T vx0 − 3T 2 ax0 /2

cx4 = −15(xf − x0 ) + 8T vx0 + 3T 2 ax0 /2
cx5 = 6(xf − x0 ) − 3T vx0 − T 2 ax0 /2
cy3 = 10(yf − y0 ) − 6T vy0 − 3T 2 ay0 /2
cy4 = −15(yf − y0 ) + 8T vy0 + 3T 2 ay0 /2 (30)
2
cy5 = 6(yf − y0 ) − 3T vy0 − T ay0 /2
cz3 = 10(zf − z0 ) − 6T vy0 − 3T 2 az0 /2
cz4 = −15(zf − z0 ) + 8T vz0 + 3T 2 az0 /2
cz5 = 6(zf − z0 ) − 3T vz0 − T 2 az0 /2
Where all initial states are resulting from the estimation of Kalman filter, and the final ones result
from the pose estimation of the moving pad.

15
5.2. Feedback controlled trajectory


The described trajectory by P (t) : x(t), y(t), z(t) is known and it is T dependent. so at every
instant labeled by t0 , we define

Px (t) = [x(t) ẋ(t) ẍ(t)]T (31)

Px0 = [x0 vx0 ax0 ]T (32)

In fact using the jerk equations:
... 2 3 3 3
x
 ...(t) = 60cx5 τ /T + 24cx4 τ /T + 6cx3 /T

y (t) = 60cy5 τ 2 /T 3 + 24cy4 τ /T 3 + 6cy3 /T 3 (33)
 ...

z (t) = 60cz5 τ 2 /T 3 + 24cz4 τ /T 3 + 6cz3 /T 3
... 3 3 2
 ...(t0) = 6cx3 /T = 60/T (xf − x0 ) − 36/T vx0 − 9/T ax0
 x
y (t0) = 6cy3 /T 3 = 60/T 3 (yf − y0 ) − 36/T 2 vy0 − 9/T ay0 (34)
 ...

z (t0) = 6cz3 /T 3 = 60/T 3 (zf − z0 ) − 36/T 2 vz0 − 9/T az0
Eqs.(34) present three dynamics for x(t), y(t) & z(t) between initial time t0 which is the starting
time of each iteration and tf which is the estimated landing time.
      
ẋ 0 1 0 x 0
 ẍ  =  0 0 1  ẋ +  0  xf (35)
... 3 2
x −60/T −36/T −9/T ẍ 60/T 3
By analogy we get dynamics for y(t) and z(t). so giving an initial state at initial time of iteration,
the dynamic described in Eq.(35) will provide a smooth trajectory that minimize the jerk.
The unknown ”T” value, will be defined by an expert Fuzzy Logic Controller (FLC), that
considers many inputs to validate the right velocity to target and the estimated time for the landing.
Jerk, Snap, crackle and pop are the third, fourth, fifth, and sixth derivatives of position respec-
tively. In [25] Vijay et al used snap optimization to generate trajectory with minimal effort for the
quadrotor, Nevertheless, Richardson et al analysed how X(t) would change as a function of ’n’ in
the optimization functional.
Z tf

2
J= x(t)(n) dt (36)
t0
They demonstrated that at higher derivative order the more solution tends to prove a higher change
of speed at the trajectory inflexion point, change becomes narrower as we minimize jerk, snap and
crackle [26]. in our project, we aimed essentially for a smooth control and landing which justify the
minimization of the Jerk.

5.3. Fuzzy Velocity control

In [11], Alvika et al processed the velocity by a log polynomial to smooth the trajectory. However,
to bio-inspire the human piloting, a FLC was designed as a velocity controller, other FLC approaches
for adaptive intelligent control were discussed in [27].

16
 System of Eqs.(25) represents time position of the quadrotor in the normalized time that depends
on T , the total descent time. Using expert piloting data, we aimed to generate the T via a fuzzy
logic processing to emulate the human piloting grasp.
For that, the FLC was conceptualized to generate the total time T based on instantaneous speed
and distance to target. the classical approach to determine the fuzzy rules base was processed, the
antecedent and consequent partitions will be designed by human expert knowledge for quadrotor
landing.
Due to the complexity of time estimation, the principle will be based on deciding the average
possible speed based on the knowledge of quadrotor dynamics, the initial state, and relative pose
and velocity.

5.3.1. Fuzzy logic

In the velcity control approach we opted for FLC as it is more linguistic and to inspire the
expert human pilot methodology to land with enough robustness and softness.
For the FLC design, inputs are described by the instantaneous relative position, velocity, and
acceleration states of the quadrotor, outputs will be the average speed to target, from where we
extract the landing time T . this method is more advantageous and soft because of the use of expert
rules.
The velocity FLC is basically, consisting of:
A) Fuzzification: Both of the estimation from EKF and the relative estimated pose from the
camera are transformed to a memberships functions defined by the linguistic variables. This fuzzy
sets will be used by the inference mechanism to validate the next stage rules as demonstrated in
Fig.7.

Figure 7: FLC model for velocity control

b) Inference mechanism: it is the processing stage that evaluate in every execution cycle each
one of a set of ”if ... then ... statements” already stored in Rules base as mentioned in Fig.8, and
which are a combination of valid rules filled by Experienced agent.

17
 Figure 8: Fuzzy decision maker: Defuzzification and base of rules

c) Defuzzification: the Output stage gives the average speed that converts the combined appro-
priate rules from inference stage to a decision velocity of the quadrotor.
Calculated Time will be based on the average velocity and distance to the landing pad.
q
T = ∆x2p,q + ∆yp,q 2 + ∆z 2 /V + T
p,q min (37)

The block diagram of the fuzzy logic velocity controller is illustrated in Fig. 9.

Figure 9: velocity control & time to land by Fuzzy logic control scheme

18
5.4. Trajectory generation algorithm
Algorithm 1: Min Jerk Trajectory generation with Fuzzy velocity control
Result: Estimation of time for landing ’T’ and Cartesian coordinates
Initialization and load data file;
Import state data from EKF;
Import data from relative pose estimator ;
while ’Auto-landing is ON’ & ’ArUco marker detected’ do
if All Data are Available then
- Fuzzification of inputs to membership functions;
- Process Inference mechanism ;
- Defuzzification of outputs and decision making on Average velocity ’V’;
end
Calculate the estimated time for landing ’T’ from the average speed and instantaneous
distance Eq.37;
Calculate trajectory coefficients by Eq.30;
Calculate x(t), y(t), and z(t) based on Eq.25;
Send desired Trajectory coordinates to tracking control;
end
if ’Auto-landing is OFF’ OR ’ArUco Markers NOT detected’ then
- Hold quadrotor position;
- Send flag to control station ;
- Request new GPS position;
end

6. Control and Tracking of designed trajectory

MPC is an optimal control method used to satisfy a set of constraints. MPC is chosen based
of the quadrotor nature, the multiple manipulative variables with constraints to met, and the
perturbation which has been considered as a white Gaussian noise during the simulation,
The Jacobian of quadrotor dynamics is used to linearize the model, the Adaptive design will be
based on linearization at operating points[28].

6.1. Tracking Control strategy

The Model Predictive Control (MPC) is a finite-horizon optimization based on the dynamics
model. Fig. 10 shows the control strategy with MPC to track the generated min Jerk trajectory
with the fuzzy velocity control.
Considering the system as:
ẋi+1 = Ai xi + Bi ui + wi
(38)
y i = C i x i + vi
vi ,wi are discrete-time Gaussian white noises with the respective covariance matrices Q and R.
To ensure optimal tracking [29], a quadratic cost function ”J” is defined as:

N
X −1
J = 1/2(CN xN − ZN )T SN (CN xN − ZN ) + 1/2 (Ci xi − Zi )T Qi (Ci xi − Zi ) + uTi Ri ui


(39)
i=1

19
 Zi represent the Minimal Jerk trajectory along the horizon N .
The optimal Control can be demonstrated to be [29]:

ui = − (Ri + BiT Pi+1 Bi )−1 BiT Pi+1 Ai xi + (Ri + BiT Pi+1 Bi )−1 BiT gi+1
   
(40)
Pi and Gi are recursively calculated as follows:
  −1
 Pi = Vi + ATi Pi+1 I + Bi Ri−1 BiT Pi+1 Ai
  (41)
 g = W Z + AT I − P (I + B R−1 B T P )−1 B R−1 B T g
i i i i i+1 i i i i+1 i i i i+1

With Vi = CiT Qi Ci and Wi = CiT Qi ,

T

PN = CN SN CN
T (42)
gN = CN SN ZN

The control solution is calculated on the Horizon of N points to the landing on the pad, at every
iteration the control is calculated repeatedly and applied just for that iteration.
The whole MPC control based architecture is shown in Fig.10.

Figure 10: Complete Scheme of MPC control with Fuzzy min Jerk trajectory

20
 The Control Algorithm of MPC can be designed as:
Algorithm 2: MPC control Algorithm
Result: Control input Ui
Recall the last estimation of xi , or initial conditions;
Verify initialisation procedure;
while Control verification is Ok do
Consider the appropriate Ai , Bi and Ci from best estimate of EKF and sensor available
data;
Calculate Vi and Wi .;
N=100 (number of points can be adaptive );
Calculate PN and gN thru Eq.42 ;
for k=N:i do
Calculate Pk and gk with Eq.41 ;
end
Calculate ui by Pi and gi as in Eq.40 ;
Apply control ;
end

7. Results

In this Section, we conduct all the simulations and experiments necessary for the autonomous
landing project.

7.1. Vision processing

The designed landing pad was detected easily from different heights and from different angles
with heading orientation, the Fig.11 shows the detection and estimation from different positions.

21
 Figure 11: Detection and estimation from different position

Fig.11 shows different scenarios, Sub Figure (A), shows the designed landing pad, Sub Figure
(B) demonstrates the detection process and the pose estimation from a far pose, the filter uses only
larger markers even with the possible detection of closer ones, in Sub Figure (C), The filter considers
only medium size markers. in Sub Figure (D), the pose estimation is based only on smaller markers.
F, M and C denotation shows the filter choice of markers.

7.2. Sensor fusion and EKF of the quadrotor

The Data fusion filter combined with the EKF was based on the MPU 9250 and the GPS with
calibrated magnetometer. AHRS algorithm calculates the quaternion orientation. Fig.12 presents
the AHRS filter real time performance.

22
 Figure 12: Static Results of AHRS

A Matlab simulation of the EKF observer of linear position is showed in Fig.13, the Observer
is able to reject noise and uncertainty of sensor. The EKF is crucial to ensure the loop stability
during tracking.

(a) EKF observer (b) EKF observer ”Zoom in”

Figure 13: EKF observer for general position of the quadrotor]

7.3. Control simulation results

7.3.1. Matlab simulation
The simulation of MPC showed an excellent stabilization and very good tracking of desired
trajectory even in presence of simulated gust wind and model uncertainty with Gaussian noise as
demonstrated in Fig.14, a comparison with PID control showed that the MPC is more performant
and more robust.
The green trajectory, shows the landing pad position, where the blue one is the minimum jerk
desired trajectory updated by iteration. Red trajectory is the MPC simulated position of quadrotor
compared to the PID one in magenta. PID control shows more deviation and less robustness to the
added noise and wind perturbation.

23
 (a) Panoramic Tracking with Obstacles avoidance (b) Top view of tracking

(c) 3D tracking

Figure 14: 3D Minimum Jerk trajectory tracking of a quadrotor performance controlled by PID vs MPC

8. Conclusion

This paper comes up with a complete robust application of quadrotor’s autonomous landing on
a moving platform that is detected and localized by an on-board camera. An optimal minimal Jerk
trajectory tracking by MPC controller was successfully opted subject to parametric uncertainties
and external disturbance. The relative pose to the landing pad was based on the detection of the
special designed pad by the combined ArUco markers for best detection at different altitudes and
angles, then the identification of each marker and estimation of the pose was performed via Open
CV / python algorithm enhanced by stabilization filters. The Madgwick / EKF observer of the
quadrotor was enhanced by an asynchronous sensors fusion (MPU9250 and NEO-6M) to observe
the quadrotor states vector and sensors bias. Relative pose estimation was satisfactory, Trajectory
generation was adaptive based on both observers results with a fuzzy velocity control to estimate
the landing time to bio-inspire an experienced human pilot, then its fed to the MPC for tracking.
Comparison with PID proved the robustness and performance of the MPC scheme.

24
References

[1] Z.-C. Xu, B.-B. Hu, B. Liu, X. Wang, H.-T. Zhang, Vision-based autonomous landing of
unmanned aerial vehicle on a motional unmanned surface vessel, in: 2020 39th Chinese Control
Conference (CCC), IEEE, 2020, pp. 6845–6850.
[2] L. Danjun, Z. Yan, S. Zongying, L. Geng, Autonomous landing of quadrotor based on ground
effect modelling, in: 2015 34th Chinese Control Conference (CCC), IEEE, 2015, pp. 5647–5652.
[3] B. Hu, S. Mishra, A time-optimal trajectory generation algorithm for quadrotor landing onto a
moving platform, in: 2017 American Control Conference (ACC), IEEE, 2017, pp. 4183–4188.
[4] Y. Qi, J. Jiang, J. Wu, J. Wang, C. Wang, J. Shan, Autonomous landing solution of low-cost
quadrotor on a moving platform, Robotics and Autonomous Systems 119 (2019) 64–76.
[5] J. R. Hervas, M. Reyhanoglu, H. Tang, Automatic landing control of unmanned aerial vehicles
on moving platforms, in: 2014 IEEE 23rd international symposium on industrial electronics
(ISIE), IEEE, 2014, pp. 69–74.
[6] B. Hu, L. Lu, S. Mishra, Fast, safe and precise landing of a quadrotor on an oscillating platform,
in: 2015 American Control Conference (ACC), IEEE, 2015, pp. 3836–3841.
[7] C. Aguilar-Ibanez, M. S. Suarez-Castanon, O. Gutierrez-Frias, J. d. J. Rubio, J. A. Meda-
Campana, A robust control strategy for landing an unmanned aerial vehicle on a vertically
moving platform, Complexity 2020 (2020).
[8] G. Schweighofer, A. Pinz, Robust pose estimation from a planar target, IEEE transactions on
pattern analysis and machine intelligence 28 (12) (2006) 2024–2030.
[9] B. Li, J. Wu, X. Tan, B. Wang, Aruco marker detection under occlusion using convolutional
neural network, in: 2020 5th International Conference on Automation, Control and Robotics
Engineering (CACRE), IEEE, 2020, pp. 706–711.
[10] S. Wilson, H. Eberle, Y. Hayashi, S. O. Madgwick, A. McGregor, X. Jing, R. Vaidyanathan,
Formulation of a new gradient descent marg orientation algorithm: Case study on robot tele-
operation, Mechanical Systems and Signal Processing 130 (2019) 183–200.
[11] A. Gautam, P. Sujit, S. Saripalli, Autonomous quadrotor landing using vision and pursuit
guidance, IFAC-PapersOnLine 50 (1) (2017) 10501–10506.
[12] H. Das, K. Sridhar, R. Padhi, Bio-inspired landing of quadrotor using improved state estima-
tion, IFAC-PapersOnLine 51 (1) (2018) 462–467.
[13] T. Lee, M. Leok, N. H. McClamroch, Nonlinear robust tracking control of a quadrotor uav on
se (3), Asian Journal of Control 15 (2) (2013) 391–408.
[14] S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, M. J. Marı́n-Jiménez, Automatic
generation and detection of highly reliable fiducial markers under occlusion, Pattern Recogni-
tion 47 (6) (2014) 2280–2292.
[15] S. Malik, G. Roth, C. McDonald, Robust corner tracking for real-time augmented reality,
configurations 1 (2) (2002) 12.

25
[16] O. Bouaiss, R. Mechgoug, R. Ajgou, Modeling, control and simulation of quadrotor uav, in:
020 1st International Conference on Communications, Control Systems and Signal Processing
(CCSSP), IEEE, 2020, pp. 340–345.
[17] S. Madgwick, An efficient orientation filter for inertial and inertial/magnetic sensor arrays,
Report x-io and University of Bristol (UK) 25 (2010) 113–118.

[18] J.-J. Xiong, E.-H. Zheng, Optimal kalman filter for state estimation of a quadrotor uav, Optik
126 (21) (2015) 2862–2868.
[19] L. Wang, Z. Zhang, P. Sun, Quaternion-based kalman filter for ahrs using an adaptive-step
gradient descent algorithm, International Journal of Advanced Robotic Systems 12 (9) (2015)
131.
[20] U. AG, Neo-6: U-blox 6 gps modules data sheet (2018).
[21] E. A. Morelli, Estimating noise characteristics from flight test data using optimal fourier
smoothing, Journal of Aircraft 32 (4) (1995) 689–695.

[22] A. Kaba, E. Kıyak, Optimizing a kalman filter with an evolutionary algorithm for nonlinear
quadrotor attitude dynamics, Journal of Computational Science 39 (2020) 101051.
[23] A. Nayak, R. N. Banavar, D. Maithripala, Stabilizing a spherical pendulum on a quadrotor,
Asian Journal of Control (2020).
[24] D. Mellinger, V. Kumar, Minimum snap trajectory generation and control for quadrotors, in:
2011 IEEE international conference on robotics and automation, IEEE, 2011, pp. 2520–2525.
[25] K. Vijay, Unmaned aerial vehicles (2017).
URL https://www.youtube.com/watch?v=nLlLEw_hiNc[Accessed25December2020]
[26] M. J. Richardson, T. Flash, Comparing smooth arm movements with the two-thirds power law
and the related segmented-control hypothesis, Journal of neuroscience 22 (18) (2002) 8201–
8211.
[27] H. Mo, G. Farid, Nonlinear and adaptive intelligent control techniques for quadrotor uav–a
survey, Asian Journal of Control 21 (2) (2019) 989–1008.
[28] Wiki, Linear–quadratic–gaussian control (2021).
URL https://en.wikipedia.org/wiki/Linear\OT1\textendashquadratic\OT1\
textendashGaussian_control[Accessed25December2020]
[29] M. Bohner, N. Wintz, The linear quadratic tracker on time scales, International Journal of
Dynamical Systems and Differential Equations 3 (4) (2011) 423–447.

26