Hovering Control for Automatic Landing Operation of An Inspection Drone to A Mobile Platform

Proceedings of the 3rd IFAC Workshop on Proceedings of the 3rd IFAC Workshop on Automatic Control Offshore Oil and Gas Proceedings of thein 3rd IFAC Workshop on Production Automatic Control in Offshore Oil andAvailable Gas Production online at www.sciencedirect.com May 30 - June 2018. Esbjerg, Denmark Automatic Control in Offshore Oil and Gas Proceedings of1, 3rd IFAC Workshop on Production May 30 - June 1,the 2018. Esbjerg, Denmark May 30 - June 1, 2018. Esbjerg,Oil Denmark Automatic Control in Offshore and Gas Production May 30 - June 1, 2018. Esbjerg, Denmark

ScienceDirect

IFAC PapersOnLine 51-8 (2018) 245–250

Hovering Control for Automatic Landing Hovering Hovering Control Control for for Automatic Automatic Landing Landing Operation of An Inspection Drone to A Operation of An Inspection Drone to A Hovering Control for Automatic Landing Operation of An Inspection Drone to A Mobile Platform Mobile Platform Operation of An Inspection Drone to A Mobile Platform Mobile Platform Shaobao Li, Petar Durdevic, Zhenyu Yang

Shaobao Li, Petar Durdevic, Zhenyu Yang Shaobao Li, Petar Durdevic, Zhenyu Yang Shaobao Li, Petar Durdevic, Zhenyu YangEsbjerg Department of Energy Energy Technology, Aalborg University, Department of Technology, Aalborg University, Esbjerg Campus, Niels Bohus Vej 8, Esbjerg, 6700, Denmark (e-mail: Department of Energy Technology, Aalborg University, Esbjerg Campus, Niels Bohus Vej 8, Esbjerg, 6700, Denmark (e-mail: [email protected];[email protected];[email protected]). Department of Energy Technology, Aalborg Esbjerg Campus,[email protected];[email protected];[email protected]). Niels Bohus Vej 8, Esbjerg, 6700,University, Denmark (e-mail: Campus,[email protected];[email protected];[email protected]). Niels Bohus Vej 8, Esbjerg, 6700, Denmark (e-mail: [email protected];[email protected];[email protected]). Abstract: Abstract: This This paper paper investigates investigates the the hovering hovering control control problem problem of of aa drone drone on on aa 6-DOF 6-DOF mobile mobile Abstract:An This paper investigates the hovering control problem of a is drone on a 6-DOF mobile platform. inner-outer loop strategy based on high-gain observer presented. platform. An inner-outer loop strategy based on high-gain observer is presented. In In the the outer outer loop, the high-gain observer is designed to estimate the states of the drone such that its velocity Abstract: This paper investigates the hovering control problem of a drone on a 6-DOF mobile platform. An inner-outer loop strategy based on high-gain observer is presented. In the outer loop, the high-gain observer is designed to estimate the states of the drone such that its velocity and acceleration measurements are not necessary, and then a nested saturation controller is platform. An inner-outer loop strategy based on high-gain observer is presented. In the outer loop, the high-gain observer is designed to estimate the states of the drone such that its velocity and acceleration measurements are not necessary, and then a nested saturation controller is designed. In the inner loop, a hybrid controller which can be effectively used for avoiding loop, the high-gain observer is designed to estimate the states of the drone such that its velocity and acceleration measurements are not necessary, and then a nested saturation controller is designed. In the inner loop, a hybrid controller which can be effectively used for avoiding designed. In the measurements inner loop, a hybrid controller which canofabe effectively used afor avoiding unwinding phenomenon is applied to regulate the attitude the drone. Finally, simulation and acceleration are not necessary, and then nested saturation controller is unwinding phenomenon is applied to regulate the attitude of the drone. Finally, a simulation example used demonstrate the designed.is theto loop, a the hybrid controller canof be effectively used aforsimulation avoiding unwinding phenomenon is applied toeffectiveness regulate theof attitude thecontrol drone. method. Finally, example isIn used toinner demonstrate the effectiveness ofwhich the proposed proposed control method. unwinding is applied regulate theof attitude of thecontrol drone. method. Finally, a simulation example is phenomenon used to demonstrate thetoeffectiveness the proposed © 2018, IFAC (International Federation Automatic Control) Hosting by control Elsevier method. Ltd. All rights reserved. example is Hovering, used to demonstrate the ofeffectiveness of the proposed Keywords: landing, high-gain observer, hybrid control, underactuated Keywords: Hovering, landing, high-gain observer, hybrid control, underactuated system. system. Keywords: Hovering, landing, high-gain observer, hybrid control, underactuated system. Keywords: Hovering, landing, high-gain observer,the hybrid control, underactuated system. 1. INTRODUCTION INTRODUCTION position of the the drone. However, However, this method method is is usually usually 1. the position of drone. this 1. INTRODUCTION the position of the drone. However, this method is usually not effective for large drones due to large inertia and not effective for large drones due to large inertia and input input effectiveIn for(Tan large due to large inertia and input 1. INTRODUCTION saturation. etdrones al. However, (2016)), an invariant ellipsoidthe position of the drone. thisinvariant method is usually Drones have have been been widely widely applied applied for for environment environment and and not saturation. In (Tan et al. (2016)), an ellipsoidDrones saturation. In (Tan et al. (2016)), an invariant ellipsoidbased method control law was proposed for hovering of aa not effective for large drones due to large inertia and Drones havesurveillance been widely for environment and based method control law was proposed for hoveringinput production suchapplied as offshore offshore oil & & gas gas exploexploof production surveillance such as oil based method control law was proposed for hovering of a drone on a moving ship deck. Only the motion on heave of saturation. In (Tan et al. (2016)), an invariant ellipsoidproduction surveillance such as offshore oil & gas exploration and production, wind farm inspection, inspection, geography Dronesand haveproduction, been widely applied for environment and drone on a moving ship deck. Only the motion on heave of ration wind farm geography drone on a moving ship deck. Only the motion on heave of the ship is considered and attitude control is not studied. based method control law was proposed for hovering of a ration andand production, wind farm inspection, geography mapping, so on on (Sch¨ (Sch¨ feras et al. (2016); Lee et al. al. the ship is considered and attitude control is not studied. production surveillance such offshore oil & Lee gas explomapping, and so aafer et al. (2016); et the ship is considered and attitude control is not studied. According to our survey, the existing approaches are still drone on a moving ship deck. Only the motion on heave of mapping, and so on aferproduction et al. (2016); et al. According to our survey, the existing approaches are still (2016)). Take the oil (Sch¨ &wind gas as an anLee example. ration and production, farm inspection, geography (2016)). Take the oil & gas production as example. ourhovering survey, existing approaches aredrone still not effective for and landing control of studied. the ship is to considered andthe attitude control is not (2016)). Take the oil & gas production as effecitivity anLee example. Drones have been applied to increase the of According mapping, and so on (Sch¨ a fer et al. (2016); et al. not effective for hovering and landing control of aa drone Drones have been applied to increase the effecitivity of not effective for hovering and landing control of a drone on a 6-DOF moving deck. More recently, some methods According to our survey, the existing approaches are still Drones have been applied to increase the effecitivity of areial inspection foroil facility integrity check,aspollution pollution mon- on a 6-DOF moving deck. More recently, some methods (2016)). Take the & gas production an example. areial inspection for facility integrity check, monon 6-DOF moving deck. More recently, some based on inner-outer inner-outer loopand control strategy provide an not aeffective for hovering landing control of methods a drone areial inspection for facility integrity check, pollution monitoring, as well as facility/pipeline inspection etc. Drone is Drones have been applied to increase the effecitivity of based on loop control strategy provide an itoring, as well as facility/pipeline inspection etc. Drone is based on inner-outer loop control strategy provide an inspiration for hovering control on a 6-DOF moving deck. on a 6-DOF moving deck. More recently, some methods itoring, as well remotely asfor facility/pipeline inspection is inspiration for hovering control on a 6-DOF moving deck. often operated by professional professional operators from the areial operated inspection facility integrity check, pollution monetc. from Drone often remotely by operators the for hovering control on a 6-DOF moving deck. Cao and Lynch (2016) presented an inner-outer loop basedand on Lynch inner-outer loop control strategy provide an often operated remotely by professional operators from the platform or boats. However, to realize a smooth and safe itoring, as well as facility/pipeline inspection etc. Drone is inspiration Cao (2016) presented an inner-outer loop platform or boats. However, to realize a smooth and safe Cao and Lynch (2016) presented an inner-outer loop control strategy for underactuated drone with input and inspiration for hovering control on a 6-DOF moving deck. platform ora commissioning boats. However, to realize aoperators smoothdeck andissafe landing of drone on the moving far often operated remotely by professional from the strategy for underactuated drone with input and landing of a commissioning drone on the moving deck is far control strategy underactuated with input and state constraints, achieving globally drone asymptotically stable Cao and Lynch for (2016) presented an inner-outer loop landing ofora commissioning on to theus deck far more complicated, and there seems that there there no control platform boats. and However, to realize amoving smooth andis drone state constraints, achieving globally asymptotically stable more complicated, there seems to us that issafe no state constraints, achieving globally asymptotically stable results for position and attitude tracking. Naldi et al. control strategy for underactuated drone with input more complicated, and there seems to that there much study or development development being found about automatic landing of a commissioning drone onfound theus moving deck is far no results for position and attitude tracking. Naldi etand al. much study or being about automatic results for position and attitude tracking. Naldi et al. (2017) proposed a robust inner-outer loop control scheme state constraints, achieving globally asymptotically stable much study or development being found about automatic landing of drone drone on onand moving platform either. more complicated, there seems to us that there is no (2017) proposed a robust inner-outer loop control scheme landing of aa moving platform either. proposed robust loop control for drone with auncertain uncertain intertialtracking. matrix. However, the results for position and inner-outer attitude Naldischeme et the al. landing of drone on a movingbeing platform much study or development foundeither. about automatic (2017) for aa drone with intertial matrix. However, Similar to the manned aircrafts’ landing on a mobile for a drone with uncertain intertial matrix. However, the velocity and acceleration are required to be measurable in (2017) proposed a robust inner-outer loop control scheme Similar to drone the manned aircrafts’ landing on a mobile velocity and acceleration are required to be measurable landing of on a moving platform either. in Similar toprocedure, the manned aircrafts’ landing onof aa mobile platform the automatic landing drone velocity and acceleration are required to be measurable in these two works. In most of low-cost drones, acceleration for a drone with uncertain intertial matrix. However, the platform procedure, the automatic landing of a drone these two works. In most of low-cost drones, acceleration platform the aircrafts’ automatic landinghovering drone on moving deck consists of two two landing phases: and these Similar toprocedure, thedeck manned onof aa mobile two works. In most of low-cost drones, acceleration cannot be measured. Therefore, hovering control for drones drones velocity and acceleration are required to control be measurable in on aa moving consists of phases: hovering and cannot be measured. Therefore, hovering for on a moving deck consists of two phases: hovering and landing. During hovering phase, drones need to track the cannot platform procedure, the automatic landing of a drone beusing measured. Therefore, hovering control for drones without velocity and acceleration measurements these two works. In most of low-cost drones, acceleration landing. During hovering phase, drones need to track the without using velocity and acceleration measurements landing. During 3D trajectory of the moving deck, keep fixed height toand the without on trajectory a moving deck consists of two phases: hovering phase, drones needhovering to track velocity and acceleration measurements would be more desirable. cannotbe beusing measured. Therefore, hovering control for drones 3D of the moving deck, keep aa fixed height to the would more desirable. 3D trajectory of the moving deck, keep a fixed height to the moving deck and adjust their attitudes properly for final landing. During hovering phase, drones need to track would be more desirable. without using velocity and acceleration measurements moving deck and adjust their attitudes properly for final In this paper, we investigate the hovering control problem moving deck and adjust attitudes properly for landing. In this this paper, we their will focus on the the hovering control 3D trajectory ofpaper, the moving deck, keep a fixed height tofinal the In this be paper, investigate the hovering control problem would morewe desirable. landing. In we will focus on hovering control In paper, we investigate the hovering control problem of aathis drone above 6-DOF moving moving deck under under disturbances. landing. In this paper, we their will focus on theproperly hovering of a drone. Normally, due to the functional controllability moving deck and adjust attitudes for final control drone above aa 6-DOF deck disturbances. of a drone. Normally, due to the functional controllability of of a drone above a 6-DOF moving The control objective is to make the drone track the trajecIn this paper, we investigate the hovering control deck under disturbances. of a drone. Normally, due tocannot the functional controllability constraint of the drone, itwill track the trajectory of The control objective is to make the drone track theproblem landing. In of this paper, weit focus on thethe hovering control trajecconstraint the drone, cannot track trajectory of The control objective is to make the drone track the trajectory of the moving deck while achieving the best matching of a drone above a 6-DOF moving deck under disturbances. constraint of the drone, it cannot track the trajectory of the moving deck while whiledue keeping their attitudes matching. tory of the moving deck while achieving the best matching of a moving drone. Normally, to thetheir functional controllability the deck keeping attitudes matching. of the moving deck while achieving the bestthe matching attitude between the drone and thedrone deck while safisfying The control objective isdrone to make the track trajecthe moving while keeping their attitudes matching. In recent years, some approaches have been developed to constraint ofdeck thesome drone, it cannot track the developed trajectory of tory attitude between the and the deck while safisfying In recent years, approaches have been to attitude between the drone and the deck while safisfying the functional controllability constraint. Towards this end, tory of the moving deck while achieving the best matching In have been For developed to the functional controllability constraint. Towards this end, solve the years, similar hovering control problem. example, therecent moving decksome whileapproaches keeping their attitudes solve the similar hovering control problem. Formatching. example, the functional controllability constraint. Towards this end, mapping between the deck and the desired trajectory attitude between the drone and the deck while safisfying solve the Ahn similar hovering control problem. Choi and (2015) studied the point point tracking ofexample, drone In recent years, some approaches have been For developed to the mapping between the deck and the desired trajectory Choi and Ahn (2015) studied the tracking of aa drone mapping between the and thecontrollability desired of the the drone satisfying satisfying thedeck functional confunctional controllability constraint. Towardstrajectory this conend, Choi and Ahn (2015) studied the point tracking ofexample, a drone the in 3D space and presented a backstepping-like feedback solve the similar hovering control problem. For drone the functional controllability in 3D space and presented a backstepping-like feedback of of the drone satisfying the functional controllability constraint is constructed, and then an inner-outer loop control the mapping between the deck and the desired trajectory in 3Dand space and presented athe backstepping-like feedback linearization method for control design. The proposed Choi Ahn (2015) studied point tracking of a drone is constructed, and then an inner-outer loop control linearization method for control design. The proposed straint isbased constructed, and an inner-outer loop scheme on high-gain high-gain observer is employed employed for control control of the drone thethen functional controllability conlinearization method for for control design. The proposed method is only only designed static position tracking. In straint in 3D space and presented backstepping-like feedback scheme based satisfying on observer is for method is designed fora static position tracking. In scheme based on high-gain observer is employed for control development, where a nested saturation controller based straint is constructed, and then an inner-outer loop method is only designed for static position tracking. In (Serra et al. al. (2016)), (2016)), visual-servo-based control law was was development, where a nested saturation controller based linearization method aa for control design. control The proposed (Serra et visual-servo-based law where a nested saturation based on the high-gain high-gain observer is proposed proposed forcontroller the outer outer loop scheme based on high-gain observer is employed for control (Serra etisfor al.only (2016)), a visual-servo-based control law was proposed landing control of a drone on a moving target, method designed for static position tracking. In development, on the observer is for the loop proposed for landing control of a drone on a moving target, on the high-gain observer is proposed for the outer loop control, and a hybrid control is applied for the inner-loop development, where a nested saturation controller based proposed for landinginner-loop control of acontrol drone is onapplied acontrol moving target, where the high-gain such that (Serra et al. (2016)), a visual-servo-based law was control, and a hybrid control is applied for the inner-loop where the high-gain inner-loop control is applied such that control, and a hybrid control is applied for for the the outer inner-loop control to avoid unwinding phenomenon. on the high-gain observer isphenomenon. proposed loop where thefor high-gain inner-loop is such that control the attitude dynamcis are of fast enough tomoving neglect their proposed landing control acontrol drone onapplied ato target, to avoid unwinding the attitude dynamcis are fast enough neglect their control to avoid unwinding phenomenon. control, and a hybrid control is applied for the inner-loop the attitude dynamcis are fast enough to neglect their affects on high-gain the outer-loop outer-loop control, whichis is is used to to control where the inner-loop control applied such that affects on the control, which used control affects on the dynamcis outer-loopare control, which istoused to control the attitude fast enough neglect their control to avoid unwinding phenomenon. affects on© the control,Federation which is ofused to control 2405-8963 2018, IFAC (International Automatic Control) Copyright 2018outer-loop IFAC 245 Hosting by Elsevier Ltd. All rights reserved. Copyright © 2018 IFAC 245 Peer review©under of International Federation of Automatic Copyright 2018 responsibility IFAC 245Control. 10.1016/j.ifacol.2018.06.384 Copyright © 2018 IFAC 245

IFAC OOGP 2018 246 Esbjerg, Denmark. May 30 - June 1, 2018

Y

Shaobao Li et al. / IFAC PapersOnLine 51-8 (2018) 245–250

zY

Y

zY

yY

 K

z



;ĂͿ

TĚ ,Ĩ IĚ

K

z

y

yY

ƌ

y

K

 T where ν(ω) = 0 ω T and ⊗ is an operator between two   T T quaternions qi = ηi i , i = 1, 2, defined as follows:    η2 −Ti η1 . q1 ⊗ q2 = 2 1 η1 I3 + S(1 )

TĚ

z 

z

y

\Ě

y

2.2 Hovering Control Problem



;ďͿ

Fig. 1. Coordinate frames and hovering problem description Notations: Throughout this paper, R, Rm and Rn×m denote the spaces of real numbers, real m-vectors and real n × m matrices, respectively. The terms  · p and  · ∞ denote the p-norm and infinity norm, respectively. 2. PROBLEM FORMULATION 2.1 Dynamic Model of Drone To describe the motion of a drone and a 6-DOF moving deck, three reference frames are defined as shown in Fig. 1(a): the inertial reference frame {I} fixed to the earth surface, the platform body-fixed frame {B} attached to the deck surface and the drone body-fixed frame {Q} attached to the drone’s gravity center. The drone is described by the following dynamic model: p˙ = v M v˙ = −uf Re3 + M ge3 + df (1) R˙ = RS(ω) J ω˙ = S(Jω)ω + uτ + dτ where p = [x, y, z]T ∈ R3 and v = [vx , vy , vz ]T ∈ R3 denote the position of the gravity center of the drone and its velocity in the inertial reference frame {I}, respectively, uf ∈ R is the scale translational control force, uτ ∈ R3 is the attitude control torque, M > 0 ∈ R and J = J T > 0 ∈ R3×3 are the mass and the inertia matrix of the drone, respectively, df ∈ R3 and dτ ∈ R3 are the bounded unknown disturbances, g = 9.8 is the acceleration of gravity, e3 = [0, 0, 1]T , S(·) ∈ R3×3 is a skew symmetric matrix regarding a vector x = [x1 , x2 , x3 ]T ∈ R3 given as   0 −x3 x2 (2) S(x) = x3 0 −x1 , −x2 x1 0

and R ∈ SO(3) is the rotation matrix from {Q} to {I}, where SO(3) is the special orthogonal group of 3rd order:

SO(3) = {R ∈ R3×3 : RT R = RRT = I3 , |R| = 1}. (3) The element of SO(3) can be parametrized by a unit  T quaternion as q = η T through Rodrigues formula:

R(q) = I3 + 2ηS() + 2S()2 , where η ∈ R and  ∈ R3 are the scalar and vector components of the unit quaternion q. See more details in (Shuster (1993)). Then the kinematic equation (the third equation of (1)) can be replaced by the following equation:   1 1 −T ω, (4) q˙ = q ⊗ ν(ω) = 2 2 ηI3 + S() 246

This paper focuses on hovering control of a drone above a 6-DOF moving deck. We assume that the desired trajectory and attitude of the deck are determined by the T nonlinear functions pd (t) = [xd , yd , zd ] ∈ R3 and γd (t) = T [φd , θd , ψd ] ∈ R3 with respect to time t, respectively, where pd (t) and γd (t) are 4th continuous differentiable. Therefore, we can define the velocity and angular velocity of the deck as vd (t) = p˙d and wd (t) = γ˙d , respectively. Hovering control aims at tracking the position of the moving deck and keeping a constant heaving distance to the deck. This process is very important for a drone landing on a moving deck safely. The most desirable control is that the controlled drone can track the deck while keeping the same attitude as that of the deck. However, it is noted that the second equation of (1) is underactuated, which means that as the drone flies along the desired trajectory pd , its desired attitude R∗ must satisfy the following functional controllability constraint by neglecting disturbance df : M ge3 − M p¨d . (5) R ∗ e3 = uf Obviously, the moblie platform has its own intrinsic attitude Rd (γd ) that usually cannot satisfy the constaint condition (5). Since position plays a major role in landing control, this paper will study how to make the drone track the position of the deck while achieving the best attitude matching. To make the descent of the drone smooth, a descenting function H(t) is defined. It is conveniently designed to be a smooth sigmoid function. An example will be given in Section 4. Denote H0 = H(t0 ) be the initial height and Hf = H(∞) be the final height of the drone in the heave of the deck as shown in Fig. 1(b). It is noted that, to keep the heaving distance Hf , the desired position z ∗ of the drone in the inertial frame needs to follow the following trajectory: Hf z∗ = + zd . (6) cos(θd ) Therefore, the hovering control problem can be solved if the drone can track the desired trajectory p∗d described as Hf e3 . (7) p∗d = pd + H(t)e3 + cos(θd ) while keeping an attitude satisfying the constraint (5). 3. HOVERING CONTROLLER DESIGN In this section, an inner-outer loop control strategy will be introduced to design the hovering controller. The structure of the inner-outer loop control is shown in Fig. 2.

IFAC OOGP 2018 Esbjerg, Denmark. May 30 - June 1, 2018

ĞƐŝƌĞĚ dƌĂũĞĐƚŽƌLJ

pd

Shaobao Li et al. / IFAC PapersOnLine 51-8 (2018) 245–250

uf

dƌĂŶƐůĂƚŝŽŶĂů ŽŶƚƌŽů

Id T d

\ d qd  Zd pÖ  vÖ

,ŝŐŚŐĂŝŶ ŽďƐĞƌǀĞƌ

p

Recall the translational motion control design with velocity feedback in (Naldi et al. (2017)) first. Let z1 = p − p∗d , z2 = v − p˙∗d . With the first two equations of (1), we have  z˙1 = z2 (14) M z˙2 = −uf Re3 + M ge3 − M p¨∗d + df

KƵƚĞƌ>ŽŽƉ

ƚƚŝƚƵĚĞ ŽŶƚƌŽů

uW

/ŶŶĞƌ>ŽŽƉ ƌŽŶĞ

Fig. 2. Structure of Inner-Outer Loop Control 3.1 High-Gain Observer The high-gain observer is introduced in the outer loop control because velocity and acceleration measurement may be unavailable for some low-cost drones. It is designed based on the following lemma: Lemma 1. (Behtash (1990)). Consider a system with its output y(t) ∈ Rm and its first n − 1 derivative being bounded, i.e., there are postive constants Yk , k = 1, · · · , n − 1 such that y (k)  < Yk . Given a high-gain observer  δ ξ˙i = ξi+1 , i = 1, · · · , n − 1 (8) δ ξ˙n = −λ1 ξn − λ2 ξn−1 − · · · − λn−1 ξ2 − ξ1 + y(t) with δ being any small positive constant, ξi , i = 1, · · · , n being the observer states, and λi , i = 1, · · · , n − 1, being chosen such that the polynomial sn +λ1 sn−1 +· · ·+λn−1 s+ 1 is Hurwitz, then we have ξk+1 δk

− y (k) = −δΨ(k+1) , k = 0, 1, · · · , n − 1, where Ψ = ξn + λ1 ξn−1 + · · · + λn−1 ξ1 . 2) There exist positive constants t∗ and bk , only depending on Yk , k = 1, · · · , n, δ and λi , i = 1, · · · , n − 1, such that Ψ(k)  ≤ bk for all t > t∗ . 1)

247

Obviously, Lemma 1 implies that y (k) can be estimated by ξk+1 δk bounded by δbk+1 , k = 0, 1, · · · , n − 1. It is noted that the high-gain observer does not need to know the system dynamics but only its output. This property has made the high-gain observer widely applied to estimate complex nonlinear systems. In this paper, a high-gain based on Lemma 1 is designed to estimate the position and velocity of the drone under disturbances. The designed high-gain observer is given as follows:  δ π˙ 1 = π2 (9) δ π˙ 2 = −λ1 π2 − π1 + p where π1 , π2 ∈ R3 are the observer states. Choose λ1 such that s2 + λ1 s + 1 is Hurwitz, and then the estimation of the position and velocity of the drone can be described as pˆ = π1 (10) π2 vˆ = (11) δ and there exist constants b1 and b2 such that p − pˆ ≤ δb1 (12) v − vˆ ≤ δb2 . (13) 3.2 Translational Motion Control Translational motion control aims at making the drone track the desired trajectory p∗d such that the qudrotor can hover on the deck while keeping a desired heaving distance. 247

A vectored-thrust control strategy can be applied to design the scalar controller uf . Define a control force vector Fc (z1 , z2 ) as follows: Fc (z1 , z2 ) = M ge3 − M p¨∗d + κ(z1 , z2 ). (15) where κ(z1 , z2 ) is a state feedback law satisfying κ(0, 0) = 0. Substituting (15) into the second equation of (14), we have M z˙2 = −uf Re3 + Fc (z1 , z2 ) − κ(z1 , z2 ) + df . (16) We can define a desired attitude Rc (z1 , z2 ) ∈ SO(3) satisfying uf Rc e3 = Fc (z1 , z2 ). Choose the translational motion control law as uf = Fc (z1 , z2 ). (17) Then, we have Fc (z1 , z2 ) R c e3 = . (18) Fc (z1 , z2 ) Then, system (14) can be rewritten as  z˙1 = z2 (19) M z˙2 = −uf (R − Rc )e3 − κ(z1 , z2 ) + df . A nested saturation function σ(·) can be used to design κ(z1 , z2 ) to meet the saturation requirement of the scalar control force uf . According to (Naldi et al. (2017)), the following state feedback law is available to guarantee the stability of (19) under the scalar control (17).     k2 k1 κ(z1 , z2 ) = γ2 σ z1 z2 + λ 1 σ , (20) γ2 γ1 where k1 , k2 , γ1 , and γ2 are some positive constants, and σ(·) is the saturation function satisfying the following properties for any s ∈ R:

1) 2) 3) 4) 5)

|σ(s)| ˙ ≤ 2; |¨ σ (s)| ≤ d for some d > 0; sσ(s) > 0 for all s = 0, and σ(0) = 0; σ(s) = sgn(s) for |s| ≥ 1; |s| < |σ(s)| < 1 for |s| < 1.

For a vector s, σ(·) is simply extended by each element satisfying the above properties. Note that each dimension of κ(z1 , z2 ) is bounded by γ2 . Next, we will further discuss the translational motion control design independent of velocity and acceleration measurements. With the high-gain observer proposed in subsection 3.1, a new control law replacing the position and velocity by their estimates is designed as     k2 k1 zˆ2 + γ1 σ , (21) z1 , zˆ2 ) = γ2 σ zˆ1 κ (ˆ γ2 γ1 where zˆ1 = pˆ − p∗d and zˆ2 = vˆ − p˙ ∗d . Then we can define a new control force vector Fc (ˆ z1 , zˆ2 ) as  Fc (ˆ z1 , zˆ2 ) = M ge3 − M p¨∗d + κ (ˆ z1 , zˆ2 ). (22)

IFAC OOGP 2018 248 Esbjerg, Denmark. May 30 - June 1, 2018

Shaobao Li et al. / IFAC PapersOnLine 51-8 (2018) 245–250

Let the translational motion control law in the form as z1 , zˆ2 ), (23) uf = Fc (ˆ and the desired attitude Rc∗ satisfying functional controllability constraint is given by Fc (ˆ z1 , zˆ2 ) Rc∗ e3 = . (24) Fc (ˆ z1 , zˆ2 ) With the new translational motion control law (23), system (19) is changed into  z˙1 = z2 (25) M z˙2 = −uf (R − Rc∗ )e3 − κ (ˆ z1 , zˆ2 ) + df . Then, we have the following results. Theorem 2. Consider the drone system (1) tracking the desired trajectory p∗d described in (7). Design a high-gain observer in the form of (9) with the ploynomial s2 +λ1 s+1 being Hurwitz by choosing proper constant λ1 . Assume that there exists a constant χ∗ > 0 such that uf (R − Rc∗ )e3 ∞ ≤ χ∗ . Under the translational motion control law (23) with κ (ˆ z1 , zˆ2 ) given in (21) and z1 , zˆ2 ) and Fc (ˆ (22), respectively, and k1 , k2 , γ1 and γ2 satisfying γ1 γ2 144k1 γ2 , < , 4k1 γ1 < < 1, (26) k2 4 4M k2 system (25) is ISS with respect to the input −uf (R − Rc∗ )e3 +df , without restricions on the initial states and the input. In particular, the states of system (25) are bounded by the following asymptotic bound 6M 3M z1 p ≤ v2 p , z2 p ≤ v2 p , (27) k1 k2 k2 where v2 = (uf (R−Rc∗ )e3 +(κ(z1 , z2 )−κ (ˆ z1 , zˆ2 ))+df )/M . Proof. Rewrite (25) in the form as   z˙1 = z2 M z˙2 = −κ(z1 , z2 ) − uf (R − Rc∗ )e3  +(κ(z1 , z2 ) − κ (ˆ z1 , zˆ2 )) + df

(28)

Let χ(R, Rc∗ ) = −uf (R−Rc∗ )e3 . System (28) can be viewed as a special case of system (C.1) in Appx C of (Isidori et al. 1 (2012)) with n = 2, q1 (t) = 1, q2 (t) = M , v1 = 0 and v2 = (χ(R, Rc∗ ) + (κ(z1 , z2 ) − κ (ˆ z1 , zˆ2 )) + df ) /M .

It is noted that the inequalities (12) and (13) imply that there exists a constant κ∗ > 0 such that κ(z1 , z2 ) − κ (ˆ z1 , zˆ2 )∞ ≤ κ∗ . Therefore, there exists a constant ∗ v2 > 0 such that v2 ∞ ≤ v2∗ . Directly using the Lemma C.2.1 in (Isidori et al. (2012)), this theorem can be proved.

the quaternion qc∗ corresponding to Rc∗ can be obtained based on the quaternion algebra (Shuster (1993)). Based on the third equation of (1), the desired angular velocity ωc∗ corresponding to Rc∗ is calculated by ωc∗ = GRc∗T R˙ c∗ e3 + ψ˙ d e3 , where G is the matrix with the first, second and third rows given by [0, −1, 0], [1, 0, 0] and [0, 0, 0], respectively, and ψ˙d is the angular velocity of the deck in yaw. Define the quanternion error q˜ and the angular velocity error ω ˜ as (31) q˜ = qc∗−1 ⊗ q ¯c, (32) ω ˜c = ω − ω

q )T ωc∗ . Then, the attitude error system can where ω ¯ c = R(˜ be  written as   1 0  q˜˙ = q˜ ⊗ w ˜c 2  ˙ Jω ˜ c = Λ(˜ ωc , ω ¯ c )˜ ωc + S(J ω ¯ c )¯ ωc − JR(˜ q )T ω˙ c∗ + uτ + dτ (33) with Λ(˜ ωc , ω ¯ c ) defined as Λ(˜ ωc , ω ¯ c ) = S(J ω ˜ c ) + S(J ω ¯ c ) − S(¯ ωc )J − JS(¯ ωc ). Remark 3. q˜ = [±1, 0, 0, 0]T are both the equilibriums of the first equation of (33), because q˜ = [±1, 0, 0, 0]T denote the same attitude in the 3D space. Referring to the hybrid control method proposed in (Mayhew et al. (2011)), the attitude control law is designed as q )T ω˙ c∗ − S(J ω ¯ c )¯ ωc − k3 h˜  − k4 ω ˜c, (34) uτ = JR(˜ where ˜ is the vector component of q˜, k3 and k4 are postive constants, and h ∈ {−1, 1} is a variable governed by  h˙ = 0, h˜ η > −ζ (35) h+ ∈ sgn(˜ η ), h˜ η ≤ −ζ where η˜ is the scalar of q˜, ζ ∈ (0, 1) is the hysteresis threshold, and sgn(˜ η ) is defined as  sgn(˜ η ), |˜ η| > 0 sgn(˜ η) = {−1, 1}, η˜ = 0. The attitude control law (34) is effective to handle the unwinding phenomenon (Bhat and Bernstein (2000)) and noise induced chattering (Mayhew et al. (2011)). Then, we have the following theorem: Theorem 4. Given the attitude error system (33), under the control law (34), system state x ˜ = [˜ qT , ω ˜ cT , h]T is globally uniformly ultimately bounded. In particular, if dτ ≡ 0, system (33) is asymptotically stable. z ,ˆ z ) F  (ˆ Remark 5. Equation (24) implies R˙ ∗ e3 = d c 1 2 . c

Then, ω˙ c∗ is computed by

3.3 Attitude Control In order to satisfy χ(R, Rc∗ )∞ ≤ χ∗ , an attitude control will be designed to make R track Rc∗ . Note that the rotation matrix Rc∗ is a mapping [φ∗c , θc∗ , ψc∗ ]T → Rc∗ , and have the relationship described in XYZ convention as shown in (30). Therefore, through (24) we can fix ψc∗ and θc∗ . ψc∗ can be chosen randomly, but in order to achieve the best matching attitude with the deck, the thrid Euler angle of the desired attitude is fixed by ψc∗ = ψd . In this way, the desired attitude of the drone is determined, and 248

dt |Fc (ˆ z1 ,ˆ z2 )|

¨ ∗ e3 + ψ¨d e3 ω˙ c∗ = GR˙ c∗T R˙ c∗ e3 + GRc∗T R c  (ˆ d ˆ2 ) z1 , zˆ2 ) F d2 F  (ˆ c z1 , z = GS(ωc∗ )T Rc∗T + G 2 c + ψ¨d e3 .  dt |Fc (ˆ z1 , zˆ2 )| dt |Fc (ˆ z1 , zˆ2 )|

3.4 Stability of the Closed-Loop System Combing Theorem 2 and Theorem 4, we can further obtain the stability of the whole closed-loop control system. Theorem 6. Consider the drone system (1) and the desired trajectory p∗d described in (7). Under the scalar translational motion controller (21) with γ1 , γ2 , k1 and k2

cos(θc∗ ) cos(ψc∗ ) ∗ cos(φc ) sin(ψc∗ ) + cos(ψc∗ ) sin(φ∗c ) sin(θc∗ ) sin(φ∗c ) sin(ψc∗ ) − cos(φ∗c ) cos(ψc∗ ) sin(θc∗ )

satisfying (26), and the attitude controller (34) with k3 > 0 and k4 > 0, the tracking errors z1 , z2 , q˜ and ω ˜ c are globally uniformly ultimately bounded. This theorem is obviously based on the ISS property in Theorem 2 and the globally uniformly ultimately bounded property in Theorem 4, and thus the proof is omitted here. Remark 7. It can be observed from Theorem 2 and Theorem 4 that the tracking errors are dependent on df , dτ and the estimation errors of the high-gain observer. Since small δ can enhance the accuracy of the high-gain observer, the tracking errors of the drone can also mitigated by choosing small δ.

1 0 -1 1 0 -1 1 0 -1

40

20 time/s

30

30

35

40

0

5

10

15

20

25

30

35

40

0

5

10

15

20 time/s

25

30

35

40

q*

c

q

0

2

4

6

8

10

0

2

4

6

8

10

0

2

4

6

8

10

0

2

4

6

8

10

time/s

40

f

10

u (N/m) u (N/m) u (N/m)

x

1 0 -1 20

30

40

y

10

1 0 -1 0

10

20

30

40

z

x

v y

25

Fig. 6. The attitude trajectory of the drone.

0

v

20

Fig. 5. The position trajectory of the drone.

40

Fig. 3. The position estimation error of the high-gain observer.

z

15

1 0 -1

1 0 -1 0

v

10

0

1

30

5

-10

2

20

d

p 0

0

3

10

p*

10

u (N/m)

x y

30

(30)

-10

2 0 -2 0

z

20



10

2 0 -2 10

sin(θc∗ ) − cos(θc∗ ) sin(φ∗c ) cos(φ∗c ) cos(θc∗ )

10 0 -10

4. SIMULATION STUDIES

0

249

− cos(θc∗ ) sin(ψc∗ ) ∗ cos(φc ) cos(ψc∗ ) − sin(φ∗c ) sin(θc∗ ) sin(ψc∗ ) cos(ψc∗ ) sin(φ∗c ) + cos(φ∗c ) sin(θc∗ ) sin(ψc∗ )

x/m

=



y/m

Rc∗

Shaobao Li et al. / IFAC PapersOnLine 51-8 (2018) 245–250

z/m

IFAC OOGP 2018 Esbjerg, Denmark. May 30 - June 1, 2018

1 0 -1 0

10

20 time/s

30

40

60 40 20 0

5

10

15

20

0

5

10

15

20

0

5

10

15

20

0

5

10 time/s

15

20

50 0 -50 20 0 -20 10 0 -10

Fig. 7. The translational motion control force and attitude control torques of the drone.

Fig. 4. The velocity estimation error of the high-gain observer. In this section, a numerical example is presented to verify the effectiveness of the proposed control solution. The dynamic model of the drone is given by (1) with system parameters: M = 3.25kg, J = diag(0.032, 0.032, 0.164), and df and dτ being the Gaussian white noises with maximum aplitude of 1.5N and 0.05Nm, respectively. The position trajectory and Euler angles of the 6-DOF deck follow the following nonlinear functions: 249

pd (t) =



π 6−5 cos( 30 t) π t) −0.5−4 sin( 30 sin(0.8t)



, γd (t) = 0.05

 − cos(2t)  sin(2t) − cos(2t)

,

respectively. The descending function H(t) is defined as Hf − H0 , (36) H(t) = H0 + −6(2t−10) 1 + e 10 where, the initial height H0 of the drone is determined by its initial position in Z axis. The drone is desired to hover at the height Hf = 1m above the deck. Then, the desired position trajectory p∗d in (7) is obtained.

IFAC OOGP 2018 250 Esbjerg, Denmark. May 30 - June 1, 2018

Shaobao Li et al. / IFAC PapersOnLine 51-8 (2018) 245–250

10

p

of the drone achieve the best matching with the attitude of the deck while satisfying the functional controllability constraint of the drone. The hybrid control is also available to aviod the unwinding phenomenon. The stability of the whole closed-loop system is theoretically proved. In the future, it would be interesting to further investigate visual based landing control of a drone on a moving deck and make an implementation on a practical drone.

d

z/m

p 5 0

ACKNOWLEDGEMENTS

10 5 0 -5

y/m

0

15

10

5

This work is partially supported by the National Natural Science Foundation of P. R. China under Grant 61403334 and the Natural Science Foundation of Hebei Province under Grant F2017203109.

x/m

Fig. 8. The tracking trajectory of the drone in 3D.

z/m

y/m

x/m

REFERENCES 10 0 -10 0

10

20

30

40

0

10

20

30

40

0

10

20 time/s

30

40

10 5 0 10 5 0

Fig. 9. The tracking errors of the drone. The parameters of the high-gain observer (9), the control force (21) and the control torques (34) are given as: δ = 0.1, λ1 = 2, k1 = 0.1, k2 = 26, γ1 = 0.5, γ2 = 26, k3 = 40 and k4 = 2. Simulation results are shown in Figures 3-9. Figures 3 and 4 show the position estimation error and velocity estimation error of the high-gain observer, respectively. It is observed that the estimation errors are very small and thus the designed high-gain observer is effective. In Figure 5, the blue dash line denotes the desired trajectory p∗d and the red line is the trajectory of the drone. It is observed that the drone tracks the desired trajectory with very small tracking errors. Figure 6 illustates that the attitude of the drone tracks the desired attitude qc∗ . Figure 7 shows that the control force uf and the control torques uτ are bounded. In Figure 8, the blue line denotes the trajectory of the deck and the red line is the trajectory of the drone. From Figure 8 as well as Figure 9, it is observed that the drone tracks the moving deck while keeping an approximate height Hf = 1m above the deck. In summary, the simulation results demonstrate the effectiveness of the proposed control solution. 5. CONCLUSION This paper studies the hovering problem of a drone above a 6-DOF moving deck. A high-gain observer is designed to estimate the position and velocity of the drone. Then, a nested saturation control is designed based on the highgain observer to make the drone track the trajectory of the deck and keep a fixed heaving distance above the moving deck. A hybrid control is applied to make the attitude 250

Behtash, S. (1990). Robust output tracking for non-linear systems. International Journal of Control, 51(6), 1381– 1407. Bhat, S.P. and Bernstein, D.S. (2000). A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon. Systems & Control Letters, 39(1), 63–70. Cao, N. and Lynch, A.F. (2016). Inner–outer loop control for quadrotor uavs with input and state constraints. IEEE Transactions on Control Systems Technology, 24(5), 1797–1804. Choi, Y.C. and Ahn, H.S. (2015). Nonlinear control of quadrotor for point tracking: Actual implementation and experimental tests. IEEE/ASME transactions on mechatronics, 20(3), 1179–1192. Isidori, A., Marconi, L., and Serrani, A. (2012). Robust autonomous guidance: an internal model approach. Springer Science & Business Media. Lee, H., Hwang, Y.S., Jang, G., Kim, T., and Lee, J.M. (2016). 3d map building using the lrf and sinusoidal trajectory of a quadrotor. In Robotics and Biomimetics (ROBIO), 2016 IEEE International Conference on, 690–695. IEEE. Mayhew, C.G., Sanfelice, R.G., and Teel, A.R. (2011). Quaternion-based hybrid control for robust global attitude tracking. IEEE Transactions on Automatic Control, 56(11), 2555–2566. Naldi, R., Furci, M., Sanfelice, R.G., and Marconi, L. (2017). Robust global trajectory tracking for underactuated vtol aerial vehicles using inner-outer loop control paradigms. IEEE Transactions on Automatic Control, 62(1), 97–112. Sch¨afer, B.E., Picchi, D., Engelhardt, T., and Abel, D. (2016). Multicopter unmanned aerial vehicle for automated inspection of wind turbines. In Control and Automation (MED), 2016 24th Mediterranean Conference on, 244–249. IEEE. Serra, P., Cunha, R., Hamel, T., Cabecinhas, D., and Silvestre, C. (2016). Landing of a quadrotor on a moving target using dynamic image-based visual servo control. IEEE Transactions on Robotics, 32(6), 1524–1535. Shuster, M.D. (1993). A survey of attitude representations. Navigation, 8(9), 439–517. Tan, C.K., Wang, J., Paw, Y.C., and Liao, F. (2016). Autonomous ship deck landing of a quadrotor using invariant ellipsoid method. IEEE Transactions on Aerospace and Electronic Systems, 52(2), 891–903.