- Email: [email protected]
Nonlinear Model Predictive Control of Position and Attitude in a Hexacopter with Three Failed Rotors⁎
6th IFAC Conference on Nonlinear Model Predictive Control 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 2018 6th IFAC Conference on Nonlinear Model Predictive Control Available online at www.sciencedirect.com Madison, WI, USA, August 19-22, 2018 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 2018 Madison, WI, USA, August 19-22, 2018
ScienceDirect
IFAC PapersOnLine 51-20 (2018) 228–233
Nonlinear Model Predictive Control of Nonlinear Model Predictive Control of Nonlinear Model Predictive Control of Position and Attitude in Nonlinear Model Predictive Control of Position and Attitude in Position and Attitude in a Hexacopter with Three Failed Position and Attitude inRotors a Hexacopter with Three Failed Rotors a with Three Failed Rotors a Hexacopter Hexacopter with Three Failed Rotors ∗∗ Yusuke Aoki ∗∗ Yuta Asano ∗∗ ∗∗ Akihiko Honda ∗∗
∗∗ Akihiko Honda ∗∗ Yusuke Aoki ∗∗ Yuta Asano ∗∗ ∗∗ ∗ ∗∗ Norizumi Ohtsuka Yusuke AokiMotooka Honda ∗∗ Toshiyuki ∗ ∗∗ ∗ Yuta Asano ∗∗ Akihiko Norizumi Motooka Toshiyuki Ohtsuka Yusuke Aoki Yuta Asano Akihiko Honda ∗∗ Norizumi Motooka ∗∗ Toshiyuki Ohtsuka ∗∗ Norizumi Motooka Ohtsuka ∗ of Systems Science, Toshiyuki Graduate School of Informatics, ∗ Department ∗ Department of Systems Science, Graduate School of Informatics, Kyoto University, Sakyo-ku, Kyoto 606-8501, Department of Systems Science, Graduate School ofJapan Informatics, ∗ Kyoto University, Sakyo-ku, Kyoto 606-8501, ∗∗ Department of Systems Science, Graduate School ofJapan Informatics, Kyoto University, Sakyo-ku, Kyoto 606-8501, Japan Technology R&D Center, Mitsubishi Electric Corporation, ∗∗ Advanced Technology R&D Center,Kyoto Mitsubishi Electric Corporation, Kyoto University, Sakyo-ku, 606-8501, Japan ∗∗ Advanced Amagasaki, Hyogo 661-8661, Japan Advanced Technology R&D Center, Mitsubishi Electric Corporation, ∗∗ Amagasaki, Hyogo 661-8661, Japan Advanced Technology R&D Center, Mitsubishi Electric Corporation, Amagasaki, Hyogo 661-8661, Japan Amagasaki, Hyogo 661-8661, Japan Abstract: We We applied applied nonlinear nonlinear model model predictive predictive control control (NMPC) (NMPC) to to an an unmanned unmanned aerial aerial Abstract: vehicle (a hexacopter) with three failed rotors out of the six rotors available. We considered Abstract: We applied nonlinear model predictive control (NMPC) to an unmanned aerial vehicle (a hexacopter) with three failed rotors out ofcontrol the six(NMPC) rotors available. We considered Abstract: Wefailure: applied nonlinear model predictive to stop, an unmanned aerial three cases cases of awith case in which which three rotors inofalternate alternate positions aWe caseconsidered in which which vehicle (a hexacopter) three failed rotors outin the six rotors available. three of failure: a case in three rotors positions stop, a case in vehicle (a hexacopter) three failed rotors outin ofcase the in sixwhich rotors available. considered three cases ofrotors failure:and awith case in which three rotors alternate positions stop, aWe case in which two adjacent a separate rotor stop, and a three adjacent rotors stop. two adjacent and a separate rotor stop, and in a case in which three stop, adjacent rotors stop. three cases results ofrotors failure: a case inbywhich three rotors alternate positions athe case in which Simulation show that, applying NMPC, the position and attitude of hexacopter two adjacent rotors and a separate rotor stop, and a case in which three adjacent rotors stop. Simulation results show that, by applying NMPC, position and three attitude of the rotors hexacopter two adjacent rotorsfrom and an a separate rotorthat stop, and the a case in which stop. can be be controlled initial state significantly deviates from a adjacent state of equilibrium equilibrium Simulation results show that, by applying NMPC, the position andfrom attitude of the hexacopter can controlled from an initial state that significantly deviates a state of Simulation results show that, by applying NMPC, the position and attitude of the hexacopter in consideration consideration offrom constraints. Moreover, wesignificantly simulated aadeviates case in infrom which there of was no static static can be controlledof an initialMoreover, state thatwe a state equilibrium in constraints. simulated case which there was no can be controlled from an initialMoreover, state thatwesignificantly a state equilibrium equilibrium state and succeeded in controlling the hexacopter’s in consideration of constraints. simulated adeviates caseposition. infrom which there of was no static equilibrium state and succeeded in controlling the hexacopter’s position. in consideration constraints. we the simulated a caseposition. in which there was no static equilibrium state of and succeeded Moreover, in controlling hexacopter’s equilibrium and succeeded in controlling hexacopter’s position. © 2018, IFACstate (International Federation of Automaticthe Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Nonlinear Nonlinear Control, Control, Predictive Predictive Control, Control, Optimal Optimal Control, Control, Unmanned Unmanned Aerial Aerial Keywords: Vehicles, Fault-tolerant Control Keywords: Nonlinear Control, Predictive Control, Optimal Control, Unmanned Aerial Vehicles, Fault-tolerant Control Keywords: Nonlinear Control, Vehicles, Fault-tolerant ControlPredictive Control, Optimal Control, Unmanned Aerial Vehicles, Fault-tolerant Control 1. INTRODUCTION INTRODUCTION only only one one rotor rotor stopped. stopped. On On the the other other hand, hand, Muller Muller et et al. al. 1. (2014) and Hirai (2017) considered cases in which 1. INTRODUCTION only one rotor stopped. On the othersome hand, Muller et al. (2014) and Hirai (2017) considered some cases in which 1. INTRODUCTION only one rotor stopped. On the other hand, Muller et al. one, two, or three but they used and (2017) stopped, considered in which Multirotor unmanned unmanned aerial aerial vehicles vehicles (UAVs) (UAVs) have have become become (2014) one, two, or Hirai three rotors rotors stopped, butsome they cases used linearized linearized Multirotor (2014) and Hirai (2017) considered some cases in which models for control design. In cases, is a two, three rotors theyit used very popular popular (Thompson (2015)). It is is expected that they one, Multirotor unmanned aerial vehicles (UAVs) have become models foror design.stopped, In some somebut cases, is not notlinearized a trivial trivial very (Thompson (2015)). It expected that they one, two, orcontrol three rotors stopped, but theyit used linearized Multirotor unmanned aerial vehicles (UAVs) have become task to find an equilibrium, and, in any case, linearized models for control design. In some cases, it is not a trivial will be useful in the fields of agriculture (Rasmussen et al. very popular (Thompson (2015)). It is expected that they task to find an equilibrium, and, in any case, linearized will be useful in the fields of agriculture (Rasmussen et al. models for control design. In some cases, it is not a triviala very popular (Thompson It isdisaster expected that they models if the state deviates significantly from to are findinvalid an equilibrium, and, in any case, linearized et al. task (2013)), logistics (Fink et (2015)). al. (2011)), relief (Greer will be useful in the fields of agriculture (Rasmussen models are invalid if the state deviates significantly from a (2013)), logistics (Fink et al. (2011)), disaster relief (Greer task to find an equilibrium, and, in any case, linearized will be(2002)), useful insurveying the fields of agriculture (Rasmussen et al. models state of equilibrium. Moreover, Hirai (2017) gave priority are invalid if the state deviates significantly from a et al. et al. (2008)), (2013)), logistics (Fink et (Murphy al. (2011)), disaster reliefbiologi(Greer state of equilibrium. Moreover, Hirai (2017) gave priority et al. (2002)), surveying (Murphy et al. (2008)), biologiinvalid if the state deviates significantly from a (2013)), logistics (Fink et (Murphy al. et (2011)), reliefbiologi(Greer models to altitude control did stabilization of state of are equilibrium. Moreover, Hirai (2017) gave priority calal. animal research (Selby al. (2011)), (2011)), infrastructure et (2002)), surveying et disaster al. (2008)), to altitude control and and did not not achieve achieve stabilization of the the cal animal research (Selby et al. infrastructure state of equilibrium. Moreover, Hirai (2017) gave priority et al. (2002)), surveying (Murphy et al. (2008)), biologito altitude control and did not achieve stabilization of the position. inspection (Nonami et al. (2016)), (2016)), so cal animal (Nonami research et (Selby et al. and (2011)), infrastructure horizontal horizontal inspection al. and so on. on. altitude position. control and did not achieve stabilization of the cal animal (Nonami research et (Selby et al. and (2011)), infrastructure to horizontal position. inspection al. (2016)), so on. As preceding However, UAVs also have a risk of accidents, such as horizontal position. inspection (Nonami et al. (2016)), and so on. preceding research research related related to to this, this, stabilization stabilization control control However, UAVs also have a risk of accidents, such as As As preceding research related to this, stabilization control of a quadcopter or a hexacopter when aa part of the rotor crashes or collisions. In particular, a UAV crash can result However, UAVs also have a risk of accidents, such as of a quadcopter or a hexacopter when part of the rotor crashes or collisions. In particular, a UAV crash can result As preceding research related to this, stabilization control However, UAVs also have a risk of accidents, such as stops by applying LQ control has been studied (Muller of a quadcopter or a hexacopter when a part of the rotor in a serious accident when it is flying over people. Therecrashes or collisions. In particular, a UAV crash can result stops by applying LQ control has been studied (Muller in a serious accident In when it is flying overcrash people. Thereof a quadcopter or a hexacopter when a part of the rotor crashes or collisions. particular, a UAV can result et al. (2014) and Hirai (2017)). However, the UAV model of stops by applying LQ control has been studied (Muller fore, UAVs should be controlled so as not to crash even in a serious accident when it is flying over people. Thereal. (2014) and Hirai (2017)). However, the UAV model of fore, UAVs should bewhen controlled so asover not people. to crash even et stops by applying LQ control has been studied (Muller in a serious accident it is flying Therethese research studies was a linear approximation. Thereet al. (2014) and Hirai (2017)). However, the UAV model of in the case of faults or failure in system components. One fore, UAVs should be controlled so as not to crash even these research studies was a linear approximation. Therein the case of faults or failure in system components. One al. (2014) and Hirai (2017)). However, thenot UAV model of fore, UAVs should be controlled so as not to crash even these research studies was a linear approximation. Therefore, a UAV with such a model might be able to One et possible cause of UAV accidents is failure of rotors because in the case of faults or failure in system components. fore, aresearch UAV with such a amodel might not be able to possible cause of UAV accidents issystem failurecomponents. of rotors because these studies was linear approximation. Therein the case of faults or failure in One stabilize its attitude if it is difficult to find an equilibfore, a UAV with such a model might not be able to possible cause of UAV accidents is failure of rotors because the specified specified equilibrium equilibrium may may no no longer longer be be stabilized stabilized with with stabilize its attitude if it is difficult to find an equilibthe fore, aor UAV with such model might not be equilibable to possible cause of UAV accidents islonger failurebe ofrotors. rotors To because rium significantly deviated from an itsis if ita is difficult to find an thrusts generated by the remaining cope the specified equilibrium may no stabilized with stabilize rium or it it isattitude significantly deviated from an equilibrium equilibrium the thrusts generated by the remaining rotors. To cope stabilize its attitude if it is difficult to find an equilibthe specified equilibrium may no longer be stabilized with state. or Consequently, errors the may conit is significantly deviated from andegrade equilibrium withthrusts such situations, situations, itbyis is the necessary to develop develop control the generatedit remaining rotors. aaTo cope rium Consequently, errors in in the model model may degrade conwith such necessary to control rium or it is significantly deviated from an equilibrium the thrusts generated by the remaining rotors. To cope state. state. Consequently, errors in the model may degrade control performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed with such situations, it is necessary to develop a control trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed state. Consequently, errors in the model may degrade conwith such situations, it is necessary to develop a control (2017)), position control in the horizontal direction was trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed rotors. (2017)), position control in the horizontal direction was rotors. trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed not achieved because attitude control was a priority. In this (2017)), position control in the horizontal direction was rotors. not achieved because attitude control was a priority. In this Various methods methods of of fault–tolerant fault–tolerant control control have have been been propro- (2017)), position control in with the horizontal direction was rotors. research, we describe a UAV six rotors (a hexacopter) not achieved because attitude control was a priority. In this Various research, we describe a UAV with six rotors (a hexacopter) Various methods ofsliding fault–tolerant control have beenetproposed for UAVs: mode control (Sharifi al. not achieved because attitude control was a priority. In this by a nonlinear model instead of the linearized models research, we describe a UAV with six rotors (a hexacopter) posed for UAVs: sliding mode control (Sharifi et al. by a nonlinear model instead of the linearized models Various methods fault–tolerant control have (2010)),for gain-scheduled PIDmode control (Milhim et been al. (2010) (2010) we describe a UAV withofsixthe rotors (a hexacopter) posed UAVs:ofsliding control (Sharifi etproal. research, used previous research. We nonlinear model by a in instead linearized (2010)), gain-scheduled PID control (Milhim et al. posed for UAVs: sliding mode control (Sharifi et al. used innonlinear previous model research. We considered considered nonlinearmodels model and Sadeghzadeh et al. (2012)), PD control (Du et al. by a nonlinear model instead of the linearized models (2010)), gain-scheduled PID control (Milhim et al. (2010) predictive control (NMPC) for the hexacopter in the case used in previous research. We considered nonlinear model and Sadeghzadeh et al. (2012)), PD control (Du et al. (2010)), gain-scheduled PID control (Milhim et al. (2010) predictive control (NMPC) for the hexacopter in the case (2014)), model predictive predictive controlPD(Yu (Yu et al. al.(Du (2013)), in previous research. We considered nonlinear model and Sadeghzadeh et al. (2012)), control et al. used of having three failed rotors out of the six available. In predictive control (NMPC) for the hexacopter in the case (2014)), model control et (2013)), and Sadeghzadeh et al. et (2012)), PD(Yu control et al. predictive of having three failed rotors out ofhexacopter the six available. In (2014)), model (Muller predictive control etHirai al.(Du (2013)), and LQ control al. (2014) and (2017)). control (NMPC) for the in the particular, we simultaneously controlled and of having three failed rotors out of the the six position available.case In and LQ control (Muller et al. (2014)(Yu andetHirai (2017)). (2014)), model predictive control al. (2013)), particular, we simultaneously controlled the position and However, Sharifi et al. (2010), Milhim et al. (2010), of having three failed rotors out of the six available. In and LQ control (Muller et al. (2014) and Hirai (2017)). attitude of the hexacopter in consideration of the conparticular, we simultaneously controlled the position and However, Sharifi et al. (2010), Milhim et al. (2010), and LQ control (Muller et al. Yu (2014) and Hirai (2017)). attitude of we thesimultaneously hexacopter incontrolled consideration of the conSadeghzadeh et al. (2012), and et al. (2013) considered particular, the position and However, Sharifi et al. (2010), Milhim et al. (2010), straints of itsthe thrusts. We five ofthe freedom hexacopter in consideration conSadeghzadeh et al. et (2012), and Yu Milhim et al. (2013) considered However, al.in (2010), et aal. (2010), attitude straints ofof thrusts. We controlled controlled five degrees degreesof freedom a reduction reductionSharifi ofet thrusts thrusts rotors rather than complete attitude ofits the hexacopter inwith consideration ofof the conSadeghzadeh al. (2012), and Yurather et al. (2013) considered straints of its thrusts. We controlled five degrees of freedom except for the yaw direction three rotors. We show a of in rotors than a complete Sadeghzadeh al.al. (2012), andconsidered Yurather et al. (2013) except for thethrusts. yaw direction with three rotors.ofWe show astop, reduction ofetet thrusts in rotors than aconsidered complete and Du (2014) a case in which straints of its Wecontrolled controlled fivehexacopter, degrees freedom that NMPC successfully the even if except for the yaw direction with three rotors. We show stop, and Du et al. (2014) considered a case in which a reduction of et thrusts in rotors rather than complete NMPC successfully controlled the hexacopter, if except for the yaw direction with three rotors. Weeven show stop, and Du al. (2014) considered a casea in which that it was difficult or impossible to design linear controllers. that NMPC successfully controlled the hexacopter, even if stop, and Du et al. (2014) considered a case in which it was difficult or impossible to design linear controllers. This work was partly supported by JSPS KAKENHI Grant This work was partly supported by JSPS KAKENHI Grant that NMPC successfully controlled the linear hexacopter, even if it was difficult or impossible to design controllers. Number 15H02257 This work was partly supported by JSPS KAKENHI Grant it was difficult or impossible to design linear controllers. Number 15H02257 This work was partly supported by JSPS KAKENHI Grant
Number 15H02257 Number 15H02257 2405-8963 © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2018 IFAC 262 Copyright 2018 IFAC 262 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 262 10.1016/j.ifacol.2018.11.018 Copyright © 2018 IFAC 262
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
⎛
2. MODELING 2.1 State Equation of a Hexacopter Our hexacopter was based on the configuration depicted in Fig. 1. To derive the equation of motion, we referred to Nonami et al. (2010). We numbered the rotors and set the
⎜ ⎜ ⎜ x˙ d = ⎜ ⎜ ⎝
229
ξ˙ η˙
sud i Re3 − gei3 m � � M (η)−1 Ψ (η)T (T ud − Dη) ˙ − C(η, η) ˙ η˙
⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠
(4) =: fd (xd , ud ), where m is the hexacopter mass, g is gravitational acceleration, and R ∈ R3×3 is a rotation matrix defined as: R = Rψ Rθ Rφ � � cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ . −sθ sφcθ cφcθ
(5)
Note that s(·) and c(·) are abbreviations for sin(·) and cos(·), respectively. M (η) = Ψ T (η)JΨ (η) (θ �= nπ/2; n = ±1, ±2, . . . ) is a positive-definite inertia matrix, where J ∈ R3×3 is the inertia matrix of the hexacopter. The matrix Ψ (η) ∈ R3×3 is given by: Ψ (η) = Fig. 1. Hexacopter configuration and coordinate axes
T
˙T
T T
12
6
ud = (f1 · · · f6 ) ∈ R , (1) where ξ = (x y z)T ∈ R3 is the position of the hexacopter, η = (φ θ ψ)T ∈ R3 is the vector of the ZYX Euler angles in the inertial frame, and f1 , . . . , f6 are the six independent thrusts generated by the six rotors. The sum of the thrusts, fall , and the torque, τ , can be written as fall = sud ∈ R and τ = T ud − Dη˙ ∈ R3 , respectively, where s = (1 · · · 1) ∈ R1×6 , and the matrices, T ∈ R3×6 and D ∈ R3×3 , are defined as follows: xd = (ξ
η ξ
η˙ ) ∈ R ,
T
⎛
⎞ 1 1 1 1 l l l ⎟ ⎜ − l −l − l 2 2 2 2 ⎟ ⎜ √ √ √ √ ⎜ ⎟ ⎜ 3 3 3 ⎟ T = ⎜ − 3l 0 ⎟, l l 0 − l ⎜ ⎟ 2 2 2 2 ⎜ ⎟ ⎝ ⎠ k −k k −k k −k � � 00 0 D= 0 0 0 , 00γ
1 0 − sin θ 0 cos φ cos θ sin φ 0 − sin φ cos θ cos φ
�
.
(6)
Finally, C(η, η) ˙ is a Coriolis term ˙ (η). C(η, η) ˙ = Ψ (η)T J Ψ˙ (η) + Ψ (η)T ν(Ψ (η)η)JΨ
coordinate axes, as shown in Fig. 1. The coordinate axes eij and ebj (j = 1, 2, 3) denote the inertial frame and body– fixed frame, respectively. The state vector and the control input vector of the hexacopter are defined as follows: T
�
(2)
(3)
where l is the distance from the hexacopter mass center to each rotor, k is a coefficient representing the relation between the thrust and the reaction torque, and γ is a coefficient of air resistance. Accordingly, the state equation of the hexacopter can be described as 263
(7) In this equation, ν(x) is a skew-symmetric matrix defined by ν(x)y := x × y for any vector, y ∈ R3 .
2.2 Cases of Three Failed Rotors The objective of this research was to control the position and attitude of a hexacopter with three failed rotors, and it was necessary to consider all possible cases of failure. It was assumed that the loss of thrusts in failed rotors could be detected by some means. There are essentially three cases of failure of three rotors due to symmetry, as shown in Figs. 2 to 4. In the first case, three rotors in alternate positions stop (Case 1), in the second case, two adjacent rotors and a separate rotor stop (Case 2), and in the third case, three adjacent rotors stop (Case 3). First, we approached Case 1, in which three rotors in alternate positions stop (Fig. 2). For modeling, it was assumed that all rotors were on the vertexes of a regular hexagon, as shown in Fig. 1. Therefore, it sufficed to consider a case in which rotors No. 2, No. 4, and No. 6 stopped. The control input ud in equation (4) is expressed as T ud = ( f1 0 f3 0 f5 0 ) ∈ R6 . (8) Since all the remaining rotors rotated in the same direction, it was inevitable that the rotation of yaw direction occurred. Next, we approached Case 2, in which two adjacent rotors and a separate rotor stop (Fig. 3). In this case, it sufficed to consider a case where rotors No. 3, No. 5, and No. 6 stopped. The control input ud in equation (4) is expressed as T ud = ( f1 f2 0 f4 0 0 ) ∈ R6 . (9) Note that, since rotor No. 2 could not generate a negative thrust, the rotational motion around the axis connecting
2018 IFAC NMPC 230 Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
rotors No. 1 and No. 4 could not be stabilized by any linear feedback controller. Finally, we approached Case 3, in which three adjacent rotors stop (Fig. 4). In this case, it sufficed to consider a case in which rotors No. 4, No. 5, and No. 6 stopped. The control input ud in equation (4) is expressed as T
ud = ( f1 f2 f3 0 0 0 ) ∈ R6 . (10) As can be seen from the configuration of the rotors, there was no static equilibrium, and it was not trivial task to find an equilibrium with nonzero angular velocity.
NMPC is used to perform feedback control by numerically solving an optimal control problem. We used a real-time numerical algorithm called C/GMRES (Ohtsuka (2004)) to solve the NMPC problem. There are a lot of numerical algorithms for NMPC (Diehl et al. (2009), Wolf et al. (2016), and Ohtsuka (2017)). Any of those algorithms could be applied to our NMPC problem as long as realtime optimization is achieved. In this section, we summarize the NMPC and formulate the control problem of the hexacopter. 3.1 Nonlinear Model Predictive Control We consider a general nonlinear system governed by the state equation, x(t) ˙ = f (x(t), u(t), p(t)),
(11)
where x(t) is the state vector, u(t) is the control input vector, and p(t) is the vector of given time-varying parameters. Moreover, an equality condition is imposed as C(x(t), u(t), p(t)) = 0. An inequality constraint can be transformed into an equality constraint by introducing a dummy input. We consider a performance index, J, with a receding finite horizon, represented as follows: Fig. 2. Case 1: three rotors in alternate positions stop
J =ϕ(x(t + T ), p(t + T )) t+T + L(x(τ ), u(τ ), p(τ ))dτ.
(12)
t
For each time t, x(τ ), u(τ ) and p(τ ) (t ≤ τ ≤ t + T ) respectively denote the state vector, control input vector, and vector of the given time-varying parameters on the finite horizon. Note that these vectors predicted on the horizon do not necessarily coincide with the actual state, control input, or time-varying parameter.
Fig. 3. Case 2: two adjacent rotors and a separated rotor stop
By choosing the appropriate functions, ϕ and L, for the performance index, we can formulate various control problems. The optimal control uopt (τ ) (t ≤ τ ≤ t + T ) minimizing J is determined as a time function within the horizon. Accordingly, the actual control input u(t) at time t is given by the initial value of uopt (τ ), which acts as a state feedback control. This feedback control method is called NMPC or nonlinear receding-horizon control. 3.2 Control Methods of a Hexacopter with Failed Rotors Here, we formulated the control problem of the hexacopter. We employed the nonlinear model of the hexacopter expressed by the state equation (4). The thrust generated in each rotor was constrained as follows: umin = 0.144 ≤ ui ≤ 6 = umax [N].
Fig. 4. Case 3: three adjacent rotors stop 3. CONTROL METHODS The control problem of a hexacopter with three failed rotors can be considered as an optimal control problem. 264
(13)
Then, dummy inputs u i (i = 1, . . . , 6) were introduced to transform this inequality into an equality as follows: 2 2 umax − umin umin + umax 2 + u i − = 0, Ci = ui − 2 2 (14) where umin and umax are the minimum and maximum thrusts, respectively, that each rotor can generate. The performance index was
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
J = (xd − xref )T Sf (xd − xref ) t+T + (xd − xref )T Q(xd − xref ) +
t 6 i=1
ri (ui − urefi )2 −
6 i=1
231
4. SIMULATIONS (15)
ri� u�i dτ,
where ui = fi (i = 1, . . . , 6) are control inputs, urefi (i = 1, . . . , 6) are reference values of inputs, xref ∈ R12 is a reference vector of state, Sf , Q ∈ R12×12 are weighting matrices, and ri , ri� (i = 1, . . . , 6) are weighting values. The reference values of the inputs in each case are listed in Table 1, the weighting matrices of state are in Table 2, and the weighting values of inputs are in Table 3. The linear penalty for the dummy inputs in the performance index was introduced to guarantee the uniqueness of the solution. We chose these weights by trial and error. The terminal weights were set to be equal to the stage weights. In Case 1, in which three rotors in alternate positions stop, it was impossible to control the yaw angle because all of the remaining rotors rotated in the same direction. Therefore, the weight of the yaw angle was set to zero. In Case 2, in which two adjacent rotors and a separate rotor stop, stabilization of the attitude was a top priority for maintaining altitude. We set up reference values of inputs to support the hexacopter by thrusts of two diagonal rotors (u1 and u4 ) and reduced the thrust of the remaining rotor, u2 . Since the hexacopter was supposed to be able to support its own weight only by the thrusts of u1 and u4 , the input constraint was relaxed for the simulation study as follows: umin = 0 [N], umax = 12 [N].
4.1 Simulation Conditions The simulations were performed on a personal computer (CPU: Intel(R) Core(TM) i5-6200U 2.30 [GHz], RAM: 8.00 [GB], OS: Windows 7 Professional). The program in C language was written by an automatic code generation system called AutoGenU (Ohtsuka (2015)). AutoGenU is a tool from a symbolic computing environment, Maple, for generating a simulation program for NMPC. The physical parameters of the hexacopter were: m = 1.44 [kg], g = 9.81 [m/s2 ], l = 0.23 [m], k = 1.60×10−9 [m], Jxx = 0.0348 [kg · m2 ], Jyy = 0.0459 [kg · m2 ], and Jzz = 0.0977 [kg · m2 ]. These parameters were measured from a Firefly from AscTec. γ = 1 × 10−2 [N · m · s/rad] was arbitrarily determined with reference to Muller et al. (2014). 4.2 Simulation Results In all three cases, the optimal control problem in the finite horizon of T = 1.0 [s] was solved within 0.75 [ms] for the sampling period 1 [ms]. That is, real-time optimization was achieved. In addition, as shown below, NMPC was superior to the conventional linear control because the position and attitude could be controlled from an initial state that significantly deviated from the state of equilibrium or despite the absence of static equilibrium.
(16)
Although this relaxed constraint cannot be satisfied by Firefly, we can investigate the performance of NMPC for a generic hexacopter. Finally, we considered Case 3, in which three adjacent rotors stop. In this case, since the thrust exists only on one side of the hexacopter, and it is impossible to maintain the attitude from a static state, we thought to maintain horizontal attitude and altitude by rotating in the yaw direction. Therefore, the reference value ψ˙ ref = 12 [rad/s] was, in this case, only given for the component of yaw angular velocity as follows: T xref = 0 · · · 0 ψ˙ ref ∈ R12 . (17) We chose this reference by trial and error.
Fig. 5. Time histories of state variables in Case 1
Table 1. Reference values of the inputs uref1 Case 1 Case 2 Case 3
mg 3 mg 2 mg 3
uref2 – 0 mg 3
uref3 mg 3
uref4 –
mg 3
–
–
mg 2
uref5 mg 3
– –
The simulation results when three rotors stop are shown in Figs. 5 to 10. First, we considered Case 1, in which three rotors in alternate positions stopped. The initial conditions were given as ξ0 = (0 0 0)T , η0 = (1 1 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 0)T . These initial conditions were determined by considering the delay in fault detection. As shown in Figs. 5 and 6, the position and attitude were controlled from an initial state that significantly deviated from the state of equilibrium, while in previous research (Hirai (2017)), the horizontal position was unable to be controlled unless it was from a state of near equilibrium. The rotation around the yaw axis increased as time passed because all three rotors rotated in the same direction and
uref6 – – –
Table 2. Weighting matrices of state Case 1 Case 2 Case 3
Sf , Q diag[5 5 20 10 10 0 0.1 0.1 0.1 0.1 0.1 0] diag[1 1 15 10 10 0 0.1 0.1 0.1 0.1 0.1 0] diag[10 10 10 10 10 0 0.1 0.1 0.1 0.1 0.1 10]
265
2018 IFAC NMPC 232 Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
Table 3. Weighting values of inputs Case 1 Case 2 Case 3
r1 1 1 10
r2 0 1 10
r3 1 0 10
r4 0 1 0
r5 1 0 0
r6 0 0 0
Fig. 6. Time histories of input variables in Case 1
r1 0.1 0.1 0.01
r2 0 0.1 0.01
r3 0.1 0 0.01
r4 0 0.1 0
r5 0.1 0 0
r6 0 0 0
Fig. 8. Time histories of input variables in Case 2
the force around the yaw axis was large. After a certain period of time, the torque in the yaw direction became balanced with the air drag, and the yaw angular velocity ψ˙ converged to a constant.
Fig. 9. Time histories of state variables in Case 3
Fig. 7. Time histories of state variables in Case 2 Next, we considered Case 2, in which two adjacent rotors and a separate rotor stopped. The initial conditions were given as ξ0 = (0 0 0)T , η0 = (1 1 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 0)T . These initial conditions were determined by considering the delay in fault detection. As shown in Figs. 7 and 8, the weight of the hexacopter was supported mainly by the thrusts of two diagonal rotors (u1 and u4 ), and its position and attitude were successfully controlled. In contrast, it had been impossible to maintain the altitude of the hexacopter by conventional linear control, which shows an advantage of NMPC because it explicitly takes nonlinear dynamics into account. 266
Finally, we considered Case 3, in which three adjacent rotors stopped. Unlike the other cases, the initial conditions were given as ξ0 = (0 0 0)T , η0 = (0 0 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 12)T . This initial yaw angular velocity was chosen through trial and error because there was no static equilibrium state and it was difficult to maintain the attitude without the yaw angular velocity. As shown in Figs. 9 and 10, we succeeded in simultaneously maintaining the altitude of the hexacopter and controlling the position in a horizontal direction. It should be noted that there was no need to find a nonzero equilibrium beforehand during the NMPC design. In Fig. 10, we can see the oscillation of the inputs. The frequency of this oscillation is the same as that of the yaw rotation. 5. CONCLUSION In this research, we applied NMPC in order to control the position and attitude of a hexacopter with three
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
Fig. 10. Time histories of input variables in Case 3 failed rotors. As a result, it was possible to control the horizontal position in addition to maintaining altitude, whereas it was impossible to control the horizontal position in previous research using linear control (Hirai (2017)). In particular, the equilibrium in Case 2 could not be stabilized by any linear feedback control, and it was not a trivial task to find a nonzero equilibrium in Case 3. NMPC could handle all the cases with the same performance index and constraints on the control inputs. Future work includes controlling a hexacopter with four or five failed rotors, fault detection, considering the delay of motors, and a hardware experiment with an actual hexacopter to demonstrate real-time optimization of NMPC. REFERENCES C. Thompson. Here’s where the real money is in drones. CNBC, 2015, http://www.cnbc.com/2015/05/13/hereswhere-the-real-money-is-in-drones.html (accessed 20181-19). J. Rasmussen, J. Nielsen, F. Garcia-Ruiz, S. Christensen, and J. C. Streibig. Potential uses of small unmanned aircraft systems (UAS) in weed research. Weed Research, Vol. 53, No. 4, pp. 242–248, 2013. J. Fink, N. Michael, S. Kim, and V. Kumar. Planning and control for cooperative manipulation and transportation with aerial robots. The International Journal of Robotics Research, Vol. 30, No. 3, pp. 324–334, 2011. D. Greer, P. J. M’Kerrow, and J. Abrantes. Robots in urban research and rescue operations. Proceedings of the ACRA’2002, pp. 25–30, 2002. R. R. Murphy, E. Steimle, C. Griffin, C. Cullins, M. Hall, and K. Pratt. Cooperative use of unmanned sea surface and micro aerial vehicles at Hurricane Wilma. Journal of Field Robotics, Vol. 25, No. 3, pp. 164–180, 2008. W. Selby, P. Corke, and D. Rus. Autonomous aerial navigation and tracking of marine animals. Proceedings of the 2011 Australasian Conference on Robotics and Automation, pp. 1–7, 2011. K. Nonami et al. Drone in Progress, (in Japanese). NTS, 2016. F. Sharifi, M. Mirzaei, B. W. Gordon, and Y. Zhang. Fault tolerant control of a quadrotor UAV using sliding mode control. Proceedings of the 2010 Conference on Control and Fault Tolerant Systems, pp. 239–244, 2010. 267
233
A. B. Milhim, Y. Zhang, and C. A. Rabbath. Gain scheduling based PID controller for fault tolerant control of a quad-rotor UAV. Proceedings of AIAA Infotech@Aerospace 2010, 2010. I. Sadeghzadeh, A. Mehta, A. Chamseddine, and Y. Zhang. Active fault tolerant control of a quadrotor UAV based on gain-scheduled PID control. Proceedings of 25th IEEE Canadian Conference on Electrical and Computer Engineering (CCECE), pp. 1–4, 2012. G. X. Du, Q. Quan, and K. Y. Cai. Controllability analysis and degraded control for a class of hexacopters subject to rotor failures. Journal of Intelligent and Robotic Systems, Vol. 78, No. 1, pp. 143–157, 2015. B. Yu, Y. Zhang, I. Minchala, and Y. Qu. Fault tolerant control with linear quadratic and model predictive control techniques against actuator faults in a quadrotor UAV. Proceedings of 2013 Conference on Control and Fault-Tolerant Systems (SysTol), pp. 661–666, 2013. M. W. Muller and R. D’Andrea. Stabillity and control of a quadrocopter despite the complete loss of one, two, or three propellers. Proceedings of the 2014 IEEE International Conference on Robotics & Automation (ICRA), pp. 45–52, 2014. K. Hirai. Equilibrium State Analysis and Control of a Hexacopter with Stopped Rotors, (in Japanese). Department of Physical Engineering, Kyoto University, Kyoto, Japan, Bachelor’s Thesis, 2017. K. Nonami, F. Kendoul, S. Suzuki, W. Wang, and D. Nakazawa. Autonomous Flying Robots–Unmanned Aerial Vehicles and Micro Aerial Vehicles. Springer, 2010. T. Ohtsuka. A continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, Vol. 40, No. 4, pp. 563–574, 2004. M. Diehl, H. J. Ferreau, and N. Haverbeke. Efficient numerical methods for nonlinear MPC and moving horizon estimation. L. Magni, D. M. Raimondo, F. Allg¨ower (Eds.). Nonlinear Model Predictive Control, Springer, pp. 391–417, 2009. I. J. Wolf and W. Marquardt. Fast NMPC schemes for regulatory and economic NMPC – A review. Journal of Process Control, Vol. 44, pp. 162–183, 2016. T. Ohtsuka. Research trend of nonlinear model predictive control, (in Japanese). Systems, Control and Information, Vol. 61, No. 2, pp. 42–50, 2017. T. Ohtsuka. Practical Applications of Control by RealTime Optimization, (in Japanese). Corona Publishing, 2015.
ScienceDirect
IFAC PapersOnLine 51-20 (2018) 228–233
Nonlinear Model Predictive Control of Nonlinear Model Predictive Control of Nonlinear Model Predictive Control of Position and Attitude in Nonlinear Model Predictive Control of Position and Attitude in Position and Attitude in a Hexacopter with Three Failed Position and Attitude inRotors a Hexacopter with Three Failed Rotors a with Three Failed Rotors a Hexacopter Hexacopter with Three Failed Rotors ∗∗ Yusuke Aoki ∗∗ Yuta Asano ∗∗ ∗∗ Akihiko Honda ∗∗
∗∗ Akihiko Honda ∗∗ Yusuke Aoki ∗∗ Yuta Asano ∗∗ ∗∗ ∗ ∗∗ Norizumi Ohtsuka Yusuke AokiMotooka Honda ∗∗ Toshiyuki ∗ ∗∗ ∗ Yuta Asano ∗∗ Akihiko Norizumi Motooka Toshiyuki Ohtsuka Yusuke Aoki Yuta Asano Akihiko Honda ∗∗ Norizumi Motooka ∗∗ Toshiyuki Ohtsuka ∗∗ Norizumi Motooka Ohtsuka ∗ of Systems Science, Toshiyuki Graduate School of Informatics, ∗ Department ∗ Department of Systems Science, Graduate School of Informatics, Kyoto University, Sakyo-ku, Kyoto 606-8501, Department of Systems Science, Graduate School ofJapan Informatics, ∗ Kyoto University, Sakyo-ku, Kyoto 606-8501, ∗∗ Department of Systems Science, Graduate School ofJapan Informatics, Kyoto University, Sakyo-ku, Kyoto 606-8501, Japan Technology R&D Center, Mitsubishi Electric Corporation, ∗∗ Advanced Technology R&D Center,Kyoto Mitsubishi Electric Corporation, Kyoto University, Sakyo-ku, 606-8501, Japan ∗∗ Advanced Amagasaki, Hyogo 661-8661, Japan Advanced Technology R&D Center, Mitsubishi Electric Corporation, ∗∗ Amagasaki, Hyogo 661-8661, Japan Advanced Technology R&D Center, Mitsubishi Electric Corporation, Amagasaki, Hyogo 661-8661, Japan Amagasaki, Hyogo 661-8661, Japan Abstract: We We applied applied nonlinear nonlinear model model predictive predictive control control (NMPC) (NMPC) to to an an unmanned unmanned aerial aerial Abstract: vehicle (a hexacopter) with three failed rotors out of the six rotors available. We considered Abstract: We applied nonlinear model predictive control (NMPC) to an unmanned aerial vehicle (a hexacopter) with three failed rotors out ofcontrol the six(NMPC) rotors available. We considered Abstract: Wefailure: applied nonlinear model predictive to stop, an unmanned aerial three cases cases of awith case in which which three rotors inofalternate alternate positions aWe caseconsidered in which which vehicle (a hexacopter) three failed rotors outin the six rotors available. three of failure: a case in three rotors positions stop, a case in vehicle (a hexacopter) three failed rotors outin ofcase the in sixwhich rotors available. considered three cases ofrotors failure:and awith case in which three rotors alternate positions stop, aWe case in which two adjacent a separate rotor stop, and a three adjacent rotors stop. two adjacent and a separate rotor stop, and in a case in which three stop, adjacent rotors stop. three cases results ofrotors failure: a case inbywhich three rotors alternate positions athe case in which Simulation show that, applying NMPC, the position and attitude of hexacopter two adjacent rotors and a separate rotor stop, and a case in which three adjacent rotors stop. Simulation results show that, by applying NMPC, position and three attitude of the rotors hexacopter two adjacent rotorsfrom and an a separate rotorthat stop, and the a case in which stop. can be be controlled initial state significantly deviates from a adjacent state of equilibrium equilibrium Simulation results show that, by applying NMPC, the position andfrom attitude of the hexacopter can controlled from an initial state that significantly deviates a state of Simulation results show that, by applying NMPC, the position and attitude of the hexacopter in consideration consideration offrom constraints. Moreover, wesignificantly simulated aadeviates case in infrom which there of was no static static can be controlledof an initialMoreover, state thatwe a state equilibrium in constraints. simulated case which there was no can be controlled from an initialMoreover, state thatwesignificantly a state equilibrium equilibrium state and succeeded in controlling the hexacopter’s in consideration of constraints. simulated adeviates caseposition. infrom which there of was no static equilibrium state and succeeded in controlling the hexacopter’s position. in consideration constraints. we the simulated a caseposition. in which there was no static equilibrium state of and succeeded Moreover, in controlling hexacopter’s equilibrium and succeeded in controlling hexacopter’s position. © 2018, IFACstate (International Federation of Automaticthe Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Nonlinear Nonlinear Control, Control, Predictive Predictive Control, Control, Optimal Optimal Control, Control, Unmanned Unmanned Aerial Aerial Keywords: Vehicles, Fault-tolerant Control Keywords: Nonlinear Control, Predictive Control, Optimal Control, Unmanned Aerial Vehicles, Fault-tolerant Control Keywords: Nonlinear Control, Vehicles, Fault-tolerant ControlPredictive Control, Optimal Control, Unmanned Aerial Vehicles, Fault-tolerant Control 1. INTRODUCTION INTRODUCTION only only one one rotor rotor stopped. stopped. On On the the other other hand, hand, Muller Muller et et al. al. 1. (2014) and Hirai (2017) considered cases in which 1. INTRODUCTION only one rotor stopped. On the othersome hand, Muller et al. (2014) and Hirai (2017) considered some cases in which 1. INTRODUCTION only one rotor stopped. On the other hand, Muller et al. one, two, or three but they used and (2017) stopped, considered in which Multirotor unmanned unmanned aerial aerial vehicles vehicles (UAVs) (UAVs) have have become become (2014) one, two, or Hirai three rotors rotors stopped, butsome they cases used linearized linearized Multirotor (2014) and Hirai (2017) considered some cases in which models for control design. In cases, is a two, three rotors theyit used very popular popular (Thompson (2015)). It is is expected that they one, Multirotor unmanned aerial vehicles (UAVs) have become models foror design.stopped, In some somebut cases, is not notlinearized a trivial trivial very (Thompson (2015)). It expected that they one, two, orcontrol three rotors stopped, but theyit used linearized Multirotor unmanned aerial vehicles (UAVs) have become task to find an equilibrium, and, in any case, linearized models for control design. In some cases, it is not a trivial will be useful in the fields of agriculture (Rasmussen et al. very popular (Thompson (2015)). It is expected that they task to find an equilibrium, and, in any case, linearized will be useful in the fields of agriculture (Rasmussen et al. models for control design. In some cases, it is not a triviala very popular (Thompson It isdisaster expected that they models if the state deviates significantly from to are findinvalid an equilibrium, and, in any case, linearized et al. task (2013)), logistics (Fink et (2015)). al. (2011)), relief (Greer will be useful in the fields of agriculture (Rasmussen models are invalid if the state deviates significantly from a (2013)), logistics (Fink et al. (2011)), disaster relief (Greer task to find an equilibrium, and, in any case, linearized will be(2002)), useful insurveying the fields of agriculture (Rasmussen et al. models state of equilibrium. Moreover, Hirai (2017) gave priority are invalid if the state deviates significantly from a et al. et al. (2008)), (2013)), logistics (Fink et (Murphy al. (2011)), disaster reliefbiologi(Greer state of equilibrium. Moreover, Hirai (2017) gave priority et al. (2002)), surveying (Murphy et al. (2008)), biologiinvalid if the state deviates significantly from a (2013)), logistics (Fink et (Murphy al. et (2011)), reliefbiologi(Greer models to altitude control did stabilization of state of are equilibrium. Moreover, Hirai (2017) gave priority calal. animal research (Selby al. (2011)), (2011)), infrastructure et (2002)), surveying et disaster al. (2008)), to altitude control and and did not not achieve achieve stabilization of the the cal animal research (Selby et al. infrastructure state of equilibrium. Moreover, Hirai (2017) gave priority et al. (2002)), surveying (Murphy et al. (2008)), biologito altitude control and did not achieve stabilization of the position. inspection (Nonami et al. (2016)), (2016)), so cal animal (Nonami research et (Selby et al. and (2011)), infrastructure horizontal horizontal inspection al. and so on. on. altitude position. control and did not achieve stabilization of the cal animal (Nonami research et (Selby et al. and (2011)), infrastructure to horizontal position. inspection al. (2016)), so on. As preceding However, UAVs also have a risk of accidents, such as horizontal position. inspection (Nonami et al. (2016)), and so on. preceding research research related related to to this, this, stabilization stabilization control control However, UAVs also have a risk of accidents, such as As As preceding research related to this, stabilization control of a quadcopter or a hexacopter when aa part of the rotor crashes or collisions. In particular, a UAV crash can result However, UAVs also have a risk of accidents, such as of a quadcopter or a hexacopter when part of the rotor crashes or collisions. In particular, a UAV crash can result As preceding research related to this, stabilization control However, UAVs also have a risk of accidents, such as stops by applying LQ control has been studied (Muller of a quadcopter or a hexacopter when a part of the rotor in a serious accident when it is flying over people. Therecrashes or collisions. In particular, a UAV crash can result stops by applying LQ control has been studied (Muller in a serious accident In when it is flying overcrash people. Thereof a quadcopter or a hexacopter when a part of the rotor crashes or collisions. particular, a UAV can result et al. (2014) and Hirai (2017)). However, the UAV model of stops by applying LQ control has been studied (Muller fore, UAVs should be controlled so as not to crash even in a serious accident when it is flying over people. Thereal. (2014) and Hirai (2017)). However, the UAV model of fore, UAVs should bewhen controlled so asover not people. to crash even et stops by applying LQ control has been studied (Muller in a serious accident it is flying Therethese research studies was a linear approximation. Thereet al. (2014) and Hirai (2017)). However, the UAV model of in the case of faults or failure in system components. One fore, UAVs should be controlled so as not to crash even these research studies was a linear approximation. Therein the case of faults or failure in system components. One al. (2014) and Hirai (2017)). However, thenot UAV model of fore, UAVs should be controlled so as not to crash even these research studies was a linear approximation. Therefore, a UAV with such a model might be able to One et possible cause of UAV accidents is failure of rotors because in the case of faults or failure in system components. fore, aresearch UAV with such a amodel might not be able to possible cause of UAV accidents issystem failurecomponents. of rotors because these studies was linear approximation. Therein the case of faults or failure in One stabilize its attitude if it is difficult to find an equilibfore, a UAV with such a model might not be able to possible cause of UAV accidents is failure of rotors because the specified specified equilibrium equilibrium may may no no longer longer be be stabilized stabilized with with stabilize its attitude if it is difficult to find an equilibthe fore, aor UAV with such model might not be equilibable to possible cause of UAV accidents islonger failurebe ofrotors. rotors To because rium significantly deviated from an itsis if ita is difficult to find an thrusts generated by the remaining cope the specified equilibrium may no stabilized with stabilize rium or it it isattitude significantly deviated from an equilibrium equilibrium the thrusts generated by the remaining rotors. To cope stabilize its attitude if it is difficult to find an equilibthe specified equilibrium may no longer be stabilized with state. or Consequently, errors the may conit is significantly deviated from andegrade equilibrium withthrusts such situations, situations, itbyis is the necessary to develop develop control the generatedit remaining rotors. aaTo cope rium Consequently, errors in in the model model may degrade conwith such necessary to control rium or it is significantly deviated from an equilibrium the thrusts generated by the remaining rotors. To cope state. state. Consequently, errors in the model may degrade control performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed with such situations, it is necessary to develop a control trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed state. Consequently, errors in the model may degrade conwith such situations, it is necessary to develop a control (2017)), position control in the horizontal direction was trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed rotors. (2017)), position control in the horizontal direction was rotors. trol performance. Especially, in the latter research (Hirai method for a UAV on the basis of their suffering failed not achieved because attitude control was a priority. In this (2017)), position control in the horizontal direction was rotors. not achieved because attitude control was a priority. In this Various methods methods of of fault–tolerant fault–tolerant control control have have been been propro- (2017)), position control in with the horizontal direction was rotors. research, we describe a UAV six rotors (a hexacopter) not achieved because attitude control was a priority. In this Various research, we describe a UAV with six rotors (a hexacopter) Various methods ofsliding fault–tolerant control have beenetproposed for UAVs: mode control (Sharifi al. not achieved because attitude control was a priority. In this by a nonlinear model instead of the linearized models research, we describe a UAV with six rotors (a hexacopter) posed for UAVs: sliding mode control (Sharifi et al. by a nonlinear model instead of the linearized models Various methods fault–tolerant control have (2010)),for gain-scheduled PIDmode control (Milhim et been al. (2010) (2010) we describe a UAV withofsixthe rotors (a hexacopter) posed UAVs:ofsliding control (Sharifi etproal. research, used previous research. We nonlinear model by a in instead linearized (2010)), gain-scheduled PID control (Milhim et al. posed for UAVs: sliding mode control (Sharifi et al. used innonlinear previous model research. We considered considered nonlinearmodels model and Sadeghzadeh et al. (2012)), PD control (Du et al. by a nonlinear model instead of the linearized models (2010)), gain-scheduled PID control (Milhim et al. (2010) predictive control (NMPC) for the hexacopter in the case used in previous research. We considered nonlinear model and Sadeghzadeh et al. (2012)), PD control (Du et al. (2010)), gain-scheduled PID control (Milhim et al. (2010) predictive control (NMPC) for the hexacopter in the case (2014)), model predictive predictive controlPD(Yu (Yu et al. al.(Du (2013)), in previous research. We considered nonlinear model and Sadeghzadeh et al. (2012)), control et al. used of having three failed rotors out of the six available. In predictive control (NMPC) for the hexacopter in the case (2014)), model control et (2013)), and Sadeghzadeh et al. et (2012)), PD(Yu control et al. predictive of having three failed rotors out ofhexacopter the six available. In (2014)), model (Muller predictive control etHirai al.(Du (2013)), and LQ control al. (2014) and (2017)). control (NMPC) for the in the particular, we simultaneously controlled and of having three failed rotors out of the the six position available.case In and LQ control (Muller et al. (2014)(Yu andetHirai (2017)). (2014)), model predictive control al. (2013)), particular, we simultaneously controlled the position and However, Sharifi et al. (2010), Milhim et al. (2010), of having three failed rotors out of the six available. In and LQ control (Muller et al. (2014) and Hirai (2017)). attitude of the hexacopter in consideration of the conparticular, we simultaneously controlled the position and However, Sharifi et al. (2010), Milhim et al. (2010), and LQ control (Muller et al. Yu (2014) and Hirai (2017)). attitude of we thesimultaneously hexacopter incontrolled consideration of the conSadeghzadeh et al. (2012), and et al. (2013) considered particular, the position and However, Sharifi et al. (2010), Milhim et al. (2010), straints of itsthe thrusts. We five ofthe freedom hexacopter in consideration conSadeghzadeh et al. et (2012), and Yu Milhim et al. (2013) considered However, al.in (2010), et aal. (2010), attitude straints ofof thrusts. We controlled controlled five degrees degreesof freedom a reduction reductionSharifi ofet thrusts thrusts rotors rather than complete attitude ofits the hexacopter inwith consideration ofof the conSadeghzadeh al. (2012), and Yurather et al. (2013) considered straints of its thrusts. We controlled five degrees of freedom except for the yaw direction three rotors. We show a of in rotors than a complete Sadeghzadeh al.al. (2012), andconsidered Yurather et al. (2013) except for thethrusts. yaw direction with three rotors.ofWe show astop, reduction ofetet thrusts in rotors than aconsidered complete and Du (2014) a case in which straints of its Wecontrolled controlled fivehexacopter, degrees freedom that NMPC successfully the even if except for the yaw direction with three rotors. We show stop, and Du et al. (2014) considered a case in which a reduction of et thrusts in rotors rather than complete NMPC successfully controlled the hexacopter, if except for the yaw direction with three rotors. Weeven show stop, and Du al. (2014) considered a casea in which that it was difficult or impossible to design linear controllers. that NMPC successfully controlled the hexacopter, even if stop, and Du et al. (2014) considered a case in which it was difficult or impossible to design linear controllers. This work was partly supported by JSPS KAKENHI Grant This work was partly supported by JSPS KAKENHI Grant that NMPC successfully controlled the linear hexacopter, even if it was difficult or impossible to design controllers. Number 15H02257 This work was partly supported by JSPS KAKENHI Grant it was difficult or impossible to design linear controllers. Number 15H02257 This work was partly supported by JSPS KAKENHI Grant
Number 15H02257 Number 15H02257 2405-8963 © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2018 IFAC 262 Copyright 2018 IFAC 262 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 262 10.1016/j.ifacol.2018.11.018 Copyright © 2018 IFAC 262
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
⎛
2. MODELING 2.1 State Equation of a Hexacopter Our hexacopter was based on the configuration depicted in Fig. 1. To derive the equation of motion, we referred to Nonami et al. (2010). We numbered the rotors and set the
⎜ ⎜ ⎜ x˙ d = ⎜ ⎜ ⎝
229
ξ˙ η˙
sud i Re3 − gei3 m � � M (η)−1 Ψ (η)T (T ud − Dη) ˙ − C(η, η) ˙ η˙
⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠
(4) =: fd (xd , ud ), where m is the hexacopter mass, g is gravitational acceleration, and R ∈ R3×3 is a rotation matrix defined as: R = Rψ Rθ Rφ � � cθcψ sφsθcψ − cφsψ cφsθcψ + sφsψ = cθsψ sφsθsψ + cφcψ cφsθsψ − sφcψ . −sθ sφcθ cφcθ
(5)
Note that s(·) and c(·) are abbreviations for sin(·) and cos(·), respectively. M (η) = Ψ T (η)JΨ (η) (θ �= nπ/2; n = ±1, ±2, . . . ) is a positive-definite inertia matrix, where J ∈ R3×3 is the inertia matrix of the hexacopter. The matrix Ψ (η) ∈ R3×3 is given by: Ψ (η) = Fig. 1. Hexacopter configuration and coordinate axes
T
˙T
T T
12
6
ud = (f1 · · · f6 ) ∈ R , (1) where ξ = (x y z)T ∈ R3 is the position of the hexacopter, η = (φ θ ψ)T ∈ R3 is the vector of the ZYX Euler angles in the inertial frame, and f1 , . . . , f6 are the six independent thrusts generated by the six rotors. The sum of the thrusts, fall , and the torque, τ , can be written as fall = sud ∈ R and τ = T ud − Dη˙ ∈ R3 , respectively, where s = (1 · · · 1) ∈ R1×6 , and the matrices, T ∈ R3×6 and D ∈ R3×3 , are defined as follows: xd = (ξ
η ξ
η˙ ) ∈ R ,
T
⎛
⎞ 1 1 1 1 l l l ⎟ ⎜ − l −l − l 2 2 2 2 ⎟ ⎜ √ √ √ √ ⎜ ⎟ ⎜ 3 3 3 ⎟ T = ⎜ − 3l 0 ⎟, l l 0 − l ⎜ ⎟ 2 2 2 2 ⎜ ⎟ ⎝ ⎠ k −k k −k k −k � � 00 0 D= 0 0 0 , 00γ
1 0 − sin θ 0 cos φ cos θ sin φ 0 − sin φ cos θ cos φ
�
.
(6)
Finally, C(η, η) ˙ is a Coriolis term ˙ (η). C(η, η) ˙ = Ψ (η)T J Ψ˙ (η) + Ψ (η)T ν(Ψ (η)η)JΨ
coordinate axes, as shown in Fig. 1. The coordinate axes eij and ebj (j = 1, 2, 3) denote the inertial frame and body– fixed frame, respectively. The state vector and the control input vector of the hexacopter are defined as follows: T
�
(2)
(3)
where l is the distance from the hexacopter mass center to each rotor, k is a coefficient representing the relation between the thrust and the reaction torque, and γ is a coefficient of air resistance. Accordingly, the state equation of the hexacopter can be described as 263
(7) In this equation, ν(x) is a skew-symmetric matrix defined by ν(x)y := x × y for any vector, y ∈ R3 .
2.2 Cases of Three Failed Rotors The objective of this research was to control the position and attitude of a hexacopter with three failed rotors, and it was necessary to consider all possible cases of failure. It was assumed that the loss of thrusts in failed rotors could be detected by some means. There are essentially three cases of failure of three rotors due to symmetry, as shown in Figs. 2 to 4. In the first case, three rotors in alternate positions stop (Case 1), in the second case, two adjacent rotors and a separate rotor stop (Case 2), and in the third case, three adjacent rotors stop (Case 3). First, we approached Case 1, in which three rotors in alternate positions stop (Fig. 2). For modeling, it was assumed that all rotors were on the vertexes of a regular hexagon, as shown in Fig. 1. Therefore, it sufficed to consider a case in which rotors No. 2, No. 4, and No. 6 stopped. The control input ud in equation (4) is expressed as T ud = ( f1 0 f3 0 f5 0 ) ∈ R6 . (8) Since all the remaining rotors rotated in the same direction, it was inevitable that the rotation of yaw direction occurred. Next, we approached Case 2, in which two adjacent rotors and a separate rotor stop (Fig. 3). In this case, it sufficed to consider a case where rotors No. 3, No. 5, and No. 6 stopped. The control input ud in equation (4) is expressed as T ud = ( f1 f2 0 f4 0 0 ) ∈ R6 . (9) Note that, since rotor No. 2 could not generate a negative thrust, the rotational motion around the axis connecting
2018 IFAC NMPC 230 Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
rotors No. 1 and No. 4 could not be stabilized by any linear feedback controller. Finally, we approached Case 3, in which three adjacent rotors stop (Fig. 4). In this case, it sufficed to consider a case in which rotors No. 4, No. 5, and No. 6 stopped. The control input ud in equation (4) is expressed as T
ud = ( f1 f2 f3 0 0 0 ) ∈ R6 . (10) As can be seen from the configuration of the rotors, there was no static equilibrium, and it was not trivial task to find an equilibrium with nonzero angular velocity.
NMPC is used to perform feedback control by numerically solving an optimal control problem. We used a real-time numerical algorithm called C/GMRES (Ohtsuka (2004)) to solve the NMPC problem. There are a lot of numerical algorithms for NMPC (Diehl et al. (2009), Wolf et al. (2016), and Ohtsuka (2017)). Any of those algorithms could be applied to our NMPC problem as long as realtime optimization is achieved. In this section, we summarize the NMPC and formulate the control problem of the hexacopter. 3.1 Nonlinear Model Predictive Control We consider a general nonlinear system governed by the state equation, x(t) ˙ = f (x(t), u(t), p(t)),
(11)
where x(t) is the state vector, u(t) is the control input vector, and p(t) is the vector of given time-varying parameters. Moreover, an equality condition is imposed as C(x(t), u(t), p(t)) = 0. An inequality constraint can be transformed into an equality constraint by introducing a dummy input. We consider a performance index, J, with a receding finite horizon, represented as follows: Fig. 2. Case 1: three rotors in alternate positions stop
J =ϕ(x(t + T ), p(t + T )) t+T + L(x(τ ), u(τ ), p(τ ))dτ.
(12)
t
For each time t, x(τ ), u(τ ) and p(τ ) (t ≤ τ ≤ t + T ) respectively denote the state vector, control input vector, and vector of the given time-varying parameters on the finite horizon. Note that these vectors predicted on the horizon do not necessarily coincide with the actual state, control input, or time-varying parameter.
Fig. 3. Case 2: two adjacent rotors and a separated rotor stop
By choosing the appropriate functions, ϕ and L, for the performance index, we can formulate various control problems. The optimal control uopt (τ ) (t ≤ τ ≤ t + T ) minimizing J is determined as a time function within the horizon. Accordingly, the actual control input u(t) at time t is given by the initial value of uopt (τ ), which acts as a state feedback control. This feedback control method is called NMPC or nonlinear receding-horizon control. 3.2 Control Methods of a Hexacopter with Failed Rotors Here, we formulated the control problem of the hexacopter. We employed the nonlinear model of the hexacopter expressed by the state equation (4). The thrust generated in each rotor was constrained as follows: umin = 0.144 ≤ ui ≤ 6 = umax [N].
Fig. 4. Case 3: three adjacent rotors stop 3. CONTROL METHODS The control problem of a hexacopter with three failed rotors can be considered as an optimal control problem. 264
(13)
Then, dummy inputs u i (i = 1, . . . , 6) were introduced to transform this inequality into an equality as follows: 2 2 umax − umin umin + umax 2 + u i − = 0, Ci = ui − 2 2 (14) where umin and umax are the minimum and maximum thrusts, respectively, that each rotor can generate. The performance index was
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
J = (xd − xref )T Sf (xd − xref ) t+T + (xd − xref )T Q(xd − xref ) +
t 6 i=1
ri (ui − urefi )2 −
6 i=1
231
4. SIMULATIONS (15)
ri� u�i dτ,
where ui = fi (i = 1, . . . , 6) are control inputs, urefi (i = 1, . . . , 6) are reference values of inputs, xref ∈ R12 is a reference vector of state, Sf , Q ∈ R12×12 are weighting matrices, and ri , ri� (i = 1, . . . , 6) are weighting values. The reference values of the inputs in each case are listed in Table 1, the weighting matrices of state are in Table 2, and the weighting values of inputs are in Table 3. The linear penalty for the dummy inputs in the performance index was introduced to guarantee the uniqueness of the solution. We chose these weights by trial and error. The terminal weights were set to be equal to the stage weights. In Case 1, in which three rotors in alternate positions stop, it was impossible to control the yaw angle because all of the remaining rotors rotated in the same direction. Therefore, the weight of the yaw angle was set to zero. In Case 2, in which two adjacent rotors and a separate rotor stop, stabilization of the attitude was a top priority for maintaining altitude. We set up reference values of inputs to support the hexacopter by thrusts of two diagonal rotors (u1 and u4 ) and reduced the thrust of the remaining rotor, u2 . Since the hexacopter was supposed to be able to support its own weight only by the thrusts of u1 and u4 , the input constraint was relaxed for the simulation study as follows: umin = 0 [N], umax = 12 [N].
4.1 Simulation Conditions The simulations were performed on a personal computer (CPU: Intel(R) Core(TM) i5-6200U 2.30 [GHz], RAM: 8.00 [GB], OS: Windows 7 Professional). The program in C language was written by an automatic code generation system called AutoGenU (Ohtsuka (2015)). AutoGenU is a tool from a symbolic computing environment, Maple, for generating a simulation program for NMPC. The physical parameters of the hexacopter were: m = 1.44 [kg], g = 9.81 [m/s2 ], l = 0.23 [m], k = 1.60×10−9 [m], Jxx = 0.0348 [kg · m2 ], Jyy = 0.0459 [kg · m2 ], and Jzz = 0.0977 [kg · m2 ]. These parameters were measured from a Firefly from AscTec. γ = 1 × 10−2 [N · m · s/rad] was arbitrarily determined with reference to Muller et al. (2014). 4.2 Simulation Results In all three cases, the optimal control problem in the finite horizon of T = 1.0 [s] was solved within 0.75 [ms] for the sampling period 1 [ms]. That is, real-time optimization was achieved. In addition, as shown below, NMPC was superior to the conventional linear control because the position and attitude could be controlled from an initial state that significantly deviated from the state of equilibrium or despite the absence of static equilibrium.
(16)
Although this relaxed constraint cannot be satisfied by Firefly, we can investigate the performance of NMPC for a generic hexacopter. Finally, we considered Case 3, in which three adjacent rotors stop. In this case, since the thrust exists only on one side of the hexacopter, and it is impossible to maintain the attitude from a static state, we thought to maintain horizontal attitude and altitude by rotating in the yaw direction. Therefore, the reference value ψ˙ ref = 12 [rad/s] was, in this case, only given for the component of yaw angular velocity as follows: T xref = 0 · · · 0 ψ˙ ref ∈ R12 . (17) We chose this reference by trial and error.
Fig. 5. Time histories of state variables in Case 1
Table 1. Reference values of the inputs uref1 Case 1 Case 2 Case 3
mg 3 mg 2 mg 3
uref2 – 0 mg 3
uref3 mg 3
uref4 –
mg 3
–
–
mg 2
uref5 mg 3
– –
The simulation results when three rotors stop are shown in Figs. 5 to 10. First, we considered Case 1, in which three rotors in alternate positions stopped. The initial conditions were given as ξ0 = (0 0 0)T , η0 = (1 1 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 0)T . These initial conditions were determined by considering the delay in fault detection. As shown in Figs. 5 and 6, the position and attitude were controlled from an initial state that significantly deviated from the state of equilibrium, while in previous research (Hirai (2017)), the horizontal position was unable to be controlled unless it was from a state of near equilibrium. The rotation around the yaw axis increased as time passed because all three rotors rotated in the same direction and
uref6 – – –
Table 2. Weighting matrices of state Case 1 Case 2 Case 3
Sf , Q diag[5 5 20 10 10 0 0.1 0.1 0.1 0.1 0.1 0] diag[1 1 15 10 10 0 0.1 0.1 0.1 0.1 0.1 0] diag[10 10 10 10 10 0 0.1 0.1 0.1 0.1 0.1 10]
265
2018 IFAC NMPC 232 Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
Table 3. Weighting values of inputs Case 1 Case 2 Case 3
r1 1 1 10
r2 0 1 10
r3 1 0 10
r4 0 1 0
r5 1 0 0
r6 0 0 0
Fig. 6. Time histories of input variables in Case 1
r1 0.1 0.1 0.01
r2 0 0.1 0.01
r3 0.1 0 0.01
r4 0 0.1 0
r5 0.1 0 0
r6 0 0 0
Fig. 8. Time histories of input variables in Case 2
the force around the yaw axis was large. After a certain period of time, the torque in the yaw direction became balanced with the air drag, and the yaw angular velocity ψ˙ converged to a constant.
Fig. 9. Time histories of state variables in Case 3
Fig. 7. Time histories of state variables in Case 2 Next, we considered Case 2, in which two adjacent rotors and a separate rotor stopped. The initial conditions were given as ξ0 = (0 0 0)T , η0 = (1 1 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 0)T . These initial conditions were determined by considering the delay in fault detection. As shown in Figs. 7 and 8, the weight of the hexacopter was supported mainly by the thrusts of two diagonal rotors (u1 and u4 ), and its position and attitude were successfully controlled. In contrast, it had been impossible to maintain the altitude of the hexacopter by conventional linear control, which shows an advantage of NMPC because it explicitly takes nonlinear dynamics into account. 266
Finally, we considered Case 3, in which three adjacent rotors stopped. Unlike the other cases, the initial conditions were given as ξ0 = (0 0 0)T , η0 = (0 0 0)T , ξ˙0 = (0 0 0)T , and η˙ 0 = (0 0 12)T . This initial yaw angular velocity was chosen through trial and error because there was no static equilibrium state and it was difficult to maintain the attitude without the yaw angular velocity. As shown in Figs. 9 and 10, we succeeded in simultaneously maintaining the altitude of the hexacopter and controlling the position in a horizontal direction. It should be noted that there was no need to find a nonzero equilibrium beforehand during the NMPC design. In Fig. 10, we can see the oscillation of the inputs. The frequency of this oscillation is the same as that of the yaw rotation. 5. CONCLUSION In this research, we applied NMPC in order to control the position and attitude of a hexacopter with three
2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018
Yusuke Aoki et al. / IFAC PapersOnLine 51-20 (2018) 228–233
Fig. 10. Time histories of input variables in Case 3 failed rotors. As a result, it was possible to control the horizontal position in addition to maintaining altitude, whereas it was impossible to control the horizontal position in previous research using linear control (Hirai (2017)). In particular, the equilibrium in Case 2 could not be stabilized by any linear feedback control, and it was not a trivial task to find a nonzero equilibrium in Case 3. NMPC could handle all the cases with the same performance index and constraints on the control inputs. Future work includes controlling a hexacopter with four or five failed rotors, fault detection, considering the delay of motors, and a hardware experiment with an actual hexacopter to demonstrate real-time optimization of NMPC. REFERENCES C. Thompson. Here’s where the real money is in drones. CNBC, 2015, http://www.cnbc.com/2015/05/13/hereswhere-the-real-money-is-in-drones.html (accessed 20181-19). J. Rasmussen, J. Nielsen, F. Garcia-Ruiz, S. Christensen, and J. C. Streibig. Potential uses of small unmanned aircraft systems (UAS) in weed research. Weed Research, Vol. 53, No. 4, pp. 242–248, 2013. J. Fink, N. Michael, S. Kim, and V. Kumar. Planning and control for cooperative manipulation and transportation with aerial robots. The International Journal of Robotics Research, Vol. 30, No. 3, pp. 324–334, 2011. D. Greer, P. J. M’Kerrow, and J. Abrantes. Robots in urban research and rescue operations. Proceedings of the ACRA’2002, pp. 25–30, 2002. R. R. Murphy, E. Steimle, C. Griffin, C. Cullins, M. Hall, and K. Pratt. Cooperative use of unmanned sea surface and micro aerial vehicles at Hurricane Wilma. Journal of Field Robotics, Vol. 25, No. 3, pp. 164–180, 2008. W. Selby, P. Corke, and D. Rus. Autonomous aerial navigation and tracking of marine animals. Proceedings of the 2011 Australasian Conference on Robotics and Automation, pp. 1–7, 2011. K. Nonami et al. Drone in Progress, (in Japanese). NTS, 2016. F. Sharifi, M. Mirzaei, B. W. Gordon, and Y. Zhang. Fault tolerant control of a quadrotor UAV using sliding mode control. Proceedings of the 2010 Conference on Control and Fault Tolerant Systems, pp. 239–244, 2010. 267
233
A. B. Milhim, Y. Zhang, and C. A. Rabbath. Gain scheduling based PID controller for fault tolerant control of a quad-rotor UAV. Proceedings of AIAA Infotech@Aerospace 2010, 2010. I. Sadeghzadeh, A. Mehta, A. Chamseddine, and Y. Zhang. Active fault tolerant control of a quadrotor UAV based on gain-scheduled PID control. Proceedings of 25th IEEE Canadian Conference on Electrical and Computer Engineering (CCECE), pp. 1–4, 2012. G. X. Du, Q. Quan, and K. Y. Cai. Controllability analysis and degraded control for a class of hexacopters subject to rotor failures. Journal of Intelligent and Robotic Systems, Vol. 78, No. 1, pp. 143–157, 2015. B. Yu, Y. Zhang, I. Minchala, and Y. Qu. Fault tolerant control with linear quadratic and model predictive control techniques against actuator faults in a quadrotor UAV. Proceedings of 2013 Conference on Control and Fault-Tolerant Systems (SysTol), pp. 661–666, 2013. M. W. Muller and R. D’Andrea. Stabillity and control of a quadrocopter despite the complete loss of one, two, or three propellers. Proceedings of the 2014 IEEE International Conference on Robotics & Automation (ICRA), pp. 45–52, 2014. K. Hirai. Equilibrium State Analysis and Control of a Hexacopter with Stopped Rotors, (in Japanese). Department of Physical Engineering, Kyoto University, Kyoto, Japan, Bachelor’s Thesis, 2017. K. Nonami, F. Kendoul, S. Suzuki, W. Wang, and D. Nakazawa. Autonomous Flying Robots–Unmanned Aerial Vehicles and Micro Aerial Vehicles. Springer, 2010. T. Ohtsuka. A continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, Vol. 40, No. 4, pp. 563–574, 2004. M. Diehl, H. J. Ferreau, and N. Haverbeke. Efficient numerical methods for nonlinear MPC and moving horizon estimation. L. Magni, D. M. Raimondo, F. Allg¨ower (Eds.). Nonlinear Model Predictive Control, Springer, pp. 391–417, 2009. I. J. Wolf and W. Marquardt. Fast NMPC schemes for regulatory and economic NMPC – A review. Journal of Process Control, Vol. 44, pp. 162–183, 2016. T. Ohtsuka. Research trend of nonlinear model predictive control, (in Japanese). Systems, Control and Information, Vol. 61, No. 2, pp. 42–50, 2017. T. Ohtsuka. Practical Applications of Control by RealTime Optimization, (in Japanese). Corona Publishing, 2015.