Next Article in Journal
Reducing Slip Risk: A Feasibility Study of Gait Training with Semi-Real-Time Feedback of Foot–Floor Contact Angle
Next Article in Special Issue
Applications of Fusion Techniques in E-Commerce Environments: A Literature Review
Previous Article in Journal
Shaking Table Attached to Magnetorheological Damper: Simulation and Experiments for Structural Engineering
Previous Article in Special Issue
Modeling and Fault Detection of Brushless Direct Current Motor by Deep Learning Sensor Data Fusion
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

Error Reduction in Vision-Based Multirotor Landing System

Applied Artificial Intelligence Group (GIAA), Carlos III University of Madrid, 28270 Madrid, Spain
*
Authors to whom correspondence should be addressed.
Sensors 2022, 22(10), 3625; https://doi.org/10.3390/s22103625
Submission received: 30 March 2022 / Revised: 27 April 2022 / Accepted: 6 May 2022 / Published: 10 May 2022
(This article belongs to the Special Issue Data Fusion and Machine Learning in Sensor Networks)

Abstract

:
New applications are continuously appearing with drones as protagonists, but all of them share an essential critical maneuver—landing. New application requirements have led the study of novel landing strategies, in which vision systems have played and continue to play a key role. Generally, the new applications use the control and navigation systems embedded in the aircraft. However, the internal dynamics of these systems, initially focused on other tasks such as the smoothing trajectories between different waypoints, can trigger undesired behaviors. In this paper, we propose a landing system based on monocular vision and navigation information to estimate the helipad global position. In addition, the global estimation system includes a position error correction module by cylinder space transformation and a filtering system with a sliding window. To conclude, the landing system is evaluated with three quality metrics, showing how the proposed correction system together with stationary filtering improves the raw landing system.

1. Introduction

The growing demand for drone applications motivates the study of the support technologies for this type of small and powerful unmanned aerial vehicle (UAV). However, all new applications share an essential and critical maneuver—landing.
Generally, work in the literature about landing maneuvers, both for fixed and rotary wing UAVs, focuses on control strategies [1,2,3]. All of them require access to the internal vehicle states, the actuators or specific modes of the control or navigation system.
Under the precision landing concept is included all the solutions that approach this maneuver in an autonomous or supervised way, independent of the techniques and sensors used to estimate the vehicle states such as position or orientation, as well as its corresponding velocities and accelerations. The landing maneuver can be included within the navigation system where two main groups can be distinguished, outdoor and indoor navigation. Generally, outdoor navigation is based on the Global Navigation Satellite System (GNSS), but practically all current systems use fusion techniques that allow the integration of different strategies for estimating one or more vehicles’ states, which is necessary for the control and/or navigation system. Some of the most common cases in navigation systems are the use of barometers and/or sonars to improve the accuracy of the altitude provided by GNSS, or the use of small zenith cameras to determine small horizontal displacements [4]. Other specific navigation techniques such as visual odometry or visual Simultaneous Location And Mapping (SLAM) [5] are beginning to be used in indoor navigation.
Thus, other landing works focus on improving the accuracy of instrument systems such as [6,7,8] or even context information such as safe landing zones, as in work by Shah Alam, Md et al. shown in work [9]. Some commercial solutions focus on the use of beacons that indicate the landing region, as can be seen in the work of J. Janousek and P. Marcon [7] using a commercial infrared light beacon. This type of system includes an external controller to minimize the pixel error between the region of interest (ROI), defined by the centroid of the infrared area and the reference in the image plane, generally located at the center of the image plane. These strategies need to access internal vehicle states such as velocity or acceleration to correct the error.
In terms of context information, vision systems proved to be efficient to identify ROIs. In addition, knowing the landing context, specifically where or how the helipad is where the UAV must land, can help to improve the landing maneuver.
Developing strategies to identify and understand the context information of the aircraft allows providing the systems with greater autonomy. In the survey of autonomous landing techniques for UAVs by Alvika Gautam et al. [1], the authors describe the relationship between sensors/navigation systems and aircraft control modules, paying particular attention to vision landing techniques, generally responsible for recognizing and estimate the helipad position.
Some civil and commercial UAVs, such as certain DJI models [10], are beginning to integrate vision-based precision landing systems. In the work of Yoakum and Carreta [11], the authors conduct a study of the precision landing system of a DJI Mavic Pro, proving the aircraft and the integrated landing system meet specific accuracy requirements to use a specific wireless charging station.
Generally, vision systems for landings focus on identifying the landing area, either by means of context information of the helipad pattern or by the terrain conditions. The work of Mittal et al. [12] is an example of the identification of landing area conditions, where terrain slope is estimated to verify the feasibility of a UAV to land in urban search and rescue.
Regarding pattern recognition landing systems, works such as [13,14], among others [15,16,17,18], focus on finding the position using known patterns by Perspective-n-Point (PnP) algorithms [19]. Patterns such as Aruco [20], charuco, or new fractal patterns such as [21] or the deep learning trend You Only Look Once (YOLO) [22] try to improve the pattern pose estimation and prove to be widespread systems in the literature. In the literature and throughout this paper, an object “pose” means a set of position and heading of a specific reference system.
On the other hand, the emergence of open-source flight controllers such as Pixhawk [23], together with specific communication protocols such as Micro Air Vehicle Link (MAVLink) [24] and multiplatform APIs such as MavSDK [25], help to develop new applications and research new landing strategies.
PX4 [26] autopilot set up as a rotary wing vehicle has a planification system that allows dynamically smoothing the trajectories between different positions [27]. Specifically, it smooths the trajectories between consecutive waypoints by rounding the turns with radii over the waypoints and increasing or decreasing the drone speed when approaching or moving away from a waypoint [28,29,30]. The guidance algorithm integrated in PX4 is the L 1 algorithm introduced by Park et al. [31] under the linear approach. When forcing a new target position while the vehicle is navigating between two positions, the system changes target and tends to reach the newly added location by smoothing its current trajectory, as shown in works such as Stateczny et al. [32]. This behavior, when repeated with a certain frequency to include new waypoints referring to the same position, but with a certain noise, produces a spin effect on the aircraft that we call “inter-waypoint noise spin effect”.
In this work, we propose using the aircraft guidance system without downing the controller level for a widespread implementation of the precision landing system, contributing to UAV air safety and helping the emergence of new applications.
We propose a new contribution with respect to classical geolocation landing strategies based on global positioning by decoupling the landing in two phases, first reaching the target coordinates and then activating the landing mode, descending vertically with a constant descent speed α.
Our proposed landing strategy seeks to descend quickly when the target is found and to smooth its descent as the aircraft approaches, without having to adjust the controller parameters. This idea attempts to improve the image resolution quickly to improve the position estimation. The difference with respect to other works is that simultaneously descending and adjusting the positioning relies on the variable waypoint altitude adjustment without accessing the controller, so that without changing the internal descent controller of the PX4 [26] or adding new external control laws, the strategy allows smoothing the descent when approaching the target. This strategy allows taking further steps in the final phase of the approximation, improving the final estimate, and ensuring the stability of the system provided by the manufacturer. For this, we propose a function to modulate the default behavior of the PX4 controller which seeks a stationary descent at a constant speed.
In addition, this paper models the error of a precision landing system using a monocular vision system, context information from the helipad pattern, and the internal navigation system of the PX4 flight controller.
The vision system error modeling allows a fine calibration of helipad localization. This correction, together with a stationary filtering of the estimates by sliding time window and variable adjustment of the descent height, allow to reduce the spin effect produced by the estimation error of a vision system when integrated with the L 1 navigation algorithm, without access to the internal controller parameters or relative states of the aircraft that may require re-programming of the on-board computer.
To sum up, this paper presents two main contributions in precision landing by vision-based global position: the continuous adjustment of the approach to descent trajectory, and the improvement of the position by vision through systematic error adjustment and filtering.
The proposal strategy is evaluated after including an error model correction as well as different sliding time window filters. The work is developed on a hyper-realistic software in the loop (SITL) [33] simulation system with the PX4 flight controller and the AirSim [34] simulator.
Finally, the results of the study show the estimation error analysis and filtering of the estimates with sliding time window filters, minimizing the inter-waypoint noise spin effect generated by noisy waypoint transitions and improving three quality metrics of landing time, trajectory landing length, and landing accuracy without additional control law, enabling the use of the aircraft’s guidance system as an alternative for the deployment of precision landing technology.
This paper is organized as follows: Section 2 shows the problem formulation of helipad spatial estimation by monocular computer vision. Section 3 describes the landing strategy proposal and the global estimation module. The correction module design, the analysis of the complete landing system, and a description of the test environment can be found in Section 4. Finally, the conclusions are presented in Section 5.

2. Problem Formulation

We consider the problem of a UAV landing on a certain landing pad using its internal autopilot waypoint guidance system and a monocular vision system with gimbal integrated in the UAV.
Figure 1 shows the set of reference frames, where the superscripts { t , p h , c , z , g , b , n , e   and   g } correspond to the reference frames of the helipad (target), pinhole camera model, camera, gimbal socket for the camera, gimbal, body, North-East-Down (NED), Earth-Centered Earth-Fixed (ECEF), and global.
In this way, any given point p t t   expressed in a flat pattern reference frame { t } and the homogeneous transformation n T t between the landing pad reference frame to the NED referent frame { n } can be expressed in the global reference frame { g } system p t g applying a set of transformations shown in Equations (1) and (2).
p t n = n T b b T g g T z z T c c T t n T t p t t
p t g = g F e { e F n [ p t n , e F g ( p R e f g ) ] }
where superscript { j } over point p i j means the reference frame system, and the subscript i = { t , R e f } denotes the name of the point (target and reference). j T i means the homogeneous transformation between reference frame i   and j . On the other hand, j F i refers to nonlinear transformations between reference frame i   and j . p R e f g   indicates the global position of the body (UAV) as a global reference point.
The set of reference frame systems involved in the transformations are shown in Figure 1 and denoted as: helipad (target) { t } , pinhole camera model { p h } , camera { c } , gimbal socket for the camera { z } , gimbal { g } , body { b } , North-East-Down (NED) { n } , Earth-Centered Earth-Fixed (ECEF) { e } , and global { g } .

2.1. Pattern (Helipad) Detection

We consider as the helipad a reference pattern defined by an Aruco pattern [35] with a certain number of bits, as part of a library B . As shown in the paper [20], the system identifies candidate square regions as Aruco markers, then encodes these regions and compares them to the pattern dictionary as desired.
The full process can be divided into the following steps:
  • Image conversion: Obtain an RGB image and transform it to grayscale.
  • Edge extraction: We understand as edge an intensity change boundary, some classical algorithms are Canny [36] and Sobel [37].
  • Contour extraction: We understand a contour as a curve of points without gaps or jumps. Therefore, the objective is to identify if the edges found represent contours. An example of simple contour extraction can be given by a binarized image of an object whose outer contour can be extracted by subtracting the original binarized-dilated image from the original binarized image. To check if closed regions appear, a segmentation by connected components would provide us candidate regions of interest (ROI) as a result.
  • Contour filtering: Only show rectangular regions.
  • Removing ROI perspective distortion: For this it is necessary to find the general plane P 2 projective transformation h : P 2 P 2 |   h ( m ) = m = m H , where m is a point in a plane. H 3 × 3   is a non-singular matrix where m is the linear transformation H of m . The transformation H is biunivocal and homogeneous, in other words, a point over a plane is a unique point over another plane and k H |   k and k 0 is also the solution. This condition allows dividing the matrix H by the element h 33 , decreasing the dimension of terms to identify from 9 to 8. The correspondence between points ( x i , y i ) ( x i , y i ) can be expressed in matrix form as b i = A i h , and their relationship is expressed as Equation (3) (more details in [38]). Knowing n pairs of points, the system of 2 n equations and 8 unknowns is established as b = A h , where A = [ A 1 , A 2 , , A n ] T ,   b = [ b 1 , b 2 , , b n ] T ,   and h 3 × 3 matrix as h 33 = 1 . For n = 4 , the direct solution h = A 1 b ; if n > 4 the system is overdetermined and least squares can be applied, h = [ A T A ] 1 A T b . For cases where h 33 = 0 refer to [38].
    x = h 11 x + h 12 y + h 13 h 31 x + h 32 y + h 33 y = h 21 x + h 22 y + h 23 h 31 x + h 32 y + h 33 A = [ x y 1 0 0 0 x x x y 0 0 0 x y 1 y x y y ] ; b = [ x y ] ; h = [ h 11 , h 12 , h 13 , h 21 , h 32 ] T
  • Pattern library matching check: The binary code of the ROI is extracted, superimposing on the binarized and perspective-corrected image a grid of the same cell size as the searched one. Each grid cell receives a binary value according to if the corresponding color is black (zero) or white (one). The Hamming coding algorithm (ref) is applied to the extracted code to eliminate false negatives. This resulting code is compared with the selected pattern dictionary, filtering the regions identified as markers and belonging to the pattern dictionary from other regions. In addition, this step provides information about the marker id if the ROI belongs to the library.

2.2. Helipad Pose Estimation

For pose estimation, the Perspective-n-Point (PnP) problem [35] is formulated where the objective is to minimize the reprojection error Equation (7) of 3D points in the image plane { p h } . This problem is closely linked to a calibrated system, since it requires a camera model, pinhole, and a pattern that allows to relate identified features of an image with features of the pattern.
Given a point p t c   3 belonging to the knowing pattern located in real-world 3D space and expressed in the camera reference frame { c } , it can be expressed in the image camera plane reference frame { p h } as p t p h 2 . The relationship between the two reference frames is provided by the pinhole camera model in Equation (4).
s p t p h = A p t c
where s is a scale factor and A intrinsic camera matrix [17]. The internal matrix A is composed of the focal distances ( f x , f y ) and the principal points ( c x , c y ) . The pinhole model can be improved with radial, tangential, or prism distortion corrections, adding n set of k i parameters to the model [39,40,41]. The set of internal parameters of the camera model can be expressed by the vector δ = ( f x , f y , c x , c y ,   k 1 ,   ,   k n ) .
If the point is expressed in coordinates of the pattern reference frame p t t , there exists an extrinsic homogenous transformation c T t to relate the reference frame of the pattern to the camera reference frame Equation (5) is used.
p t c = c T t p t t
where the transformation c T t = [ c R t | v t c ] is a rototranslation composed of the pattern’s orientation c R t = R x ( θ 1 ) R y ( θ 2 ) R z ( θ 3 ) to the camera and the pattern position vector to the camera v t c 3 . Thus, the parameter vector to be identified to obtain the camera–pattern relationship is θ = ( R , v ) = ( θ 1 , θ 2 ,   , θ 6 ) 6 .
Joining Equations (4) and (5) and adding distortion models, the camera model remains as a Function (6) that projects points p t t 3 to p t p h 2 points of the camera image plane.
p t p h = Ψ ( δ , θ , p t t )
Then, helipad pose estimation is the problem of minimizing the reprojection error Equation (7) of the observed helipad pattern features. One of the classic features to identify by computer vision are the corners. If the pattern is known, we know a priori the 3D position of these corners in the reference frame of the pattern.
E ^ = a r g   m i n p i t [ Ψ ( δ , θ , p i t ) O ( p i t ) ] 2  
where p i t and is a corner set of the pattern. O ( p i t ) 2 is the corners obtained in the camera plane by a specific computer vision algorithm such as the Harris or Susan algorithm [22,42].
Furthermore, since all points p i t belong to a pattern plane, the z -component of all corners in the pattern frame will always be 0. This quality allows solving Equation (7) using specific methods such as the Infinitesimal Plane-Based Pose Estimation (IPPE) [43].
The estimation of internal camera parameters requires a learning phase modeled in Equation (7) as an optimization problem. In addition, identifying the six parameters to define the transformation c T t between the pattern calibration and the camera involves a similar process. Although both processes can be clustered as shown in Equation (7), the internal camera parameters δ will be constant for a particular vision system; however, the position of the pattern may change. For this reason, it is decoupled in two phases: on the one hand, a camera parameter learning (calibration) process, using a set of images of a known pattern to estimate internal camera parameters, and, on the other hand, the estimation of the helipad position for a certain image during flight.

2.3. Camera-Gimbal Frame

The camera is placed in a camera-gimbal socket, so it is necessary to include this referent frame { z } . As the camera-gimbal socket axis is equivalent with the general gimbal axes, but static, the z T c transformation is shown in Equation (8).
z T c = [ R c z | 0 3 × 1 ] ( R c z 0 3 × 1 0 1 × 3 1 ) ; R c z = R ( x , π 2 ) R ( z , π 2 )
where R ( x , θ ) and R ( z , ψ ) represent θ and ψ rotations about the x and z axes of the camera reference system. As the axes of the gimbal and camera-gimbal socket are equivalent, the relationship between camera-gimbal socket and gimbal corresponds to the identity matrix g T z = I 4 × 4 .

2.4. Gimbal Body Frame

The gimbal’s reference frame { g } to the UAV’s body gravity center frame is defined as the composition of a roto-translation in Equation ( 9 ) .
b T g = [ R g b | p g b ] R g b = R g ( θ ,   ϕ , ψ ) ; p g b = ( x g , y g , z g ) T
For this work, we consider the gimbal is static but located at the p g b position with R g ( θ ,   ϕ , ψ ) rotation to the body axes.
In this paper we consider b T z = [ R b ( 0 , π 2 , 0 ) | ( 0 , 0 , 0.1 ) T ] , as shown in Section 4.1.

2.5. Body-NED Frame

The North-East-Down (NED) frame coordinates { n } to the UAV body gravity center { b } , n T b is equivalent to the body rotation at the angle defined by the yaw angle ψ   to geographic north or azimuth, pitch attitude to horizon plane ϕ , and roll angle defined to gravity direction θ . These angles refer to the attitude and heading reference system (AHRS) frame of reference that groups magnetic, angular rate, and gravity (MARG) information. Generally, these systems usually include air data to provide altitude or wind speed information.
n T b = [ R b n | 0 3 × 1 ] ; R b n = ( cos θ cos ψ sin ψ sin θ sin ϕ sin ψ cos ϕ sin ψ sin θ cos ϕ + cos ψ sin ϕ cos θ sin ψ cos ψ cos θ + sin ψ sin θ sin ϕ sin ψ sin θ cos ϕ cos ψ sin ϕ sin θ cos θ sin ϕ cos θ cos ϕ   )

2.6. NED-ECEF-Global

The coordinate transformation between NED to the global reference frame { g } requires the use of the Earth-Centered Earth-Fixed (ECEF) reference system { e } , which allows us to apply the corresponding geodetic transformations to the terrestrial model and finally obtain the coordinates in global terms. In our case, we use a WGS84 (World Geodetic System 84) [44] datum.
The constant parameters of the WGS84 datum in Figure 2 refer to: r e semimajor axis (equatorial radius), r p semiminor axis (polar axis radius), ε first eccentricity and ε second eccentricity of the ellipsoid. It is important to differentiate the geocentric coordinates, referred to as the ECEF system, from the geodetic coordinates, referred to as the geodetic model (WGS84). This difference is provided by the geodetic model (datum) and is represented in the diagram on the right of Figure 2, where φ refers to geocentric latitude and φ refers to geodetic latitude.
Given a point p t n expressed in NED reference frame { n } of a local tangent plane (LTP) to a geodesic surface at a known point p R e f g = ( λ , φ , h ) T R e f , it can be expressed in ECEF coordinates { e } applying Equation (12). This equation corresponds to a translation in ECEF reference frame. However, to obtain p R e f e coordinates of our reference point in ECEF frame it is necessary to transform the global coordinates to ECEF applying Equation (14). The transformation between local coordinates and ECEF is given by the transformation Equation (13). In this work, we consider p R e f g = p U A V g .
On the other hand, a given point p t e expressed in ECEF can be expressed in global coordinates p t g applying the transformation Equation (11).
p t g = ( λ φ h ) t = g F e ( p t e ) = ( t a n 1   ( y t e x t e ) t a n 1 ( z t e + e 2 Z 0 r ) U ( 1 r p 2 r e V ) )
where ( λ , φ , h ) T means longitude, latitude, and altitude in WGS84 datum. ( x t e , y t e , z t e ) T are the coordinates in the ECEF reference frame. The transformation Equation (11) corresponds to Jijie Zhu’s algorithm [45] analyzed and compared in [46].
p t e = ( x t e y t e z t e ) = e F n ( p t n , p R e f e ) = R n e p t n + p R e f e
R n e = ( s i n φ R e f c o s λ R e f s i n λ R e f c o s φ R e f c o s λ R e f s i n φ R e f s i n λ R e f c o s λ R e f c o s φ R e f c o s λ R e f c o s φ R e f 0 s i n φ R e f )
p R e f e = ( x R e f e y R e f e z R e f e ) = ( ( r λ + h R e f   ) c o s φ R e f c o s λ R e f ( r λ + h R e f ) c o s φ R e f s i n λ R e f ( ( 1 ε 2 ) r λ + h R e f ) s i n φ R e f )  
r λ = r e 1 ε 2 sin 2 φ  

3. Proposal

In this section, first the landing strategy is described, then the method to determine the helipad’s global position is detailed, and finally the error analysis of the helipad’s position estimation is given.

3.1. Landing Strategy

The landing strategy is responsible for telling the UAV navigation system the position to which it must go and the attitude it must have to align with the target (Algorithm 1). The position of the target is static, but the attitude and altitude to helipad vary overtime when the UAV attempts to land.
Algorithm 1 Landing Strategy
1: [ p t g , ( θ , ϕ , ψ ) t g , a ] = helipad   identification h U A v , ψ U A V = UAV   navigation   sistem  
2: i f   a = True
3:Buffer [ p ,   ( θ , ϕ , ψ ) ] t g
4: i f   f r e q u e n c y = 1 H z   &   B u f f e r 10  
5: p t g ,   ψ t g = F i l t e r   ( B u f f e r )
6:Buffer reset
7:Buffer (1) [ p ,     ψ ] t g
8: ψ s e t = ψ U A V n + ψ t b
9: [ λ , φ ] s e t p t g
10: h s e t = h U A V ( 1 0.1 e 1 h t b 0.5 h 0 )
11:UAV navigation planer   [ λ s e t ,   φ s e t , h s e t ,   ψ s e t ]
12: i f   h U A V h 0
13:PX4 landing mode
14:break
15:end if
16:else
17: g o t o 1
18:end if
19:else
20: g o t o 1
21:end if

3.1.1. Helipad Azimuth

To align the drone to the marker, it is necessary to determine the azimuth of the marker ψ t n . For this, we use the azimuth of the drone ψ U A V n   and the orientation of the marker to the drone ψ t b .
Figure 3 shows how the helipad azimuth can be obtained graphically by adding to the drone azimuth the orientation of the aircraft to the landing pad in Equation (16). When both systems are aligned, the marker azimuth will be equal to the drone azimuth.
ψ t n = ψ U A V n + ψ t b

3.1.2. Altitude Setpoint Strategy

In order to change the default controller descent behavior, it is possible to use the behavior of the system in the transient state, i.e., before reaching the maximum velocity of stationary descent. Thus, if the new desired height is reached without having to reach the maximum descent speed, the behavior will be smooth, and if the destination point is far enough away, the controller saturates and descends with maximum constant speed behavior, without exceeding the internal controller parameters.
To define step points that allow a linear descent at constant speed α in an iterative loop, the new step point will correspond to the current height minus a certain parameter α.
h s e t = h U A V α
Considering this process is iterative (discrete) with a sample time of t , the previous equation can be expressed as follows:
h t + 1 = h t α t
where t   subscript means instant time. Solving the α term, it is verified that alpha corresponds to a speed term.
( h k + 1 h k ) t = α = h t = c t e .  
In our case, the aim is to design the h s e t ( h U A V ) function such that the aircraft approaches with a smooth behavior to h 0   and at that point lands automatically with internal autopilot.
To perform this, we propose Function (23).
h s e t = h U A V ( 1 β 1 e 1 h t b β 0 h 0 )
where   β 1 is the weight of the exponential function and β 0 < 1 , which allows slightly shifting the value of h 0   and to be able to switch to automatic landing mode. The β 1 = 0.1 value is set heuristically, while β 0 = 0.5 is set to shift 50% less than the switching height h 0 . The system must consider the relative flight altitude h U A V and the height of the UAV relative to the landing pad h t b .
Figure 4 shows an approximate representation of the altitude-set function behavior Equation (20) vs. constant decreasing Equation (17).
Figure 4 shows the approximate descent behavior of our proposal versus a constant speed descent. In the final phase of the approximation, the descent becomes smoother than in the linear behavior. The Figure 4 behavior should be taken as an illustrative example of the desired behavior, not as a realistic simulation. The final behavior can be seen in experimentation.

3.1.3. Filter

The states to be filtered are the global position of the helipad p t g = ( λ t ,   φ t ) and its orientation to north or azimuth ψ t n . All these variables are static, since the landing pad is static; therefore, the filter model does not need to provide information for each new measurement, rather we need to know their stationary statistical values. For this we propose to generate a data buffer with memory. The size of the buffer defines the size of the filtering window L . The initialization saves L new measurements and then finds the mean or median of the buffered data. Finally, the buffer is reset.
To propagate the information over time, the sliding window does not overlap with previous values, but the value filtered at the previous instant is included as the first measurement in the clean buffer.
As for the filter memory, if the new values change substantially it will vanish in the long term, since the weight given to the past values is 1 L versus L 1 L for each new data, so the window size can be critical for cases where the target is moving. Finally, the size of the buffer/window L is linked to time thanks to the 1   Hz system sampling time to provide new measurements.

3.2. Helipad Global Position Estimation

The helipad global position estimation system is responsible for integrating the vision system, the heliport context information, the gimbal, and the UAV navigation states, to provide the landing strategy with the helipad global position. In addition, this includes a spatial correction system for the NED frame, which is the objective of study of this work.
Figure 5 shows the diagram of the estimation system which is formulated in Equation (2). The system works as follows:
  • Aircraft global position p U A V g and attitude ( θ , ϕ , ψ ) U A V b is requested by the PX4 flight controller via MAVLink protocol [24] supported by the MAVSDK API [25].
  • The gimbal position p g b ( x , y , z )   and attitude ( θ , ϕ , ψ ) g b is requested by the AirSim simulation environment via UDP protocol described in Section 4.1. This information composes the b T z transform.
  • The vision system receives I image of W × H size and 3 RGB channels. The image is received via UDP protocol from the simulation system. In addition, the vision system has as input the context information from the helipad, the library ( L i b ) of the marker, the marker’s identification number ( I d L i b ), and the real marker’s size ( M S ) in meters. The library is characterized by the number of horizontal and vertical bits (squares) that form the geometry of the marker and the number of elements that make up the library. The vision system output provides a Boolean variable a B , that indicates if the landing pad has been detected or not. In addition, it provides the position of the landing pad to the camera p t c and the attitude ( θ , ϕ , ψ ) t c .
  • The camera pose estimation ( p c t and t T c ) is gated by the PnP method integrated in the OpenCV Aruco library [47] from a previously pre-calibrated camera (Test environment).
  • The aircraft, gimbal, and landing pad position are combined in the set of n T b , b T z , and z T c transformations to obtain the positioning p   t n and attitude ( θ , ϕ , ψ ) t n of the landing pad in NED frame.
  • The correction module provides the p t n positioning and attitude ( θ , ϕ , ψ ) t   n , tuned in NED coordinates.
  • Finally, the target position in NED frame p t n , together with the drone global position p U A V g and ellipsoid WGS84 approximation, are used to obtain the helipad global position in Equation (11).

4. Landing System Analysis

The aim of this section is to evaluate the proposed estimation system and to identify the necessary corrections to be incorporated in the “correction” module of Figure 5. To achieve this, first the test environment and the necessary parameters are detailed in the subsection Test environment. Next, the system estimation error is modeled to provide the landing system a correction module. The quality of the correction is evaluated using the root mean square error (RMSE) together with the variation in the data distribution in terms of data distribution structure, mean, and standard deviation.
Finally, a full landing system and classical linear decreasing descent are compared and evaluated with four quality metrics, which quantify the trajectory length, the time to land, and the accuracy of landing on the helipad.

4.1. Test Environment

In this work, we use a hyper-realistic test environment based on Software in The Loop (SITL). SITL systems are simulation architectures where virtual world environments interact to simulate object, vehicle, and sensor together with external systems such as a flight controller or ground station, among others. These environments are powerful testing tools for earlier phases of system integration, as they allow realistic results to be obtained without potentially dangerous and expensive risks.
In our case, AirSim [48] is used as world environment and PX4 flight controller configured as a quadcopter. The simulated physical model corresponds to the Iris quadcopter and the set of sensors, and their specifications are detailed in Table 1. The models of the simulated sensors can be found detailed in [34].
Figure 6 shows an SITL communication diagram between the main system modules in SITL. The GCS module refers to the ground control station, in our case QGround control [50]. GCS is used to help to download the .log files generated in the test missions.
The vision-based estimation system requires knowledge of the internal camera parameters { A , k i } . These parameters are obtained by standard calibration [39] using a chess pattern with nine rows, six columns and 20 cm sides of the squares. This pattern is integrated into the AirSim environment as a texture over a rectangular prism with 1.8 × 1.2 × 1.8   [ m ] sides. To capture images, we implemented a system that automatically captures images while performing a spiral upward flight over the reference pattern. This allows obtaining a large set of images with the pattern from different positions.
Figure 7 shows the image of the calibration pattern in the AirSim reference frame, random image of the image registration process used for calibration, and a sample of the reprojection error.
Finally, the internal camera parameters are shown in Table 2. The context information used for the experimentation is: L i b = 5 × 5 × 1000 ,   I d = 68 , and M S = 1   [ m ] .
The landing system was developed in Python 3.6 with the AirSim [34] and MAVSDK [25] APIs. The experiments and the SITL environment were developed on a Windows Server 2019, 64 bits, hosted in AMD Ryzen 9 3900X 12-Core Processor CPU, 3.79 GHz with 64 GB RAM and 2×1TB SSD + 2×HDD 1.5TB of internal memory, graphic card Nvidia GeForce RTX 2060.

4.2. NED Error Modeling

To evaluate the estimation error, we propose to analyze the estimation data provided by the vision system over twenty static flights located at seventeen different positions at the same relative altitude above the ground, 10 meters.
The selected positions correspond to five different headings centered, 45° {northeast, southeast, southwest, and northwest} and four different distances { 2 , 3 , 4 , 5 } 2 to the takeoff origin where the helipad is located. In each position is recorded a total of 1000 p U A V t   samples. Looking at Figure 8, while the blue line maintains the desired directions of 45° (NE, SE, SO, NO), the centers of the positions recorded by the vision system, the red line, are decoupled, showing a constant angular deviation of the positions.
We consider the aircraft control system is asymptotically stable so that in steady state its position converges to the reference one. Thus, we consider as ground truth the reference positions for the steady flight.
The error position for each of the components is given by Equation (21).
e ( x ) i f = x i f x G T i f
where e ( x ) i f means the position error of the component x of the system i in f referent frame. GT subscript means the ground truth in f reference frame.
Looking at the errors (Figure 9), the error distribution increases with increasing distance from the north-east plane origin (0, 0). This means the error position depends on the position in the NE plane.
Regarding altitude error, Figure 10a,b show for each twenty register positions a distribution with four “modes”. In Figure 10a these modes show as four scatter clusters and in Figure 10b as four peaks in each twenty distributions. In addition, the mean and median of the total error distribution, Figure 10, are displaced from the origin, showing a bias in altitude.
The different colors in Figure 10b show each of the twenty records, all of them showing four modes and centered on the same error terms. In this work, we focus on bias correction of mean and median; however, modeling the error in altitude is outside the scope of this paper.
The altitude error distribution may be a consequence of the internal discretizing of the simulator in the image render, so that the vision system, when segmenting the ROI of the helipad, extracts its contour with a size variation. This would be explainable according to the pinhole model of Equation (4) and PnP Formulation (7), since its scale factor is constant, but the size of the ROI and corner positions would change.

4.2.1. Polar Space Error Analysis

The visual results in Figure 8 and Figure 9 show an apparent angular and radial bias of the helipad global position estimation system. We change the cartesian space to the cylindrical space defined by Equation (22), where the terms E , N , D are the coordinates in the NED referent frame.
r 0 = E 2 + N 2 θ 0 = a t a n ( N E ) D 0 = D
When plotting data in the new space in Figure 11, it can be seen how the data set is apparently clustered around a constant bias in the angle and radial error.
However, when showing the distance error (radial) behavior versus distance, Figure 12 shows a high linear correlation between distance and radial error. The angular error is also tested for linear dependence on distance, but the correlation does not exceed 35 % of variance score r 2   in Equation (23), so it has been discarded.
r 2 = 1 V a r ( x x ^ ) V a r ( x )
where V a r ( x x ^ ) indicates the variance of the error between x ^ model estimation and x   data.
Figure 12 shows with “+” the radial error centers of each of the measurement positions and the linear model fitted (blue line) by least squares to these points. This model has a slope α r = 2.4923 × 10 2 and independent term β r = 2.778497 × 10 2   [ m ] . The linear model obtains around 94% of the variance score in Equation (23).
Given the results in Table 3 and Figure 12, the error bias in polar space can be tuned by the model of Equation (24), where the apostrophe over coordinates means corrected coordinate, subscript zero start value, and β i the bias of coordinate i .
r = r 0 ( 1 α r ) β r θ = θ 0 β θ D = D 0 β D

4.2.2. Error Correction in NED Space

Given N , E , and D coordinates of a point p i n in the NED frame and knowing the correction in a cylindrical space in Equation (24), the objective is to return to the NED space. For this purpose, we apply the transformation Equation (25).
N ^ = r ^ . c o s ( θ ) E ^ = r ^ . s i n ( θ ) D ^ = D 0 β D
where its terms r ^ ,   θ   are taken as shown in Equation (26):
r ^ = | v β θ | r r = α r r 0 + β r θ = a t a n ( v N v E ) v ( β θ ) = { v E = E cos ( β θ ) N sin ( β θ ) v N = E sin ( β θ ) + N cos ( β θ )
where the hat over N , E , and D in Equation (26) means the NED coordinate with cylindrical corrections. β { θ , r , D } are the biases of the radial, angular, and altitude terms, respectively. v β θ components and θ mean new position and new angular position after β θ rotation correction.
Figure 13 shows the error distributions of the raw position estimation and error distribution after applying the correction Equation (26) with the parameters of Table 1.
For each drawing in Figure 13, the distributions of the real data are shown in blue and with a blue line their error distribution function [51]. The orange line shows a Gaussian distribution equivalent to the real data in Equation (27). For the NED coordinate, three different figures are represented: raw data Figure 13a–c, data after cylindrical correction using the mean of the raw data Figure 13d–f, and data after cylindrical correction according to the median of the raw data Figure 13g–i. In addition, the mean value and the standard deviation of each case represented by μ and σ , respectively, are indicated on each graph in their legend.
N ( μ , σ , x ) = 1 σ 2 π e ( x μ ) 2 2 σ 2
Figure 13 shows how correction Equation (25) modifies the structure of the error distribution for the cases of N and E coordinates, when comparing the blue with the orange lines. It can be seen in Figure 13a,b how the initial distribution is close to a Gaussian distribution Equation (27) and Figure 13d–h.
The effect on the Gaussian approximations in mean and standard deviation represented in Figure 13 is quantified in Table 4.
Table 4 shows the effect of mean and standard deviation on the data when applying the correction Equation (25) using the mean and median bias value indicated in Table 3.
The standard deviation rows of Table 4 decrease one order of magnitude in all coordinates when applying the correction Equation (25). The same effect can be seen in Table 5 when the RMSE Equation (28) of each NED coordinate is calculated. This metric can be considered as an indicator of accuracy.
R M S E = 1 N i = 1 N ( x ^ i n x G T i n   ) 2

4.3. Landing Evaluation

To evaluate the landing system, we propose to test twenty landing missions from the same position at (10, 10, −20) NED meters to the helipad. The twenty flights are divided into four groups corresponding to using the raw landing system without correction ( w i t h o u t ) Equation (2), applying the bias correction Equation (25) ( B i a s ), with bias correction and a mean filter (Section 3.1.3) with a sliding time window ( M e a n & B i a s ) and a median filter together with the bias correction ( M e d i a n & B i a s ). For each mission, we use as quality metrics the landing trajectory distance Equation (29), time to land Equation (30), and landing accuracy. In addition, the results are compared with classical linear descent setpoints with α = 0.7   [ m / s ] .
D i s t = k = 1 K p k + 1 n p k n
T i m e = t s t a r t t l a n d
where the k   term means temporal step starting in t s t a r t instant and ending in t l a n d moment. p k n represents the global position at k instant in the NED reference frame.
For the flights’ analysis, we use the information obtained from the logs recorded by the flight controller in each flight and unloaded with the ground control station (GCS). In particular, we focus on the global positioning of the UAV. This positioning is given by the EKF2 fusion system [52] integrated in PX4 and with the specific sensor parameters indicated in the SITL [33] (Section 4.1). To ensure that we evaluate exclusively the landing phase of the UAV, we study the trajectories from the instant t s t a r t where the height gradient is detected to be negative, and the altitude is less than 99.8% of the altitude desired. The final instant t l a n d is obtained when the helipad is reached by the same method as t s t a r t .
Finally, third quality metric, landing accuracy, is obtained with the RMSE of the last ten position samples of the NED coordinates (without altitude). In this way, we ensure that we are on the ground with the same value plus a precision error. For this, we take as ground truth the control position of the marker.
Figure 14 shows the behavior of the aircraft when activating the landing system with the four modes corresponding to the landing system without correction (Figure 14a), landing with bias correction (Figure 14b), landing with bias correction and mean filter (Figure 14c,d), and landing with bias correction and median filter. The five trajectories in each of the figures show a different flight, using the corresponding landing mode in each case. The total number of flights is five for each landing mode, i.e., twenty flights.
Figure 15 illustrates the temporal behavior of each analysis group, showing the three global position components: latitude, longitude, and relative altitude. In addition, our proposal is comparing with linear descent, paying special attention to altitude evolution Figure 15e,f. In this case, estimated altitude and setpoints are shown for the exponential proposal and linear descending.
It can be seen in all cases in Figure 15 how the position of the landing pad, defined by a blue dashed horizontal line, is obtained in the stationary state. The effect of the error in precision is shown with an oscillating behavior (blue dotted line). However, when bias Equation (26) and filtering (Section 3.1.3) corrections are applied, the effect is damped.
The linear decrease approach converges in latitude and longitude (Figure 15b,d), but the system finds it difficult to dampen the spin effect. In the case of an exponential decrease, the inter-waypoint spin effect is considerably less than linear (Figure 15a–d). In both cases, the linear and exponential altitude decreasing approaches (Figure 15) show when error correction is applied and filtering estimation inter-waypoint noise spin effect is smoothed.
Figure 15e,f show a small break at the end of the trajectory that exceeds the reference and picks it up again. To identify the instant to obtain the landing surface, we keep the first term that satisfies the gradient and proximity conditions explained above.
This effect may be a consequence of a decoupling of the fusion system in the estimation of the height, for example for giving more weight to the estimation than to the measurements in the EKF2 filter. Therefore, when identifying the instant of reaching the pad, we are left with the first term that meets the conditions of gradient and proximity explained above.
The total of twenty flights with exponential decreasing approach and the other twenty flights with linear decreasing approaches are summarized in Table 6 and Table 7. These tables are built with the mean and median values of all flights, so the quality metrics means the mean and median values of all flights.
Table 6 and Table 7 show the results of the exponential and linear decreasing approaches grouped for easy comparison. The previous tables show for all cases that the exponential descent proposal improves the results of the linear descent, emphasizing that in the best-case scenario of the linear approach ( M e n a & B i a s ), the trajectory distance is reduced by 32%, the time to land by 61%, and the RMSE of the precision landing by 12%.
In both tables, the mode that provides the minimum mean landing trajectory distance is the bias correction together with the median filter. The minimum mean time to land is provided by the bias correction together with the median filter and the minimum mean RMSE is again provided by the bias correction together with the mean filter. Finally, the similarity between the mean and median values in Table 6 and Table 7 are a good indicator of normal distribution.

5. Conclusions

Through the study of the global position estimation system error of Section 4, an angular and radial bias was identified. In addition, it was shown how the error distribution increases its degree of dispersion with the distance to the origin. This position error was initially modeled in a cylindrical space in Equation (24) and transferred to the NED reference space under the transformation Equations (25) and (26). The corrections over cylindrical space produced a structural transformation of the position error distribution, approximating its distributions to the Gaussian error.
On the other hand, it was verified how using a system aimed at smoothing trajectories between waypoints can produce a spin effect if the new waypoints are updated with a frequency such that the UAV cannot obtain the previous target and these new waypoints correspond to the same position, but with high uncertainty. Therefore, the path planning system with path smoothing between waypoints can work as an error amplifier performing a circular trajectory.
Finally, we conclude that the combination of an exponential altitude decrease, together with the correction of systematic estimation error and a sliding time window filtering, improves all three quality metrics proposed and reduces the effect of the inter-waypoint noise spin effect. These results facilitate the development of new applications that require a lightweight but robust precision landing strategy.

Author Contributions

Conceptualization, J.P.L.C., J.G.H. and J.M.M.L.; Formal Analysis, J.P.L.C., J.G.H. and J.M.M.L.; Funding Acquisition, J.G.H. and J.M.M.L.; Investigation, J.P.L.C.; Methodology, J.P.L.C.; Project Administration, J.G.H. and J.M.M.L.; Resources, J.P.L.C., J.G.H. and J.M.M.L.; Software, J.P.L.C.; Supervision, J.G.H. and J.M.M.L.; Validation, J.P.L.C., J.G.H. and J.M.M.L.; Visualization, J.P.L.C.; Writing—Original Draft Preparation, J.P.L.C.; Writing—Review and Editing, J.P.L.C., J.G.H. and J.M.M.L. All authors have read and agreed to the published version of the manuscript.

Funding

This research was partially funded by public research projects of Spanish Ministry of Science and Innovation, references PID2020-118249RB-C22 and PDC2021-121567-C22—AEI/10.13039/501100011033, and by the Madrid Government (Comunidad de Madrid, Spain) under the Multiannual Agreement with UC3M in the line of Excellence of University Professors, reference EPUC3M17.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Acknowledgments

Thanks to Cristina Muntañola for her support to convert Figure 1 and Figure 2 to .svg image format.

Conflicts of Interest

The authors declare no conflict of interest.

Nomenclature

Referent frames (RF) p i j Position   of   element   i   in   j RF
{ t } Helipad (target) j T i Linear   translation   i   to   j RF
{ p h } Pinhole model image plane [ j R i | v i j ] Rotation   and   translation   between   i   and   j RF
{ c } Camera j R i Rotation   between   i   and   j RF
{ z } Camera socket v i j Translation   between   i   and   j RF
{ g } Gimbal j F i Nonlinear   RF   transformation   between   i   and   j
{ b } Body gravity center f x Focal length
{ n } North-East-Down (NED) f y
{ e } Earth-Centerd Earth-Fixed (ECEF) c x Principal point
{ g } Global WGS84 datum c y
θ , ϕ , ψ Euler angles k i Distortion coefficients
λ ,   φ ,   h Longitude, Latitude, altitude A Intrinsic camera matrix
Real number space δ Intrinsic camera parameters
B Boolean number Ψ General camera model
P 2 Planar projective space E ^ Reprojection error
x , x ^ State and estimated state O Image feature function
x Euclidian norm of x N Normal distribution
L Sliding window size μ Mean
β i Constant   bias   of   coordinate   i σ Standard deviation

References

  1. Gautam, A.; Sujit, P.; Saripalli, S. A survey of autonomous landing techniques for UAVs. In Proceedings of the 2014 International Conference on Unmanned Aircraft Systems (ICUAS), Orlando, FL, USA, 27–30 May 2014; pp. 1210–1218. [Google Scholar]
  2. Olivares-Mendez, M.A.; Mondragon, I.; Campoy, P.; Martinez, C. Fuzzy controller for UAV-landing task using 3D-position visual estimation. In Proceedings of the 2010 IEEE World International Conference on Fuzzy Systems, WCCI 2010, Barcelona, Spain, 18–23 July 2010. [Google Scholar]
  3. Shi, G.; Shi, X.; O’Connell, M.; Yu, R.; Azizzadenesheli, K.; Anandkumar, A.; Yue, Y.; Chung, S.-J. Neural lander: Stable drone landing control using learned dynamics. In Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; pp. 9784–9790. [Google Scholar]
  4. Jeong, H.-J.; Choi, J.D.; Ha, Y.-G. Vision Based Displacement Detection for Stabilized UAV Control on Cloud Server. Mob. Inf. Syst. 2016, 2016, 1–11. [Google Scholar] [CrossRef]
  5. Chen, Y.; Zhou, Y.; Lv, Q.; Diversity, K.K. A review of V-SLAM. In Proceedings of the 2018 IEEE International Conference on Information and Automation, Wuyi Mountain, China, 11–13 August 2018; pp. 603–608. [Google Scholar]
  6. Kang, Y.; Park, B.-J.; Cho, A.; Yoo, C.-S.; Kim, Y.; Choi, S.; Koo, S.-O.; Oh, S. A Precision Landing Test on Motion Platform and Shipboard of a Tilt-Rotor UAV Based on RTK-GNSS. Int. J. Aeronaut. Space Sci. 2018, 19, 994–1005. [Google Scholar] [CrossRef]
  7. Janousek, J.; Marcon, P. Precision landing options in unmanned aerial vehicles. In Proceedings of the 2018 International Interdisciplinary PhD Workshop (IIPhDW 2018), Swinoujscie, Poland, 9–12 May 2018; pp. 58–60. [Google Scholar]
  8. Aishwarya, C. The Instrument Landing System (ILS)—A Review. Int. J. Progress. Res. Sci. Eng. 2022, 3, 1–6. [Google Scholar]
  9. Alam, S.; Oluoch, J. A survey of safe landing zone detection techniques for autonomous unmanned aerial vehicles (UAVs). Expert Syst. Appl. 2021, 179, 115091. [Google Scholar] [CrossRef]
  10. DJI. Página Oficial. Available online: https://www.dji.com/es (accessed on 19 April 2022).
  11. Yoakum, C.; Cerreta, J. A Review of DJI’s Mavic Pro Precision Landing Accuracy. Int. J. Aviat. Aeronaut. Aerosp. 2020, 7, 1–19. [Google Scholar] [CrossRef]
  12. Mittal, M.; Mohan, R.; Burgard, W.; Valada, A. Vision-Based Autonomous UAV Navigation and Landing for Urban Search and Rescue. arXiv 2022, arXiv:1906.01304. [Google Scholar]
  13. Wubben, J.; Fabra, F.; Calafate, C.T.; Krzeszowski, T.; Marquez-Barja, J.M.; Cano, J.-C.; Manzoni, P. Accurate Landing of Unmanned Aerial Vehicles Using Ground Pattern Recognition. Electronics 2019, 8, 1532. [Google Scholar] [CrossRef] [Green Version]
  14. Zheng, Y.; Xie, H. Review on Neural Network Identification for Maneuvering UAVs. In Proceedings of the International Conference on Sensing, Diagnostics, Prognostics, and Control (SDPC 2018), Xi’an, China, 15–17 August 2018; pp. 339–346. [Google Scholar]
  15. Mebarki, R.; Lippiello, V.; Siciliano, B. Autonomous landing of rotary-wing aerial vehicles by image-based visual servoing in GPS-denied environments. In Proceedings of the 2015 IEEE International Symposium on Safety, Security, and Rescue Robotics, West Lafayette, IN, USA, 18–20 October 2015. [Google Scholar]
  16. Abujoub, S.; McPhee, J.; Westin, C.; Irani, R.A. Unmanned Aerial Vehicle Landing on Maritime Vessels using Signal Prediction of the Ship Motion. In Proceedings of the OCEANS 2018 MTS/IEEE, Charleston, CA, USA, 22–25 October 2018. [Google Scholar]
  17. Mondragón, I.F.; Campoy, P.; Martínez, C.; Olivares-Méndez, M.A. 3D pose estimation based on planar object tracking for UAVs control. In Proceedings of the 2021 IEEE International Conference on Robotics, Automation and Artificial Intelligence, Hong Kong, China, 21–23 April 2021; pp. 35–41. [Google Scholar]
  18. Lebedev, I.; Erashov, A.; Shabanova, A. Accurate Autonomous UAV Landing Using Vision-Based Detection of ArUco-Marker. In International Conference on Interactive Collaborative Robotics; Springer: Cham, Switzerland, 2020; Volume 12336, pp. 179–188. [Google Scholar]
  19. Li, S.; Xu, C.; Xie, M. A Robust O(n) Solution to the Perspective-n-Point Problem. IEEE Trans. Pattern Anal. Mach. Intell. 2012, 34, 1444–1450. [Google Scholar] [CrossRef]
  20. Garrido-Jurado, S.; Muñoz-Salinas, R.; Madrid-Cuevas, F.J.; Marín-Jiménez, M.J. Automatic generation and detection of highly reliable fiducial markers under occlusion. Pattern Recognit. 2014, 47, 2280–2292. [Google Scholar] [CrossRef]
  21. Romero-Ramirez, F.J.; Munoz-Salinas, R.; Medina-Carnicer, R. Fractal Markers: A New Approach for Long-Range Marker Pose Estimation Under Occlusion. IEEE Access 2019, 7, 169908–169919. [Google Scholar] [CrossRef]
  22. Li, B.; Wang, B.; Tan, X.; Wu, J.; Wei, L. Corner location and recognition of single ArUco marker under occlusion based on YOLO algorithm. J. Electron. Imaging 2021, 30, 033012. [Google Scholar] [CrossRef]
  23. Pixhawk. The Hardware Standard for Open-Source Autopilots. Available online: https://pixhawk.org/ (accessed on 13 March 2022).
  24. Koubaa, A.; Allouch, A.; Alajlan, M.; Javed, Y.; Belghith, A.; Khalgui, M. Micro Air Vehicle Link (MAVlink) in a Nutshell: A Survey. IEEE Access 2019, 7, 87658–87680. [Google Scholar] [CrossRef]
  25. Introduction MAVSDK Guide. Available online: https://mavsdk.mavlink.io/main/en/index.html (accessed on 13 March 2022).
  26. Open Source Autopilot for Drones—PX4 Autopilot. Available online: https://px4.io/ (accessed on 22 February 2022).
  27. Lizarraga, M.; Curry, R.; Elkaim, G.H. Flight test results for an improved line of sight guidance law for UAVs. In Proceedings of the American Control Conference, Washington, DC, USA, 17–19 June 2013; pp. 818–823. [Google Scholar]
  28. Anderson, E.P.; Beard, R.W.; McLain, T.W. Real-time dynamic trajectory smoothing for unmanned air vehicles. IEEE Trans. Control Syst. Technol. 2005, 13, 471–477. [Google Scholar] [CrossRef]
  29. Kikutis, R.; Stankūnas, J.; Rudinskas, D.; Masiulionis, T. Adaptation of Dubins Paths for UAV Ground Obstacle Avoidance When Using a Low Cost On-Board GNSS Sensor. Sensors 2017, 17, 2223. [Google Scholar] [CrossRef]
  30. Iii, D.W.S.; Sanfelice, R.G. Autonomous Waypoint Transitioning and Loitering for Unmanned Aerial Vehicles via Hybrid Control. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, San Diego, CA, USA, 4–8 January 2016. [Google Scholar]
  31. Park, S.; Deyst, J.; How, J. A New Nonlinear Guidance Logic for Trajectory Tracking. In Proceedings of the AIAA Guidance, Navigation, and Control Conference an Exhibit, Providence, RI, USA, 16–19 August 2004. [Google Scholar]
  32. Stateczny, A.; Burdziakowski, P.; Najdecka, K.; Domagalska-Stateczna, B. Accuracy of trajectory tracking based on nonlinear guidance logic for hydrographic unmanned surface vessels. Sensors 2020, 20, 832. [Google Scholar] [CrossRef] [Green Version]
  33. Ma, C.; Zhou, Y.; Li, Z. A New Simulation Environment Based on Airsim, ROS, and PX4 for Quadcopter Aircrafts. In Proceedings of the 6th International Conference on Control, Automation and Robotics (ICCAR 2020), Singapore, 20–23 April 2020; pp. 486–490. [Google Scholar]
  34. Shah, S.; Dey, D.; Lovett, C.; Kapoor, A. AirSim: High-Fidelity Visual and Physical Simulation for Autonomous Vehicles. Springer Proc. Adv. Robot. 2018, 5, 621–635. [Google Scholar]
  35. Adli, S.E.; Shoaran, M.; Noorani, S.M.S. GSPnP: Simple and geometric solution for PnP problem. Vis. Comput. 2019, 36, 1549–1557. [Google Scholar]
  36. PLi, P. Quantum implementation of the classical Canny edge detector. Multimed. Tools Appl. 2022, 81, 11665–11694. [Google Scholar]
  37. Luo, S.; Hou, J.; Zheng, B.; Zhong, X.; Liu, P. Research on edge detection algorithm of work piece defect in machine vision detection system. In Proceedings of the 2022 IEEE 6th Information Technology and Mechatronics Engineering Conference (ITOEC 2022), Chongqing, China, 4–6 March 2022; pp. 1231–1235. [Google Scholar]
  38. Hartley, R.; Zisserman, A. Multiple View Geometry in Computer Vision, 2nd ed.; Cambridge University Press: Cambridge, UK, 2003; pp. 262–278. [Google Scholar]
  39. Datta, A.; Kim, J.S.; Kanade, T. Accurate camera calibration using iterative refinement of control points. In Proceedings of the 2009 IEEE 12th International Conference on Computer Vision (ICCV 2009), Kyoto, Japan, 29 September–2 October 2009; pp. 1201–1208. [Google Scholar]
  40. Home—OpenCV. Available online: https://opencv.org/ (accessed on 21 July 2021).
  41. Learning OpenCV 3: Computer Vision in C++ with the OpenCV Library—Adrian Kaehler, Gary Bradski—Google Libros. Available online: https://books.google.es/books?hl=es&lr=&id=LPm3DQAAQBAJ&oi=fnd&pg=PP1&dq=G.+Bradski+and+A.+Kaehler,+Learning+OpenCV3:+ComputerVision+in+C%2B%2B+With+the+OpenCV+Library,+2nd+ed.+Newton,+MA,+USA:+O’Reilly+Media,+2013&ots=2wLqQga9C7&sig=nzLIWPd4uyeVkH93pJkiN7b3hbA&redir_esc=y#v=onepage&q&f=false (accessed on 22 March 2022).
  42. Patel, T.P.; Panchal, S.R.; Student, P.G. Corner Detection Techniques: An Introductory Survey. IJEDR 2014, 2, 2321–9939. [Google Scholar]
  43. Collins, T.; Bartoli, A. Infinitesimal Plane-Based Pose Estimation. Int. J. Comput. Vis. 2014, 109, 252–286. [Google Scholar] [CrossRef]
  44. Valavanis, K.P.; Vachtsevanos, G.J. Handbook of Unmanned Aerial Vehicles; Springer: Dordrecht, The Netherlands, 2015. [Google Scholar]
  45. Zhu, J. Conversion of Earth-centered Earth-fixed coordinates to geodetic coordinates. IEEE Trans. Aerosp. Electron. Syst. 1994, 30, 957–961. [Google Scholar] [CrossRef]
  46. Osen, K. Accurate Conversion of Earth-Fixed Earth-Centered Coordinates to Geodetic Coordinates. Ph.D. Thesis, Norwegian University of Science and Technology, Trondheim, Norway, 2017. [Google Scholar]
  47. OpenCV: Detection of ArUco Markers. Available online: https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html (accessed on 14 March 2022).
  48. Home—AirSim. Available online: https://microsoft.github.io/AirSim/ (accessed on 22 February 2022).
  49. Settings—AirSim. Available online: https://microsoft.github.io/AirSim/settings/ (accessed on 24 March 2022).
  50. QGC—QGroundControl—Drone Control. Available online: http://qgroundcontrol.com/ (accessed on 29 March 2022).
  51. Chen, Y.-C. A tutorial on kernel density estimation and recent advances. Biostat. Epidemiol. 2017, 1, 161–187. [Google Scholar] [CrossRef]
  52. García, J.; Molina, J.M.; Trincado, J. Real evaluation for designing sensor fusion in UAV platforms. Inf. Fusion 2020, 63, 136–152. [Google Scholar] [CrossRef]
Figure 1. General reference frame systems. Reference frames bottom-left to up: helipad (target), pinhole camera model (image plane), camera, gimbal socket, body, NED. Reference frames right up to down: NED, ECEF, Global.
Figure 1. General reference frame systems. Reference frames bottom-left to up: helipad (target), pinhole camera model (image plane), camera, gimbal socket, body, NED. Reference frames right up to down: NED, ECEF, Global.
Sensors 22 03625 g001
Figure 2. LTP, ECEF, and WGS84 reference systems and geometric relationships.
Figure 2. LTP, ECEF, and WGS84 reference systems and geometric relationships.
Sensors 22 03625 g002
Figure 3. Helipad azimuth set formulation.
Figure 3. Helipad azimuth set formulation.
Sensors 22 03625 g003
Figure 4. Approximate altitude setpoint evolution.
Figure 4. Approximate altitude setpoint evolution.
Sensors 22 03625 g004
Figure 5. Helipad global position estimation system.
Figure 5. Helipad global position estimation system.
Sensors 22 03625 g005
Figure 6. SITL Communication and protocol diagram.
Figure 6. SITL Communication and protocol diagram.
Sensors 22 03625 g006
Figure 7. Simulation environment in the calibration process: (a) Image of the calibration pattern in the AirSim reference frame; (b) Random image of the image registration process; (c) Example of reprojection error.
Figure 7. Simulation environment in the calibration process: (a) Image of the calibration pattern in the AirSim reference frame; (b) Random image of the image registration process; (c) Example of reprojection error.
Sensors 22 03625 g007
Figure 8. Data registration in twenty different flights. Yellow, UAV positions with vision system. Blue, UAV positions with navigation system. Blue and red line, linear approaches between navigation and vision system data centers, respectively. Red box, zoom in [−2, 2, 10] NED position.
Figure 8. Data registration in twenty different flights. Yellow, UAV positions with vision system. Blue, UAV positions with navigation system. Blue and red line, linear approaches between navigation and vision system data centers, respectively. Red box, zoom in [−2, 2, 10] NED position.
Sensors 22 03625 g008
Figure 9. Vision system error in north-east plane coordinate. Left, scatter distribution; Right, north-east boxplot with 1.5 whis.
Figure 9. Vision system error in north-east plane coordinate. Left, scatter distribution; Right, north-east boxplot with 1.5 whis.
Sensors 22 03625 g009
Figure 10. Altitude error: (a) Scatter and boxplot. Green triangle: mean, orange line: median; (b) Altitude error distribution for twenty different register positions.
Figure 10. Altitude error: (a) Scatter and boxplot. Green triangle: mean, orange line: median; (b) Altitude error distribution for twenty different register positions.
Sensors 22 03625 g010
Figure 11. Vision system error in polar space. Left, scatter distribution; Right, 2D boxplot with 1.5 whis.
Figure 11. Vision system error in polar space. Left, scatter distribution; Right, 2D boxplot with 1.5 whis.
Sensors 22 03625 g011
Figure 12. Relationship between distance and radial error. Red + symbol, centroid of each position. Blue line, linear model fitted by least squares.
Figure 12. Relationship between distance and radial error. Red + symbol, centroid of each position. Blue line, linear model fitted by least squares.
Sensors 22 03625 g012
Figure 13. NED coordinates density error distribution. North-East-Down data without correction (a,b,g). Data with mean cylindrical correction (c,d,h). Data with median cylindrical correction (e,f,i).
Figure 13. NED coordinates density error distribution. North-East-Down data without correction (a,b,g). Data with mean cylindrical correction (c,d,h). Data with median cylindrical correction (e,f,i).
Sensors 22 03625 g013
Figure 14. Five-flight 3D graphics for each of the four groups: (a) without correction; (b) bias correction; (c) bias and mean filter; (d) bias correction and median filter.
Figure 14. Five-flight 3D graphics for each of the four groups: (a) without correction; (b) bias correction; (c) bias and mean filter; (d) bias correction and median filter.
Sensors 22 03625 g014aSensors 22 03625 g014b
Figure 15. Time evolution of the latitude, longitude, and altitude of four flights with different correction modes in the landing phase: (a,b) latitude, (c,d) longitude, and (e,f) altitude. First column (a,c,e) exponential decrease, second column (b,d,f) linear decrease.
Figure 15. Time evolution of the latitude, longitude, and altitude of four flights with different correction modes in the landing phase: (a,b) latitude, (c,d) longitude, and (e,f) altitude. First column (a,c,e) exponential decrease, second column (b,d,f) linear decrease.
Sensors 22 03625 g015aSensors 22 03625 g015b
Table 1. Sensor parameters simulated in AirSim.
Table 1. Sensor parameters simulated in AirSim.
Sensor Parameters
Barometer
IMU
GPS
Magnetometer
Distance
Default AirSim settings [49]
Gimbal-Camera Resolution   W × H :   640   ×   480 Field   of   view   ( FOV ) :   95 Depth   of   field   focal   distance :   100 Depth   of   field   focal   region :   100 Depth   of   field   F - Stops :   2.8 Target   gamma :   1.5   p c b p g b = [ 0 , 0 , 0.1 ] ( θ , ϕ , ψ ) g b = ( 0 , π 2 , 0 )
Table 2. Internal camera parameters.
Table 2. Internal camera parameters.
ParameterValue
f x 293.35 [mm]
f y 293.31 [mm]
c x 319.64 [px]
c y 239.64 [px]
Distortion   coefficients   k i { 16.44 ,   35.89 ,   6.35 , 6.35 ,   100.7 } × 10 4
Table 3. Stationary error.
Table 3. Stationary error.
β θ   [ ° ] Distances [m] Altitude   β D   [ m ]
Mean−4.0070780.1536360.488412
Median−4.0568720.1455400.545400
Var0.5120710.0051820.014129
Table 4. Gaussian density distribution approximation.
Table 4. Gaussian density distribution approximation.
N ( μ i , σ i ) RawMeanMedian
North μ N   [ × 10 4 ] 7.93020.20017.120
σ N   [ × 10 2 ] 25.3345.2935.454
East μ E   [ × 10 2 ] 1.0870.8510.882
σ E   [ × 10 1 ] 2.4070.5950.594
Altitude μ D   [ × 10 1 ] −4.7740.1100.680
σ D   [ × 10 1 ] 1.1910.1190.119
Table 5. RMSE value.
Table 5. RMSE value.
Coor./RMSERawMeanMedian
North   [ × 10 2 ] 6.4180.2800.298
East   [ × 10 2 ] 5.8040.3610.361
Altitude   [ × 10 2 ] 2.4210.1430.188
Table 6. Mean value of quality metrics.
Table 6. Mean value of quality metrics.
Exp.|LinearDistance [m]Time [s] RMSE   Landing × 10 7
Without131.25|205.88143.77|136.371.650|1.74
Bias57.07|192.1544.30|131.621.011|2.38
Mean&Bias37.44|54.7344.64|116.090.875|1.17
Median&Bias37.91|67.2543.62|131.071.119|2.68
Table 7. Median value of quality metrics.
Table 7. Median value of quality metrics.
Exp.|LinearDistance [m]Time [s] RMSE   Landing × 10 7
Without130.14|205.95144.93|136.372.885|2.12
Bias57.25|195.4644.54|131.811.193|2.43
Mean&Bias34.89|60.6844.95|116.091.037|1.18
Median&Bias36.21|85.5844.34|132.731.042|2.70
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Share and Cite

MDPI and ACS Style

Llerena Caña, J.P.; García Herrero, J.; Molina López, J.M. Error Reduction in Vision-Based Multirotor Landing System. Sensors 2022, 22, 3625. https://doi.org/10.3390/s22103625

AMA Style

Llerena Caña JP, García Herrero J, Molina López JM. Error Reduction in Vision-Based Multirotor Landing System. Sensors. 2022; 22(10):3625. https://doi.org/10.3390/s22103625

Chicago/Turabian Style

Llerena Caña, Juan Pedro, Jesús García Herrero, and José Manuel Molina López. 2022. "Error Reduction in Vision-Based Multirotor Landing System" Sensors 22, no. 10: 3625. https://doi.org/10.3390/s22103625

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop