- Email: [email protected]
LQR Yaw Control System of a Quadrotor for Constant Unknown Inertia *
21st IFAC Symposium on Automatic Control in Aerospace 21st Symposium on August 27-30, 2019. Cranfield, UK Control 21st IFAC IFAC Symposium on Automatic Automatic Control in in Aerospace Aerospace 21st IFAC Symposium on Automatic Control in Aerospace August August 27-30, 27-30, 2019. 2019. Cranfield, Cranfield, UK UK Available online at www.sciencedirect.com August 27-30, 2019. Cranfield, UK Control in Aerospace 21st IFAC Symposium on Automatic August 27-30, 2019. Cranfield, UK
ScienceDirect
IFAC PapersOnLine 52-12 (2019) 170–175
MMAE/LQR Yaw Control System of a MMAE/LQR Yaw Control System of a MMAE/LQR Yaw Control System of MMAE/LQR Yaw Control System of a a Quadrotor for Constant Unknown Inertia Quadrotor for Constant Unknown Inertia MMAE/LQR Yaw Control System of a Quadrotor for Constant Unknown Inertia Quadrotor for Constant Unknown Inertia Pedro Outeiro, Carlos Cardeira, Paulo Oliveira Quadrotor for Constant Unknown Inertia Pedro Outeiro, Carlos Cardeira, Paulo Oliveira
Pedro Pedro Outeiro, Outeiro, Carlos Carlos Cardeira, Cardeira, Paulo Paulo Oliveira Oliveira IDMEC,Pedro Instituto Superior T´ e cnico, Universidade deOliveira Lisboa, Portugal Outeiro, Carlos Cardeira, Paulo IDMEC, Instituto Superior T´ T´eecnico, cnico, Universidade de Lisboa, Lisboa, Portugal IDMEC, Instituto Superior Universidade de (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} IDMEC, Instituto Superior T´ecnico, Universidade de Lisboa, Portugal Portugal (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). IDMEC, Instituto Superior T´ecnico, Universidade de Lisboa, Portugal (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). @tecnico.ulisboa.pt). (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). @tecnico.ulisboa.pt). Abstract: This paper presents a methodology for angular control of a quadrotor that transports Abstract: paper presents aa methodology for angular aa quadrotor transports This paper presents for angular control of that transports aAbstract: constant This unknown given the estimates both control inertia of angular that velocity, based Abstract: This paperload, presents a methodology methodology for on angular control ofand a quadrotor quadrotor that transports a constant unknown load, given the estimates on both inertia and angular velocity, based a constant unknown load, given the estimates on both inertia and angular velocity, based on measurements from an indoor multi-camera motion capture system and a gyroscope. The Abstract: This paper presents a methodology formotion angular control ofand a quadrotor that transports a constant unknown load, given the estimates on bothcapture inertiasystem angular velocity, based on measurements from an indoor multi-camera and a gyroscope. The on measurements from an indoor multi-camera motion capture system and a gyroscope. The proposed control method is an LQR controller and the proposed estimation method is aonconstant unknown load, given the estimates onand both inertia and estimation angular velocity, based measurements from an indoor multi-camera motion capture system and a gyroscope. Theaa proposed control method is an LQR controller the proposed method is proposed control method is an LQR controller and the proposed estimation method is aa Multi-Model Adaptive Estimator (MMAE). Themotion control system obtained is validated both in on measurements from an indoor multi-camera capture system and a gyroscope. The proposed control method is an LQR controller and the proposed estimation method is Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. proposed control method is anresorting LQR controller and thesystem proposed estimation method is in a Multi-Model Adaptive Estimator (MMAE). The control obtained is validated both simulation and experimentally, to an off-the-shelf commercially available quadrotor. simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. Copyright © 2019. The Authors. Published by Elsevier Ltd. All rights reserved. Keywords: mobile resorting robots, Estimation algorithms, Parameteravailable identification, simulation Autonomous and experimentally, to an off-the-shelf commercially quadrotor. Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Embedded systems Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Embedded systems Sensor Embedded systems Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Fusion, Embedded systems 1. INTRODUCTION of a bank of combined filters and controllers, this method Sensor Fusion, Embedded systems 1. INTRODUCTION INTRODUCTION of aa bank of combined method 1. of of filters and controllers, this method achieved settling filters times and thancontrollers, Outeiro et this al. (2018a). 1. INTRODUCTION of a bank banklower of combined combined filters and controllers, this method achieved lower settling times than Outeiro et al. (2018a). lower settling times than Outeiro et al. (2018a). The rising interest1.inINTRODUCTION drones in recent years has driven a achieved of a bank of combined filters and controllers, this method achieved lower settling times than Outeiro et al. (2018a). A more common method for handling non-linear and The rising interest in in drones drones in recent years has driven The rising interest in recent years has driven new set of applications for these unmanned aerial vehiclesaaa A more common method for handling non-linear and achieved lower settling times than Outeiro et al. (2018a). The rising interest in drones in recent years has driven A more common method for handling non-linear and parameter uncertainty is the Extended Kalman Filter new set Among of applications applications for these these unmanned unmanned aerialone vehicles A more common method for Extended handling non-linear and new set of for aerial vehicles (UAV). these applications, a noteworthy is in parameter uncertainty is the Kalman Filter The rising interest in applications, drones in recent yearsaerial has one driven a (EKF). new set of applications for these unmanned vehicles parameter uncertainty is the Extended Kalman Filter A derivative of the EKF, the Unscented Kalman (UAV). Among these a noteworthy is in A more common method for handling non-linear and parameter uncertainty is the Extended Kalman Filter (UAV). Among these applications, a noteworthy one is in delivery systems. Amazon is currently developing delivery (EKF). A derivative derivative of used the EKF, EKF, the Unscented Unscented Kalman new set Among of applications for is these unmanned aerialone vehicles (UAV). these applications, a noteworthy is in (EKF). A of the the Kalman Filter (UKF), has been in agressive flight control for delivery systems. Amazon currently developing delivery parameter uncertainty is the Extended Kalman Filter (EKF). A derivative of used the EKF, the Unscented Kalman delivery systems. Amazon is currently developing delivery drones and systems to support these drones, such as Filter (UKF), has been in agressive flight control for (UAV). Among these applications, a noteworthy one is as in quadrotors delivery and systems. Amazon is currently developing delivery Filter (UKF), has been used in agressive flight control for in Loianno et al. (2017). Although the EKF drones systems to support these drones, such (EKF). A derivative of the EKF, the Unscented Kalman Filter (UKF), has been used in agressive flight control for drones and systems to support these drones, such as adelivery multi-tiered warehouse. With these applications, one quadrotors in Loianno et al. (2017). Although the EKF systems. Amazon is currently developing delivery drones and systems to support these drones, such as quadrotors in Loianno et al. (2017). Although the EKF and its derivative methods see widespread use, there is multi-tiered warehouse. With these applications, one Filter (UKF), has been used in agressive flight control for quadrotors in Loianno et al. (2017). Although the EKF aa multi-tiered warehouse. With these applications, one major control concernto issupport the the load. its derivative methods see widespread use, there is drones and systems theseof drones, suchThe as and a multi-tiered warehouse. Withnature these applications, one and its derivative methods see widespread use, there is the possibility of divergence. major control concern is the nature of the load. The quadrotors in Loianno et al.see (2017). Although EKF and possibility its derivative methods widespread use,the there is major control concern is the nature of the load. The variable nature warehouse. of the requested goods leads to an infinite the of divergence. a multi-tiered With these applications, one major control concern is the nature of the load. The the possibility of divergence. variable nature of the requested goods leads to an infinite and its derivative methods see widespread use,controller there is the possibility of divergence. variable nature of the requested goods leads to an infinite possible set of characteristics about the transported goods. The proposed solution in this paper is an LQR major concern is the nature of the load. The The proposed solution in this paper is an LQR controller variablecontrol nature of the requested goods leads to an infinite possible set of characteristics about thethe transported goods. possibility of divergence. possible set of characteristics about the transported goods. The solution in paper LQR The mass, inertia and position of item allinfinite affect the with a Multi-Model Adaptive using a variable nature of the requested goods leads to an possible set of characteristics about thethe transported goods. The proposed proposed solution in this this Estimator paper is is an an(MMAE) LQR controller controller The mass, inertia and position of item all affect with a Multi-Model Adaptive Estimator (MMAE) using The mass, inertia and position of the item all affect with a Multi-Model Adaptive Estimator (MMAE) using a the dynamics of the full system. Therefore, solutions bank of classical Kalman Filters. The sensors used are thea possible set of characteristics aboutof the transported goods. The solution in Filters. this Estimator paper issensors an(MMAE) LQR controller The dynamics mass, inertia andfull position the item all affect bank with proposed aofMulti-Model Adaptive using a the of the system. Therefore, solutions classical Kalman The used are the the dynamics of the full system. Therefore, solutions bank of classical Kalman Filters. The sensors used are the for control and estimation that are robust to uncertain gyroscope, which is usually available on-board these types The mass, and inertia andfull position of Therefore, the item all affect with aofMulti-Model Adaptive Estimator (MMAE) using the control dynamics ofestimation the system. solutions bank classical Kalman Filters. Theon-board sensors used are thea for that are robust to uncertain gyroscope, which is usually available these types for and estimation that robust gyroscope, which is usually available these types parameters are necessary. platforms, andKalman indoor multi-camera motion capture the dynamics the full system. solutions of bank of classical Filters. Theon-board sensors used are the for control control andof estimation that are are Therefore, robust to to uncertain uncertain gyroscope, which isan usually available on-board these types parameters are necessary. of platforms, and an indoor multi-camera motion capture parameters are necessary. of platforms, and an indoor multi-camera motion capture system. for control and necessary. estimation robust gyroscope, which on-board these types parameters are of platforms, and isanusually indooravailable multi-camera motion capture Control methods for dronesthat withare loads can to be uncertain found in system. Control methods for Robust drones with with loads can be behave found in system. parameters necessary. of platforms, an indoor multi-camera motion capture system. Control for drones loads can found in Notter etmethods al.are(2016). control methods been paper is and organized as follows: the problem addressed Control methods for drones with loads can be found in This Notter et al. (2016). Robust control methods have been This paper is organized as follows: the problem addressed system. Notter al. (2016). control methods have been This organized problem addressed previously considered for drones in works like Cabecinhas thispaper workis describedas infollows: Section the 2. The physical model Control for Robust drones with can found in in Notter et etmethods al. (2016). Robust control methods have been This paper isis organized as follows: the problem addressed previously considered for drones drones in loads works like be Cabecinhas in this work is described in Section 2. The physical model previously considered for in works like Cabecinhas in this work is described in Section 2. The physical et al. (2016), and there is plenty of research on these considered is presented in Section 3. The behavior of the Notter et al. (2016). Robust control methods have been This paper is organized as follows: the problem addressed previously considered for drones in works like Cabecinhas in this workisispresented describedininSection Section3.2. The The behavior physical model model et al. (2016), and there is plenty of research on these considered of the et al. (2016), and there is plenty of research on these considered is presented in Section The behavior of methods. However, robust control methods tend to inertia is analyzed in Section 4. The3. solution for estimation previously considered for drones in works like Cabecinhas in this work is described in Section 2. The physical model et al. (2016), and there is plenty of research on these considered is presented in Section 3. The behavior of the the methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution for estimation methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution estimation generate over conservative solutions. is presented in Section 5, Section and in 3. Section 6for the solution et al. (2016), and there is plenty of research on these considered is presented in The behavior of the methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution for estimation generate over over conservative conservative solutions. solutions. is presented in Section 5, and in Section the solution generate is presented in Section 5, and in the solution theiscontrol proposed. The 666verification of methods. However, robust controlthat methods tend to for inertia analyzed in Section 4. The solution for estimation generate over conservative solutions. is presented in problem Section 5,is and in Section Section the solution An alternative to Robust Control is also designed for the control problem is proposed. The verification of for the control problem is proposed. The verification of stability for the full proposed solution is presented in An alternative to Robust Control that is also designed generate over conservative solutions. is presented in Section 5, and in Section 6 the solution for the control problem is proposed. The verification of An alternative to Robust Control that also designed for handling unknown parameters is Multi-model stability for the full proposed solution is presented in An alternative to Robust Control that is also designed stability for the full proposed solution is presented in Section 7. Simulation results are presented and discussed for handling unknown parameters is Multi-model for the control problem is proposed. The verification of stability for the full proposed solution is presented in for handling unknown parameters is Multi-model methods. Multi-model methods forthat piecewise constant Section 7. Simulation results are presented and discussed An to Robust Control designed for alternative handling unknown parameters is also Multi-model Section 7. Simulation results are presented and discussed in Section 8. the In Section 9 the quadrotor model and its methods. Multi-model methods forbeen piecewise constant stability for full proposed solution is presented in Section 7. Simulation results are presented and discussed methods. Multi-model methods for piecewise constant unknown mass of quadrotors have considered and in Section 8.presented. In Section SectionAdditionally, the quadrotor quadrotor model and and its its for handling unknown parameters isconsidered Multi-model methods. Multi-model methods forbeen piecewise constant in Section 8. In 999 the model sensors areSimulation the implementation unknown mass of quadrotors have and 7. results arequadrotor presented and discussed in Section 8.presented. In Section the model and its unknown of quadrotors have been tested in mass Outeiro et al. (2018a,b). In considered Outeiroconstant et and al. Section sensors are Additionally, the implementation methods. Multi-model methods for piecewise unknown mass of quadrotors have been considered and sensors are presented. Additionally, the implementation details are discussed in this section. The experimental tested inthe Outeiro et relied al. (2018a,b). (2018a,b). In Outeiro Outeiro et al. al. in Section In Section 9 thesection. quadrotor model and its sensors are 8.discussed presented. Additionally, the implementation tested Outeiro et al. In et (2018a), method onhave a Multi-Model Adaptive are in this The experimental unknown mass of quadrotors been tested in inthe Outeiro et relied al. (2018a,b). In considered Outeiro et and al. details details are discussed in this section. The experimental results are presented and analyzed in the Section 10. Finally, (2018a), method on a Multi-Model Adaptive sensors are presented. Additionally, implementation details are discussed in this section. The experimental (2018a), the method relied on a Multi-Model Adaptive Estimator (MMAE) bank of Integrative Kalman are presented and analyzed in Section 10. Finally, tested Outeiro al. a (2018a,b). In Outeiro et al. results (2018a),inthe methodetwith relied on a Multi-Model Adaptive results are and analyzed in 10. some concluding remarks are drawn. Estimator (MMAE) with bank of Integrative Integrative Kalman are presented discussed in this section. The experimental results are presented and analyzed in Section Section 10. Finally, Finally, Estimator (MMAE) aaa on bank of Kalman Filters and an LQRwith controller with integrativeAdaptive action. details some concluding remarks are drawn. (2018a), the method relied a Multi-Model Estimator (MMAE) with bank of Integrative Kalman some concluding remarks are drawn. Filters and an LQRhad controller with integrative action. results are presented and are analyzed some concluding remarks drawn.in Section 10. Finally, Filters and an LQR controller with integrative action. Because the system multiple Kalman Filters with an Estimator with a bankwith of Integrative Kalman Filters and an LQRhad controller integrative action. 2. PROBLEM Because the(MMAE) system multiple Kalman Filters with an some concluding remarks are STATEMENT drawn. Because the system had Kalman Filters with an integrative mechanism formultiple gravitational force, it provided 2. PROBLEM STATEMENT Filters and an LQR controller with integrative action. 2. PROBLEM STATEMENT Because the system had multiple Kalman Filters with an integrative mechanism for gravitational force, it provided 2. PROBLEM STATEMENT integrative mechanism for gravitational force, it provided reduced state-estimation error. InKalman Outeiro et al. (2018b), Because themechanism system hadformultiple Filters with an integrative gravitational force, it provided Iz ψ¨¨ = STATEMENT f (u) (1) reduced state-estimation error. In Outeiro Outeiro et al. al. (2018b), 2. PROBLEM reduced state-estimation error. In et (2018b), the method relied on afor Multi-Model Adaptive Controller IIz ψ ff (u) (1) ¨¨ = integrative mechanism gravitational force, it provided reduced state-estimation error. In Outeiro et al. (2018b), ψ = (u) (1) z the method relied on a Multi-Model Adaptive Controller ψ = f (u) is shown in (1), where (1) the method relied on aathe Multi-Model Adaptive Controller (MMAC), extending multi-model framework from The yaw dynamics of aIzquadrotor reduced state-estimation error. In Outeiro et al. (2018b), the method relied on Multi-Model Adaptive Controller (MMAC), extending the multi-model framework from Izquadrotor ψ¨ = f (u) is (1) The yaw dynamics of a shown in (1), where (MMAC), extending the multi-model framework from Outeiro et al. (2018a) to the control. Because of the use The yaw dynamics of a quadrotor is shown in (1), where its z-axis is inertia, uinis (1), the where z-axis is yaw the yaw angle, of Iz aisquadrotor the method relied on athe Controller (MMAC), extending multi-model framework Outeiro et al. al. (2018a) toMulti-Model the control.Adaptive Because of the thefrom use ψ The dynamics shown is its z-axis inertia, u is the z-axis ψ is the yaw angle, I Outeiro et (2018a) to the control. Because of use its z-axis inertia, u the z-axis is the angle, IIzz ais moment. Since the angular is related (MMAC), extending the multi-model framework from Outeiro et al. (2018a) to the control. Because of theunder use ψ This work was supported by FCT, through IDMEC, The dynamics of shown where itsacceleration z-axis is inertia, uinis is (1), thelinearly z-axis ψ is yaw the yaw yaw angle, z isquadrotor moment. Since the angular acceleration is related linearly This work was supported by FCT, through IDMEC, under moment. Since the angular acceleration is related linearly to the provided moment, this is a simple problem to solve. Outeiro et al. (2018a) to the control. Because of the use LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT This work was supported by FCT, through IDMEC, under This work was supported by FCT, through IDMEC, under is its z-axis inertia, u is the z-axis ψ is the yaw angle, I moment. Since the angular acceleration is related linearly z to the provided moment, this is aa the simple problem to solve. LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT to the provided moment, this is simple problem to solve. However, this is not as simple for problem of unknown -LAETA, PTDC/EEI-AUT/5048/2014. project UID/EMS/50022/2019 and project LOTUS of FCT moment. Since the angular acceleration is related linearly to the provided moment, this isfora the simple problem to solve. LAETA, project UID/EMS/50022/2019 project IDMEC, LOTUS ofunder FCT This work was supported by FCT,and through However, this is not as simple problem of unknown -- PTDC/EEI-AUT/5048/2014. However, this is not as simple for the problem of unknown PTDC/EEI-AUT/5048/2014. to the provided moment, this is a simple problem to solve. However, this is not as simple for the problem of unknown - PTDC/EEI-AUT/5048/2014. LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT Copyright © 2019 IFAC 170 However, this is not as simple for the problem of unknown -2405-8963 PTDC/EEI-AUT/5048/2014. © 2019. The Authors. Published by Elsevier Ltd. 170 All rights reserved. CopyrightCopyright © 2019 IFAC Copyright © 2019 IFAC 170 Copyright 2019responsibility IFAC 170Control. Peer review ©under of International Federation of Automatic 10.1016/j.ifacol.2019.11.193 Copyright © 2019 IFAC 170
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
load transportation. Taking into account the added inertia from the load Il , results in (2) (Iz + Il )ψ¨ = f (u, Il ) In this case, the solution for the estimation is not as immediate. Since there is a multiplicative factor that is unknown, the performance of the estimation could degrade when using classical estimation methods. As the error of the unknown parameter versus its assumed value increases, the state-estimation error would also increase. Additionally, only a lower bound for the inertia of the system can be known a priori, coinciding with the noload case. Therefore, an alternative to classical estimation methods is necessary. 3. PHYSICAL MODEL Consider the dynamics of a quadrotor as shown in Mahony et al. (2012). Isolating the yaw dynamics, the resulting behavior can be described by: Iz ψ¨ = au + (Ix − Iy ) θ˙φ˙ (3) Where ψ, θ and φ are the yaw, pitch and roll angles, Ix , Iy and Iz are the x, y, z axis inertia in the body frame, u is the provided z-axis angular moment and a is an input parameter. It is not intended in this paper to approach the ˙ problem, and therefore it will Coriolis effect ((Ix − Iy ) θ˙φ) be removed for the design of the proposed solution. The resulting dynamics are as follows: (4) Iz ψ¨ = au There are measurements available for both the angle and the angular rate, resulting in the following state-space representation for the problem: 0 0 ψ˙ a/Iz ψ¨ = + u 0 10 ψ ψ˙ x A B ˙ x (5) 10 ψ= x 01 C
That will be used in the remainder of the paper. 4. INERTIA BEHAVIOR ANALYSIS
Unlike the mass of a quadrotor, as was studied in Outeiro et al. (2018a,b), the inertia of a system depends on the relative position of its components. To perform system identification for the inertia, it cannot have high frequency variations during the test. The rotation of the rotors provides high speed variations. However, its relative center of mass remains constant. Assuming the rotor blades to be approximately flat in the z-plane, by the Parallel Axis Theorem the rotation does not affect the z-axis inertia. The position of the load could also be an issue, but to minimize effects on the x and y rotational behavior, the load is considered as being placed above the center of mass of the quadrotor, removing the distance in the z-axis of the load from the center of mass, thus making it constant and known a priori. 5. ESTIMATION In this section the estimation solution is discussed. First the multiple-model estimation framework is presented, 171
171
followed by the Baram Proximity Measure (BPM), which provides a measure used in model selection. 5.1 Multiple-Model Adaptive Estimation The Multiple-Model Adaptive Estimation (MMAE) algorithm [Fekri (2002)] is a combined state-estimation and system identification method. Its uses include providing a solution for parametric uncertainties and for non-linear state-estimation (using different liberalizations). As the name implies, it relies on multiple models for the same system, but assuming different values of the unknown parameter (or linearity points). For each of these underlying models a Kalman filter is designed, providing accurate estimates for its assumed model. The merging and processing of the information is provided by the bank of filters. In this algorithm, the Bayesian Posterior Probability Estimator (PPE) selects the most accurate filter. The PPE assesses the accuracy through the residues of the known sensory data, by assigning a probability to each filter. The initial value of these probabilities are known as the a priori probabilities and are commonly initialized equal for the n filters (1/n), unless there is a priori knowledge to support higher or lower probability at start. The posterior probabilities pprob k (t + 1), can be computed iteratively using the past probabilities pprob k (t) and residues of the filters r i according to (6-8) [Chang and Athans (1978)], where h represents the number of sensors used. The residual covariance matrix of each filter (S i ) is used as a weighting parameter in the calculations. β i (t) is a weighting parameter based on the residual covariance and number of sensors, and wi (t) is a quadratic weighting parameter for the residue which also uses the residual covariance. 1 β (t + 1)e− 2 wk (t+1) pprob k (t + 1) = n k p (t) (6) − 12 wj (t+1) prob k j=1 β j (t + 1)e 1 (7) β i (t + 1) = h (2π) 2 detS i (t + 1) (8) wi (t + 1) = r (t + 1)S −1 i (t + 1)r(t + 1) Given the method for updating the probabilities, the PPE will cause the probability of the filter model with the least residue to tend to one. This means that it selects the most accurate model and uses exclusively its data for estimation. Additionally, an error in the estimation will always be incurred if the real value of the parameter does not match the value assumed by any of the models. The number of filters has to be selected taking into account the estimation error when the system does not match any of the models, while also considering the added computational weight of using more filters, as will be discussed in section 5.2. The state estimation of the MMAE algorithm can be obtained with a switching or weighted average, see Fekri (2002) for details. In switching the state estimation matches that of the filter with the highest posterior probability. In the weighted average the state-estimation of all filters is averaged using the posterior probabilities as a weighting factor, as shown in (9). In this work, it
2019 IFAC ACA 172 August 27-30, 2019. Cranfield, UK
u s
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
150
x ˆ1 KF1 r1
100
x ˆ2 Weighted Average
.. .
50
x ˆT
BPM
KF2 r2
x ˆn KFn rn PPE
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
-50 -100
pprob
-150
Fig. 1. MMAE Structure. was adopted the use of the average weight, as it provides low pass filtered state estimates. n ˆT = x pprob j (t)ˆ xj (9) j=1
The resulting structure is depicted in Fig. 1. This method is required to select the real (or closest) value of the parameter. Therefore, a convergence condition must be met for the probability of the closest model, as shown in the following Theorem in Hassani et al. (2009): Theorem 5.1. Let i∗ ∈ {1, 2, ..., N } be an index of a parameter vector in κ and I := {1, 2, ..., N } \i∗ an index set. Suppose that there exists a positive constant T such that for all t ≥ 0 and all j ∈ I the following condition holds: 1 t+T (ωi∗ (τ ) − lnβj (τ ))dτ T t 1 t+T < (ωj (τ ) − lnβi∗ (τ ))dτ (10) T t Then, pi∗ (t) satisfies (11) lim pi∗ (t) = 1 t→∞
Conversely, if (11) is observed, then there exists a positive constant T such that for all t ≥ 0 and all j ∈ I 1 t+T (ωi∗ (τ ) − lnβj (τ ))dτ T t 1 t+T ≤ (ωj (τ ) − lnβi∗ (τ ))dτ 1 (12) T t
This theorem is presented and proven in Hassani et al. (2009). Although in this work the study was made for the CT-MMAE (continuous time), the findings still hold true for the discrete equivalent. 5.2 Baram Proximity Measure The Baram Proximity Measure (BPM) is a measure proposed in Baram (1976) by Yoram Baram for deciding which models to use in multi-model methods. Its use has been considered in both Fekri (2002) and Gaspar et al. (2015). The BPM provides an adequate distance metric for stochastic systems between the real model and the Kalman Filter with its assumed model. The calculation
1
0
Note that in (12) the inequality is not strict.
172
-200
4
4.5
5 Iz (kg m2 )
5.5
6 ×10−3
Fig. 2. BPM Curves of the BPM is presented in (13), where S is the residual covariance matrix of the filter and r is the residue obtained from using the filter. BP M = log(|S|) + tr(S −1 × γ) r ∗ r γ= length(r)2
(13)
To perform the model selection using this measure, it is necessary to set the desired range for the unknown parameter and the maximum allowed value of BPM. The first model can be selected by increasing the parameter until the BPM of the minimum value of the range does not exceed the maximum. Afterwards, the following models will be required to ensure that the minimum BPM curve of the current and previous models does not exceed the maximum. Finally, models will be added until the minimum BPM curve of all models does not exceed the maximum. Performing this analysis for the problem addressed in this paper with only a yaw measurement yields the results in Fig. 2. Here, it can be observed that, for a maximum BPM of 120 and for the range of values presented, the obtained number of models is six and the inertias of the models are as presented in the legend of the figure. 6. CONTROL In this section the proposed controller is presented and the requirement of zero steady-state error is addressed. The proposed control solution is an LQR controller. In the absence of exterior perturbations it should suffice for the purpose of control. A major requirements for the control is ensuring zero static error, and in this case a classical LQR controller achieves this condition. Given the closed loop transfer function for the system with a classical LQR controller with sample time T , and gains Kψ and Kψ˙ in (14), the steady-state gain is one, and the controller provides zero static error. aT 2 Kψ ψ = ψref Iz (z − 1) + aT Kψ˙ (z − 1) + aT 2 Kψ
(14)
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
7. STABILITY VERIFICATION The stability of the proposed control system architecture is central to the operation of autonomous aerial vehicles in the presence of unknown constant parameters. To analyse this issue, the following lemma from Fekri (2002) is instrumental: Lemma 7.1. In the case where the real system has an unknown constant parameter that matches the underlying model of one of the filters in the filter bank, the corresponding posterior probability will tend to one and all the other probabilities will go to zero. Thus the following lemma can be stated: Lemma 7.2. For a system in the conditions of the previous lemma and based on the Separation Theorem, there is a finite time instant Tf in such that, for t > Tf in , all variables of the closed loop control system are bounded and the yaw rate error converges to zero. The sketch of the lemma is based on the assumptions previously outlined, namely assuming that one Kalman filter is based on an underlying model with the correct parameter and the eigenvalues of the controller and the estimator are recovered. The resulting A matrix of the system is presented in (15), where T is the sample time, Im is the model inertia, Ir is the real inertia, L are the Kalman gains and K are the controller gains. In the present case, the design inertia was 5.1×10−3 kg m2 , and the real inertia was 4.8 × 10−3 kg m2 . The obtained eigenvalues were eig = {0.9902, 0.9445 ± 0.0569i, 0.2114}. As expected, all have absolute value under one and thus the overall closed loop system is stable. Kψ˙ T a Kψ T a − 1 0 − Ir Ir 2 2 Kψ˙ T a Kψ T a T 1 − − 2Ir 2Ir (15) Kψ˙ T a Kψ T a L11 L12 1 − − L −L − 11 12 Im Im 2 2 Kψ˙ T a Kψ T a L21 L22 T − L21 − 1− − L22 2Im 2Im 8. SIMULATION RESULTS In this section, the preparation and results of the simulation are presented.
rotation. The estimation error zeroes after 2 seconds in the no load case and 3 seconds in the load case. These results are expected as the load case is a non-matching case (a case where the real load does not match a model load) and the no load case is a matching case. The probabilities only evolved during the rotations and the maximum probability was attributed to the closest model. The inertia estimate for the no load case converged to the correct value, because it is a matching case. In the load case, the inertia had an error, as it is a non-matching case. 9. IMPLEMENTATION In this section the experimental set-up is presented. The model that was used for testing is the Parrot Ar.Drone 2.0 by Parrot SA. It is an off-the-shelf commercially available, general purpose drone designed for users without any drone piloting skills. It is capable of recording video and is a good starter drone, due to its stability. This model is equipped with an Inertial Measurement Unit (IMU) composed of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer. Other sensors are available on board such as an ultrasound sensor, a barometer and two cameras. One normal camera on the front and an optical-flow camera on the bottom. For the purpose of implementing the control system, the AR.Drone 2.0 Quadcopter Embedded Coder [Lee (2017)] was used, as it provided a Simulink based environment for development of software to run in the quadrotor and allows direct access to its sensors and actuators. The controller and estimator from Section 8 were transferred onto the Embedded Coder Simulink environment. To ensure the stabilization of the quadrotor, the X and Y position and angles were regulated using a cascading PID control system for defining the necessary torques. Additionally, the height was also controlled using a PID controller. The sensors used were an indoor multi-camera motion capture system, that provides the ground truth for the position and orientation, and the gyroscope, that provides the angular rates. The setup of the experiment is presented in Fig. 3.
To validate if the proposed solution could work, a simulation was prepared. The parameters of the drone used for its setup are an inertia Iz of 4.8334 × 10−3 kg m2 and an input factor a of 1. The inertia after adding a load was 5.9663 × 10−4 kg m2 . The models used were the ones presented in Section 5.2. For the purpose of the Kalman gain calculations, the covariance of the sensor noise was defined as 1 for the yaw rate, and as 10−5 for the yaw, while the process noise was given a covariance of 10−6 . The LQR gains were calculated using a Q = [1 0; 0 1] and R = 1. The results of the simulation are presented in Fig. 4 and 5. The settling times (5%) with 360o rotations are at 3.5 seconds in both tests. There was no overshoot in both tests. The actuation only saturated during the start of each 173
173
Fig. 3. Experimental setup.
2019 IFAC ACA 174 August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
8
0.02 no load
load
ref
0.01 u (Nm)
ψ (rad)
6 4 2 0
0 -0.01
0
10
20
30 40 Time (s)
50
-0.02
60
no load load
0
20
40
0.1
0.04
yaw rate yaw
0.02 0.05 r
r
0 -0.02
0 yaw rate yaw
-0.04 -0.06
0
20
40
-0.05
60
0
20
Time (s) 4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
0
10
20
30 40 Time (s)
50
p prob
p prob
6.4
×10−3
0
10
20
30 40 Time (s)
50
60
Fig. 5. Simulation results: first - (a) actuation for both tests, second - (b) state error for the load test, third - (c) posterior probabilities for the load test. no load load real no load real load
5.6 5.2 0
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
0.5
0
60
6
4.8
60
1
0.5
0
40 Time (s)
1
Iz (kg m2 )
60
Time (s)
10
20
30 40 Time (s)
50
For the purpose of the Kalman gain calculations, the covariance of the sensor noise was defined as 10−5 for the yaw and as 1 for the yaw rate, while the process noise was given a covariance of 10−5 . The LQR gains were calculated using Q = [1 0; 0 1] and R = 400.
60
Fig. 4. Simulation results: first - (a) yaw for both tests, second - (b) state error for the no load test, third (c) posterior probabilities for the no load test, fourth - (d) inertia estimate for both tests. 10. EXPERIMENTAL RESULTS In this section, the analysis of the results of the experiment are presented. To validate if the proposed solution worked, an experimental test was prepared. The real inertia Iz of the quadrotor is 4.8334 × 10−3 kg m2 . No test was performed with a load, as it was difficult to increase significantly the inertia without affecting the remainder of the dynamics. The models used were the ones presented in section 5.2. 174
The results of the test are presented in Fig. 6. The settling time (5%) with 180o rotations is at 5 seconds, but there is a small static error in some of the rotations, and a larger error (0.14 rad) in the last rotation. The actuation only saturated during the start of each rotation. The probabilities evolved in a similar fashion to what was observed in simulation, changing mostly during the rotations. The highest probability was attributed to the closest model. There were peaks in the probabilities of the other models at the start of the rotations, but this did not seem to affect the selection of the closest model. The inertia estimate had a low error, as it is a matching case. CONCLUSION This paper proposed and studied the application of Multiple-Model Kalman filtering and LQR control for transportation of unknown loads with quadrotors. The
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
of converging to the desired angle in simulation, while in testing there was static error. The Inertia estimation from the MMAE was capable of selecting the closest mass in the filter bank, and was mostly driven during the rotations. The inertia estimates had only an error in non-matching cases.
ψ (rad)
4 2 0
REFERENCES yaw
-2
0
20
40 60 Time (s)
ref
80
100
0.02
u (Nm)
0.01 0 -0.01 -0.02
0
20
40 60 Time (s)
80
100
0.5
0
p prob
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
-0.5
-1
6
0
20
40 60 Time (s)
Iz (kg m2 )
80
100
80
100
×10−3 estimate
real
5.5 5 4.5
175
0
20
40 60 Time (s)
Fig. 6. Experimental results: first - (a) yaw measurement, second - (b) actuation, third - (c) posterior probabilities, fourth - (d) inertia estimate. sensors used for the proposed solution were an indoor camera indoor multi-camera motion capture system and a gyroscope. The solution was studied in a simulation and experimentally using the Parrot Ar.Drone 2.0. The control and estimation systems provided very low settling times in simulation, while in testing it was much slower to ensure that the x, y and z controllers would be capable of accompanying the rotation. The controller was capable 175
Baram, Y. (1976). Information, Consistent Information and Dynamic System Identification. Ph.D. thesis, Massachusetts Institute of Technology. Cabecinhas, D., Naldi, R., Silvestre, C., Cunha, R., and Marconi, L. (2016). Robust landing and sliding maneuver hybrid controller for a quadrotor vehicle. IEEE Transactions on Control Systems Technology, 24(2), 400–412. doi:10.1109/TCST.2015.2454445. Chang, C.B. and Athans, M. (1978). State estimation for discrete systems with switching parameters. IEEE Transactions on Aerospace and Electronic Systems, AES-14(3), 418–425. doi:10.1109/TAES.1978.308603. Fekri, S. (2002). Robust Adaptive MIMO Control Using Multiple-Model Hypothesis Testing and Mixed-µ Synthesis. Ph.D. thesis, Instituto Superior T´ecnico. Gaspar, T., Oliveira, P., and Silvestre, C. (2015). Modelbased filters for 3-d positioning of marine mammals using ahrs- and gps-equipped uavs. IEEE Transactions on Aerospace and Electronic Systems, 51(4), 3307–3320. doi:10.1109/TAES.2015.140748. Hassani, V., Pedro Aguiar, A., Pascoal, A.M., and Athans, M. (2009). Further results on plant parameter identification using continuous-time multiple-model adaptive estimators. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, 7261–7266. doi:10.1109/CDC.2009.5400434. Lee, D. (2017). Ar.drone 2.0 support from embedded coder. https://www.mathworks.com/hardwaresupport/ar-drone.html. Loianno, G., Brunner, C., McGrath, G., and Kumar, V. (2017). Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and imu. IEEE Robotics and Automation Letters, 2(2), 404– 411. Mahony, R., Kumar, V., and Corke, P. (2012). Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor. IEEE Robotics Automation Magazine, 19(3), 20–32. doi:10.1109/MRA.2012.2206474. Notter, S., Heckmann, A., Mcfadyen, A., and Gonzalez, L. (2016). Modelling, simulation and flight test of a model predictive controlled multirotor with heavy slung load. volume 49. doi:10.1016/j.ifacol.2016.09.032. Outeiro, P., Cardeira, C., and Oliveira, P. (2018a). Lqr/mmae height control system of a quadrotor for constant unknown load transportation. In 2018 13th APCA International Conference on Automatic Control and Soft Computing (CONTROLO), 389–394. doi: 10.1109/CONTROLO.2018.8514545. Outeiro, P., Cardeira, C., and Oliveira, P. (2018b). Mmac height control system of a quadrotor for constant unknown load transportation. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 4192–4197. doi: 10.1109/IROS.2018.8594215.
ScienceDirect
IFAC PapersOnLine 52-12 (2019) 170–175
MMAE/LQR Yaw Control System of a MMAE/LQR Yaw Control System of a MMAE/LQR Yaw Control System of MMAE/LQR Yaw Control System of a a Quadrotor for Constant Unknown Inertia Quadrotor for Constant Unknown Inertia MMAE/LQR Yaw Control System of a Quadrotor for Constant Unknown Inertia Quadrotor for Constant Unknown Inertia Pedro Outeiro, Carlos Cardeira, Paulo Oliveira Quadrotor for Constant Unknown Inertia Pedro Outeiro, Carlos Cardeira, Paulo Oliveira
Pedro Pedro Outeiro, Outeiro, Carlos Carlos Cardeira, Cardeira, Paulo Paulo Oliveira Oliveira IDMEC,Pedro Instituto Superior T´ e cnico, Universidade deOliveira Lisboa, Portugal Outeiro, Carlos Cardeira, Paulo IDMEC, Instituto Superior T´ T´eecnico, cnico, Universidade de Lisboa, Lisboa, Portugal IDMEC, Instituto Superior Universidade de (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} IDMEC, Instituto Superior T´ecnico, Universidade de Lisboa, Portugal Portugal (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). IDMEC, Instituto Superior T´ecnico, Universidade de Lisboa, Portugal (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). @tecnico.ulisboa.pt). (e-mail: {pedro.outeiro, carlos.cardeira, paulo.j.oliveira} @tecnico.ulisboa.pt). @tecnico.ulisboa.pt). Abstract: This paper presents a methodology for angular control of a quadrotor that transports Abstract: paper presents aa methodology for angular aa quadrotor transports This paper presents for angular control of that transports aAbstract: constant This unknown given the estimates both control inertia of angular that velocity, based Abstract: This paperload, presents a methodology methodology for on angular control ofand a quadrotor quadrotor that transports a constant unknown load, given the estimates on both inertia and angular velocity, based a constant unknown load, given the estimates on both inertia and angular velocity, based on measurements from an indoor multi-camera motion capture system and a gyroscope. The Abstract: This paper presents a methodology formotion angular control ofand a quadrotor that transports a constant unknown load, given the estimates on bothcapture inertiasystem angular velocity, based on measurements from an indoor multi-camera and a gyroscope. The on measurements from an indoor multi-camera motion capture system and a gyroscope. The proposed control method is an LQR controller and the proposed estimation method is aonconstant unknown load, given the estimates onand both inertia and estimation angular velocity, based measurements from an indoor multi-camera motion capture system and a gyroscope. Theaa proposed control method is an LQR controller the proposed method is proposed control method is an LQR controller and the proposed estimation method is aa Multi-Model Adaptive Estimator (MMAE). Themotion control system obtained is validated both in on measurements from an indoor multi-camera capture system and a gyroscope. The proposed control method is an LQR controller and the proposed estimation method is Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. proposed control method is anresorting LQR controller and thesystem proposed estimation method is in a Multi-Model Adaptive Estimator (MMAE). The control obtained is validated both simulation and experimentally, to an off-the-shelf commercially available quadrotor. simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. Multi-Model Adaptive Estimator (MMAE). The control system obtained is validated both in simulation and experimentally, resorting to an off-the-shelf commercially available quadrotor. Copyright © 2019. The Authors. Published by Elsevier Ltd. All rights reserved. Keywords: mobile resorting robots, Estimation algorithms, Parameteravailable identification, simulation Autonomous and experimentally, to an off-the-shelf commercially quadrotor. Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Embedded systems Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Embedded systems Sensor Embedded systems Keywords: Autonomous mobile robots, Estimation algorithms, Parameter identification, Sensor Fusion, Fusion, Embedded systems 1. INTRODUCTION of a bank of combined filters and controllers, this method Sensor Fusion, Embedded systems 1. INTRODUCTION INTRODUCTION of aa bank of combined method 1. of of filters and controllers, this method achieved settling filters times and thancontrollers, Outeiro et this al. (2018a). 1. INTRODUCTION of a bank banklower of combined combined filters and controllers, this method achieved lower settling times than Outeiro et al. (2018a). lower settling times than Outeiro et al. (2018a). The rising interest1.inINTRODUCTION drones in recent years has driven a achieved of a bank of combined filters and controllers, this method achieved lower settling times than Outeiro et al. (2018a). A more common method for handling non-linear and The rising interest in in drones drones in recent years has driven The rising interest in recent years has driven new set of applications for these unmanned aerial vehiclesaaa A more common method for handling non-linear and achieved lower settling times than Outeiro et al. (2018a). The rising interest in drones in recent years has driven A more common method for handling non-linear and parameter uncertainty is the Extended Kalman Filter new set Among of applications applications for these these unmanned unmanned aerialone vehicles A more common method for Extended handling non-linear and new set of for aerial vehicles (UAV). these applications, a noteworthy is in parameter uncertainty is the Kalman Filter The rising interest in applications, drones in recent yearsaerial has one driven a (EKF). new set of applications for these unmanned vehicles parameter uncertainty is the Extended Kalman Filter A derivative of the EKF, the Unscented Kalman (UAV). Among these a noteworthy is in A more common method for handling non-linear and parameter uncertainty is the Extended Kalman Filter (UAV). Among these applications, a noteworthy one is in delivery systems. Amazon is currently developing delivery (EKF). A derivative derivative of used the EKF, EKF, the Unscented Unscented Kalman new set Among of applications for is these unmanned aerialone vehicles (UAV). these applications, a noteworthy is in (EKF). A of the the Kalman Filter (UKF), has been in agressive flight control for delivery systems. Amazon currently developing delivery parameter uncertainty is the Extended Kalman Filter (EKF). A derivative of used the EKF, the Unscented Kalman delivery systems. Amazon is currently developing delivery drones and systems to support these drones, such as Filter (UKF), has been in agressive flight control for (UAV). Among these applications, a noteworthy one is as in quadrotors delivery and systems. Amazon is currently developing delivery Filter (UKF), has been used in agressive flight control for in Loianno et al. (2017). Although the EKF drones systems to support these drones, such (EKF). A derivative of the EKF, the Unscented Kalman Filter (UKF), has been used in agressive flight control for drones and systems to support these drones, such as adelivery multi-tiered warehouse. With these applications, one quadrotors in Loianno et al. (2017). Although the EKF systems. Amazon is currently developing delivery drones and systems to support these drones, such as quadrotors in Loianno et al. (2017). Although the EKF and its derivative methods see widespread use, there is multi-tiered warehouse. With these applications, one Filter (UKF), has been used in agressive flight control for quadrotors in Loianno et al. (2017). Although the EKF aa multi-tiered warehouse. With these applications, one major control concernto issupport the the load. its derivative methods see widespread use, there is drones and systems theseof drones, suchThe as and a multi-tiered warehouse. Withnature these applications, one and its derivative methods see widespread use, there is the possibility of divergence. major control concern is the nature of the load. The quadrotors in Loianno et al.see (2017). Although EKF and possibility its derivative methods widespread use,the there is major control concern is the nature of the load. The variable nature warehouse. of the requested goods leads to an infinite the of divergence. a multi-tiered With these applications, one major control concern is the nature of the load. The the possibility of divergence. variable nature of the requested goods leads to an infinite and its derivative methods see widespread use,controller there is the possibility of divergence. variable nature of the requested goods leads to an infinite possible set of characteristics about the transported goods. The proposed solution in this paper is an LQR major concern is the nature of the load. The The proposed solution in this paper is an LQR controller variablecontrol nature of the requested goods leads to an infinite possible set of characteristics about thethe transported goods. possibility of divergence. possible set of characteristics about the transported goods. The solution in paper LQR The mass, inertia and position of item allinfinite affect the with a Multi-Model Adaptive using a variable nature of the requested goods leads to an possible set of characteristics about thethe transported goods. The proposed proposed solution in this this Estimator paper is is an an(MMAE) LQR controller controller The mass, inertia and position of item all affect with a Multi-Model Adaptive Estimator (MMAE) using The mass, inertia and position of the item all affect with a Multi-Model Adaptive Estimator (MMAE) using a the dynamics of the full system. Therefore, solutions bank of classical Kalman Filters. The sensors used are thea possible set of characteristics aboutof the transported goods. The solution in Filters. this Estimator paper issensors an(MMAE) LQR controller The dynamics mass, inertia andfull position the item all affect bank with proposed aofMulti-Model Adaptive using a the of the system. Therefore, solutions classical Kalman The used are the the dynamics of the full system. Therefore, solutions bank of classical Kalman Filters. The sensors used are the for control and estimation that are robust to uncertain gyroscope, which is usually available on-board these types The mass, and inertia andfull position of Therefore, the item all affect with aofMulti-Model Adaptive Estimator (MMAE) using the control dynamics ofestimation the system. solutions bank classical Kalman Filters. Theon-board sensors used are thea for that are robust to uncertain gyroscope, which is usually available these types for and estimation that robust gyroscope, which is usually available these types parameters are necessary. platforms, andKalman indoor multi-camera motion capture the dynamics the full system. solutions of bank of classical Filters. Theon-board sensors used are the for control control andof estimation that are are Therefore, robust to to uncertain uncertain gyroscope, which isan usually available on-board these types parameters are necessary. of platforms, and an indoor multi-camera motion capture parameters are necessary. of platforms, and an indoor multi-camera motion capture system. for control and necessary. estimation robust gyroscope, which on-board these types parameters are of platforms, and isanusually indooravailable multi-camera motion capture Control methods for dronesthat withare loads can to be uncertain found in system. Control methods for Robust drones with with loads can be behave found in system. parameters necessary. of platforms, an indoor multi-camera motion capture system. Control for drones loads can found in Notter etmethods al.are(2016). control methods been paper is and organized as follows: the problem addressed Control methods for drones with loads can be found in This Notter et al. (2016). Robust control methods have been This paper is organized as follows: the problem addressed system. Notter al. (2016). control methods have been This organized problem addressed previously considered for drones in works like Cabecinhas thispaper workis describedas infollows: Section the 2. The physical model Control for Robust drones with can found in in Notter et etmethods al. (2016). Robust control methods have been This paper isis organized as follows: the problem addressed previously considered for drones drones in loads works like be Cabecinhas in this work is described in Section 2. The physical model previously considered for in works like Cabecinhas in this work is described in Section 2. The physical et al. (2016), and there is plenty of research on these considered is presented in Section 3. The behavior of the Notter et al. (2016). Robust control methods have been This paper is organized as follows: the problem addressed previously considered for drones in works like Cabecinhas in this workisispresented describedininSection Section3.2. The The behavior physical model model et al. (2016), and there is plenty of research on these considered of the et al. (2016), and there is plenty of research on these considered is presented in Section The behavior of methods. However, robust control methods tend to inertia is analyzed in Section 4. The3. solution for estimation previously considered for drones in works like Cabecinhas in this work is described in Section 2. The physical model et al. (2016), and there is plenty of research on these considered is presented in Section 3. The behavior of the the methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution for estimation methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution estimation generate over conservative solutions. is presented in Section 5, Section and in 3. Section 6for the solution et al. (2016), and there is plenty of research on these considered is presented in The behavior of the methods. However, robust control methods tend to inertia is analyzed in Section 4. The solution for estimation generate over over conservative conservative solutions. solutions. is presented in Section 5, and in Section the solution generate is presented in Section 5, and in the solution theiscontrol proposed. The 666verification of methods. However, robust controlthat methods tend to for inertia analyzed in Section 4. The solution for estimation generate over conservative solutions. is presented in problem Section 5,is and in Section Section the solution An alternative to Robust Control is also designed for the control problem is proposed. The verification of for the control problem is proposed. The verification of stability for the full proposed solution is presented in An alternative to Robust Control that is also designed generate over conservative solutions. is presented in Section 5, and in Section 6 the solution for the control problem is proposed. The verification of An alternative to Robust Control that also designed for handling unknown parameters is Multi-model stability for the full proposed solution is presented in An alternative to Robust Control that is also designed stability for the full proposed solution is presented in Section 7. Simulation results are presented and discussed for handling unknown parameters is Multi-model for the control problem is proposed. The verification of stability for the full proposed solution is presented in for handling unknown parameters is Multi-model methods. Multi-model methods forthat piecewise constant Section 7. Simulation results are presented and discussed An to Robust Control designed for alternative handling unknown parameters is also Multi-model Section 7. Simulation results are presented and discussed in Section 8. the In Section 9 the quadrotor model and its methods. Multi-model methods forbeen piecewise constant stability for full proposed solution is presented in Section 7. Simulation results are presented and discussed methods. Multi-model methods for piecewise constant unknown mass of quadrotors have considered and in Section 8.presented. In Section SectionAdditionally, the quadrotor quadrotor model and and its its for handling unknown parameters isconsidered Multi-model methods. Multi-model methods forbeen piecewise constant in Section 8. In 999 the model sensors areSimulation the implementation unknown mass of quadrotors have and 7. results arequadrotor presented and discussed in Section 8.presented. In Section the model and its unknown of quadrotors have been tested in mass Outeiro et al. (2018a,b). In considered Outeiroconstant et and al. Section sensors are Additionally, the implementation methods. Multi-model methods for piecewise unknown mass of quadrotors have been considered and sensors are presented. Additionally, the implementation details are discussed in this section. The experimental tested inthe Outeiro et relied al. (2018a,b). (2018a,b). In Outeiro Outeiro et al. al. in Section In Section 9 thesection. quadrotor model and its sensors are 8.discussed presented. Additionally, the implementation tested Outeiro et al. In et (2018a), method onhave a Multi-Model Adaptive are in this The experimental unknown mass of quadrotors been tested in inthe Outeiro et relied al. (2018a,b). In considered Outeiro et and al. details details are discussed in this section. The experimental results are presented and analyzed in the Section 10. Finally, (2018a), method on a Multi-Model Adaptive sensors are presented. Additionally, implementation details are discussed in this section. The experimental (2018a), the method relied on a Multi-Model Adaptive Estimator (MMAE) bank of Integrative Kalman are presented and analyzed in Section 10. Finally, tested Outeiro al. a (2018a,b). In Outeiro et al. results (2018a),inthe methodetwith relied on a Multi-Model Adaptive results are and analyzed in 10. some concluding remarks are drawn. Estimator (MMAE) with bank of Integrative Integrative Kalman are presented discussed in this section. The experimental results are presented and analyzed in Section Section 10. Finally, Finally, Estimator (MMAE) aaa on bank of Kalman Filters and an LQRwith controller with integrativeAdaptive action. details some concluding remarks are drawn. (2018a), the method relied a Multi-Model Estimator (MMAE) with bank of Integrative Kalman some concluding remarks are drawn. Filters and an LQRhad controller with integrative action. results are presented and are analyzed some concluding remarks drawn.in Section 10. Finally, Filters and an LQR controller with integrative action. Because the system multiple Kalman Filters with an Estimator with a bankwith of Integrative Kalman Filters and an LQRhad controller integrative action. 2. PROBLEM Because the(MMAE) system multiple Kalman Filters with an some concluding remarks are STATEMENT drawn. Because the system had Kalman Filters with an integrative mechanism formultiple gravitational force, it provided 2. PROBLEM STATEMENT Filters and an LQR controller with integrative action. 2. PROBLEM STATEMENT Because the system had multiple Kalman Filters with an integrative mechanism for gravitational force, it provided 2. PROBLEM STATEMENT integrative mechanism for gravitational force, it provided reduced state-estimation error. InKalman Outeiro et al. (2018b), Because themechanism system hadformultiple Filters with an integrative gravitational force, it provided Iz ψ¨¨ = STATEMENT f (u) (1) reduced state-estimation error. In Outeiro Outeiro et al. al. (2018b), 2. PROBLEM reduced state-estimation error. In et (2018b), the method relied on afor Multi-Model Adaptive Controller IIz ψ ff (u) (1) ¨¨ = integrative mechanism gravitational force, it provided reduced state-estimation error. In Outeiro et al. (2018b), ψ = (u) (1) z the method relied on a Multi-Model Adaptive Controller ψ = f (u) is shown in (1), where (1) the method relied on aathe Multi-Model Adaptive Controller (MMAC), extending multi-model framework from The yaw dynamics of aIzquadrotor reduced state-estimation error. In Outeiro et al. (2018b), the method relied on Multi-Model Adaptive Controller (MMAC), extending the multi-model framework from Izquadrotor ψ¨ = f (u) is (1) The yaw dynamics of a shown in (1), where (MMAC), extending the multi-model framework from Outeiro et al. (2018a) to the control. Because of the use The yaw dynamics of a quadrotor is shown in (1), where its z-axis is inertia, uinis (1), the where z-axis is yaw the yaw angle, of Iz aisquadrotor the method relied on athe Controller (MMAC), extending multi-model framework Outeiro et al. al. (2018a) toMulti-Model the control.Adaptive Because of the thefrom use ψ The dynamics shown is its z-axis inertia, u is the z-axis ψ is the yaw angle, I Outeiro et (2018a) to the control. Because of use its z-axis inertia, u the z-axis is the angle, IIzz ais moment. Since the angular is related (MMAC), extending the multi-model framework from Outeiro et al. (2018a) to the control. Because of theunder use ψ This work was supported by FCT, through IDMEC, The dynamics of shown where itsacceleration z-axis is inertia, uinis is (1), thelinearly z-axis ψ is yaw the yaw yaw angle, z isquadrotor moment. Since the angular acceleration is related linearly This work was supported by FCT, through IDMEC, under moment. Since the angular acceleration is related linearly to the provided moment, this is a simple problem to solve. Outeiro et al. (2018a) to the control. Because of the use LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT This work was supported by FCT, through IDMEC, under This work was supported by FCT, through IDMEC, under is its z-axis inertia, u is the z-axis ψ is the yaw angle, I moment. Since the angular acceleration is related linearly z to the provided moment, this is aa the simple problem to solve. LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT to the provided moment, this is simple problem to solve. However, this is not as simple for problem of unknown -LAETA, PTDC/EEI-AUT/5048/2014. project UID/EMS/50022/2019 and project LOTUS of FCT moment. Since the angular acceleration is related linearly to the provided moment, this isfora the simple problem to solve. LAETA, project UID/EMS/50022/2019 project IDMEC, LOTUS ofunder FCT This work was supported by FCT,and through However, this is not as simple problem of unknown -- PTDC/EEI-AUT/5048/2014. However, this is not as simple for the problem of unknown PTDC/EEI-AUT/5048/2014. to the provided moment, this is a simple problem to solve. However, this is not as simple for the problem of unknown - PTDC/EEI-AUT/5048/2014. LAETA, project UID/EMS/50022/2019 and project LOTUS of FCT Copyright © 2019 IFAC 170 However, this is not as simple for the problem of unknown -2405-8963 PTDC/EEI-AUT/5048/2014. © 2019. The Authors. Published by Elsevier Ltd. 170 All rights reserved. CopyrightCopyright © 2019 IFAC Copyright © 2019 IFAC 170 Copyright 2019responsibility IFAC 170Control. Peer review ©under of International Federation of Automatic 10.1016/j.ifacol.2019.11.193 Copyright © 2019 IFAC 170
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
load transportation. Taking into account the added inertia from the load Il , results in (2) (Iz + Il )ψ¨ = f (u, Il ) In this case, the solution for the estimation is not as immediate. Since there is a multiplicative factor that is unknown, the performance of the estimation could degrade when using classical estimation methods. As the error of the unknown parameter versus its assumed value increases, the state-estimation error would also increase. Additionally, only a lower bound for the inertia of the system can be known a priori, coinciding with the noload case. Therefore, an alternative to classical estimation methods is necessary. 3. PHYSICAL MODEL Consider the dynamics of a quadrotor as shown in Mahony et al. (2012). Isolating the yaw dynamics, the resulting behavior can be described by: Iz ψ¨ = au + (Ix − Iy ) θ˙φ˙ (3) Where ψ, θ and φ are the yaw, pitch and roll angles, Ix , Iy and Iz are the x, y, z axis inertia in the body frame, u is the provided z-axis angular moment and a is an input parameter. It is not intended in this paper to approach the ˙ problem, and therefore it will Coriolis effect ((Ix − Iy ) θ˙φ) be removed for the design of the proposed solution. The resulting dynamics are as follows: (4) Iz ψ¨ = au There are measurements available for both the angle and the angular rate, resulting in the following state-space representation for the problem: 0 0 ψ˙ a/Iz ψ¨ = + u 0 10 ψ ψ˙ x A B ˙ x (5) 10 ψ= x 01 C
That will be used in the remainder of the paper. 4. INERTIA BEHAVIOR ANALYSIS
Unlike the mass of a quadrotor, as was studied in Outeiro et al. (2018a,b), the inertia of a system depends on the relative position of its components. To perform system identification for the inertia, it cannot have high frequency variations during the test. The rotation of the rotors provides high speed variations. However, its relative center of mass remains constant. Assuming the rotor blades to be approximately flat in the z-plane, by the Parallel Axis Theorem the rotation does not affect the z-axis inertia. The position of the load could also be an issue, but to minimize effects on the x and y rotational behavior, the load is considered as being placed above the center of mass of the quadrotor, removing the distance in the z-axis of the load from the center of mass, thus making it constant and known a priori. 5. ESTIMATION In this section the estimation solution is discussed. First the multiple-model estimation framework is presented, 171
171
followed by the Baram Proximity Measure (BPM), which provides a measure used in model selection. 5.1 Multiple-Model Adaptive Estimation The Multiple-Model Adaptive Estimation (MMAE) algorithm [Fekri (2002)] is a combined state-estimation and system identification method. Its uses include providing a solution for parametric uncertainties and for non-linear state-estimation (using different liberalizations). As the name implies, it relies on multiple models for the same system, but assuming different values of the unknown parameter (or linearity points). For each of these underlying models a Kalman filter is designed, providing accurate estimates for its assumed model. The merging and processing of the information is provided by the bank of filters. In this algorithm, the Bayesian Posterior Probability Estimator (PPE) selects the most accurate filter. The PPE assesses the accuracy through the residues of the known sensory data, by assigning a probability to each filter. The initial value of these probabilities are known as the a priori probabilities and are commonly initialized equal for the n filters (1/n), unless there is a priori knowledge to support higher or lower probability at start. The posterior probabilities pprob k (t + 1), can be computed iteratively using the past probabilities pprob k (t) and residues of the filters r i according to (6-8) [Chang and Athans (1978)], where h represents the number of sensors used. The residual covariance matrix of each filter (S i ) is used as a weighting parameter in the calculations. β i (t) is a weighting parameter based on the residual covariance and number of sensors, and wi (t) is a quadratic weighting parameter for the residue which also uses the residual covariance. 1 β (t + 1)e− 2 wk (t+1) pprob k (t + 1) = n k p (t) (6) − 12 wj (t+1) prob k j=1 β j (t + 1)e 1 (7) β i (t + 1) = h (2π) 2 detS i (t + 1) (8) wi (t + 1) = r (t + 1)S −1 i (t + 1)r(t + 1) Given the method for updating the probabilities, the PPE will cause the probability of the filter model with the least residue to tend to one. This means that it selects the most accurate model and uses exclusively its data for estimation. Additionally, an error in the estimation will always be incurred if the real value of the parameter does not match the value assumed by any of the models. The number of filters has to be selected taking into account the estimation error when the system does not match any of the models, while also considering the added computational weight of using more filters, as will be discussed in section 5.2. The state estimation of the MMAE algorithm can be obtained with a switching or weighted average, see Fekri (2002) for details. In switching the state estimation matches that of the filter with the highest posterior probability. In the weighted average the state-estimation of all filters is averaged using the posterior probabilities as a weighting factor, as shown in (9). In this work, it
2019 IFAC ACA 172 August 27-30, 2019. Cranfield, UK
u s
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
150
x ˆ1 KF1 r1
100
x ˆ2 Weighted Average
.. .
50
x ˆT
BPM
KF2 r2
x ˆn KFn rn PPE
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
-50 -100
pprob
-150
Fig. 1. MMAE Structure. was adopted the use of the average weight, as it provides low pass filtered state estimates. n ˆT = x pprob j (t)ˆ xj (9) j=1
The resulting structure is depicted in Fig. 1. This method is required to select the real (or closest) value of the parameter. Therefore, a convergence condition must be met for the probability of the closest model, as shown in the following Theorem in Hassani et al. (2009): Theorem 5.1. Let i∗ ∈ {1, 2, ..., N } be an index of a parameter vector in κ and I := {1, 2, ..., N } \i∗ an index set. Suppose that there exists a positive constant T such that for all t ≥ 0 and all j ∈ I the following condition holds: 1 t+T (ωi∗ (τ ) − lnβj (τ ))dτ T t 1 t+T < (ωj (τ ) − lnβi∗ (τ ))dτ (10) T t Then, pi∗ (t) satisfies (11) lim pi∗ (t) = 1 t→∞
Conversely, if (11) is observed, then there exists a positive constant T such that for all t ≥ 0 and all j ∈ I 1 t+T (ωi∗ (τ ) − lnβj (τ ))dτ T t 1 t+T ≤ (ωj (τ ) − lnβi∗ (τ ))dτ 1 (12) T t
This theorem is presented and proven in Hassani et al. (2009). Although in this work the study was made for the CT-MMAE (continuous time), the findings still hold true for the discrete equivalent. 5.2 Baram Proximity Measure The Baram Proximity Measure (BPM) is a measure proposed in Baram (1976) by Yoram Baram for deciding which models to use in multi-model methods. Its use has been considered in both Fekri (2002) and Gaspar et al. (2015). The BPM provides an adequate distance metric for stochastic systems between the real model and the Kalman Filter with its assumed model. The calculation
1
0
Note that in (12) the inequality is not strict.
172
-200
4
4.5
5 Iz (kg m2 )
5.5
6 ×10−3
Fig. 2. BPM Curves of the BPM is presented in (13), where S is the residual covariance matrix of the filter and r is the residue obtained from using the filter. BP M = log(|S|) + tr(S −1 × γ) r ∗ r γ= length(r)2
(13)
To perform the model selection using this measure, it is necessary to set the desired range for the unknown parameter and the maximum allowed value of BPM. The first model can be selected by increasing the parameter until the BPM of the minimum value of the range does not exceed the maximum. Afterwards, the following models will be required to ensure that the minimum BPM curve of the current and previous models does not exceed the maximum. Finally, models will be added until the minimum BPM curve of all models does not exceed the maximum. Performing this analysis for the problem addressed in this paper with only a yaw measurement yields the results in Fig. 2. Here, it can be observed that, for a maximum BPM of 120 and for the range of values presented, the obtained number of models is six and the inertias of the models are as presented in the legend of the figure. 6. CONTROL In this section the proposed controller is presented and the requirement of zero steady-state error is addressed. The proposed control solution is an LQR controller. In the absence of exterior perturbations it should suffice for the purpose of control. A major requirements for the control is ensuring zero static error, and in this case a classical LQR controller achieves this condition. Given the closed loop transfer function for the system with a classical LQR controller with sample time T , and gains Kψ and Kψ˙ in (14), the steady-state gain is one, and the controller provides zero static error. aT 2 Kψ ψ = ψref Iz (z − 1) + aT Kψ˙ (z − 1) + aT 2 Kψ
(14)
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
7. STABILITY VERIFICATION The stability of the proposed control system architecture is central to the operation of autonomous aerial vehicles in the presence of unknown constant parameters. To analyse this issue, the following lemma from Fekri (2002) is instrumental: Lemma 7.1. In the case where the real system has an unknown constant parameter that matches the underlying model of one of the filters in the filter bank, the corresponding posterior probability will tend to one and all the other probabilities will go to zero. Thus the following lemma can be stated: Lemma 7.2. For a system in the conditions of the previous lemma and based on the Separation Theorem, there is a finite time instant Tf in such that, for t > Tf in , all variables of the closed loop control system are bounded and the yaw rate error converges to zero. The sketch of the lemma is based on the assumptions previously outlined, namely assuming that one Kalman filter is based on an underlying model with the correct parameter and the eigenvalues of the controller and the estimator are recovered. The resulting A matrix of the system is presented in (15), where T is the sample time, Im is the model inertia, Ir is the real inertia, L are the Kalman gains and K are the controller gains. In the present case, the design inertia was 5.1×10−3 kg m2 , and the real inertia was 4.8 × 10−3 kg m2 . The obtained eigenvalues were eig = {0.9902, 0.9445 ± 0.0569i, 0.2114}. As expected, all have absolute value under one and thus the overall closed loop system is stable. Kψ˙ T a Kψ T a − 1 0 − Ir Ir 2 2 Kψ˙ T a Kψ T a T 1 − − 2Ir 2Ir (15) Kψ˙ T a Kψ T a L11 L12 1 − − L −L − 11 12 Im Im 2 2 Kψ˙ T a Kψ T a L21 L22 T − L21 − 1− − L22 2Im 2Im 8. SIMULATION RESULTS In this section, the preparation and results of the simulation are presented.
rotation. The estimation error zeroes after 2 seconds in the no load case and 3 seconds in the load case. These results are expected as the load case is a non-matching case (a case where the real load does not match a model load) and the no load case is a matching case. The probabilities only evolved during the rotations and the maximum probability was attributed to the closest model. The inertia estimate for the no load case converged to the correct value, because it is a matching case. In the load case, the inertia had an error, as it is a non-matching case. 9. IMPLEMENTATION In this section the experimental set-up is presented. The model that was used for testing is the Parrot Ar.Drone 2.0 by Parrot SA. It is an off-the-shelf commercially available, general purpose drone designed for users without any drone piloting skills. It is capable of recording video and is a good starter drone, due to its stability. This model is equipped with an Inertial Measurement Unit (IMU) composed of a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer. Other sensors are available on board such as an ultrasound sensor, a barometer and two cameras. One normal camera on the front and an optical-flow camera on the bottom. For the purpose of implementing the control system, the AR.Drone 2.0 Quadcopter Embedded Coder [Lee (2017)] was used, as it provided a Simulink based environment for development of software to run in the quadrotor and allows direct access to its sensors and actuators. The controller and estimator from Section 8 were transferred onto the Embedded Coder Simulink environment. To ensure the stabilization of the quadrotor, the X and Y position and angles were regulated using a cascading PID control system for defining the necessary torques. Additionally, the height was also controlled using a PID controller. The sensors used were an indoor multi-camera motion capture system, that provides the ground truth for the position and orientation, and the gyroscope, that provides the angular rates. The setup of the experiment is presented in Fig. 3.
To validate if the proposed solution could work, a simulation was prepared. The parameters of the drone used for its setup are an inertia Iz of 4.8334 × 10−3 kg m2 and an input factor a of 1. The inertia after adding a load was 5.9663 × 10−4 kg m2 . The models used were the ones presented in Section 5.2. For the purpose of the Kalman gain calculations, the covariance of the sensor noise was defined as 1 for the yaw rate, and as 10−5 for the yaw, while the process noise was given a covariance of 10−6 . The LQR gains were calculated using a Q = [1 0; 0 1] and R = 1. The results of the simulation are presented in Fig. 4 and 5. The settling times (5%) with 360o rotations are at 3.5 seconds in both tests. There was no overshoot in both tests. The actuation only saturated during the start of each 173
173
Fig. 3. Experimental setup.
2019 IFAC ACA 174 August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
8
0.02 no load
load
ref
0.01 u (Nm)
ψ (rad)
6 4 2 0
0 -0.01
0
10
20
30 40 Time (s)
50
-0.02
60
no load load
0
20
40
0.1
0.04
yaw rate yaw
0.02 0.05 r
r
0 -0.02
0 yaw rate yaw
-0.04 -0.06
0
20
40
-0.05
60
0
20
Time (s) 4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
0
10
20
30 40 Time (s)
50
p prob
p prob
6.4
×10−3
0
10
20
30 40 Time (s)
50
60
Fig. 5. Simulation results: first - (a) actuation for both tests, second - (b) state error for the load test, third - (c) posterior probabilities for the load test. no load load real no load real load
5.6 5.2 0
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
0.5
0
60
6
4.8
60
1
0.5
0
40 Time (s)
1
Iz (kg m2 )
60
Time (s)
10
20
30 40 Time (s)
50
For the purpose of the Kalman gain calculations, the covariance of the sensor noise was defined as 10−5 for the yaw and as 1 for the yaw rate, while the process noise was given a covariance of 10−5 . The LQR gains were calculated using Q = [1 0; 0 1] and R = 400.
60
Fig. 4. Simulation results: first - (a) yaw for both tests, second - (b) state error for the no load test, third (c) posterior probabilities for the no load test, fourth - (d) inertia estimate for both tests. 10. EXPERIMENTAL RESULTS In this section, the analysis of the results of the experiment are presented. To validate if the proposed solution worked, an experimental test was prepared. The real inertia Iz of the quadrotor is 4.8334 × 10−3 kg m2 . No test was performed with a load, as it was difficult to increase significantly the inertia without affecting the remainder of the dynamics. The models used were the ones presented in section 5.2. 174
The results of the test are presented in Fig. 6. The settling time (5%) with 180o rotations is at 5 seconds, but there is a small static error in some of the rotations, and a larger error (0.14 rad) in the last rotation. The actuation only saturated during the start of each rotation. The probabilities evolved in a similar fashion to what was observed in simulation, changing mostly during the rotations. The highest probability was attributed to the closest model. There were peaks in the probabilities of the other models at the start of the rotations, but this did not seem to affect the selection of the closest model. The inertia estimate had a low error, as it is a matching case. CONCLUSION This paper proposed and studied the application of Multiple-Model Kalman filtering and LQR control for transportation of unknown loads with quadrotors. The
2019 IFAC ACA August 27-30, 2019. Cranfield, UK
Pedro Outeiro et al. / IFAC PapersOnLine 52-12 (2019) 170–175
of converging to the desired angle in simulation, while in testing there was static error. The Inertia estimation from the MMAE was capable of selecting the closest mass in the filter bank, and was mostly driven during the rotations. The inertia estimates had only an error in non-matching cases.
ψ (rad)
4 2 0
REFERENCES yaw
-2
0
20
40 60 Time (s)
ref
80
100
0.02
u (Nm)
0.01 0 -0.01 -0.02
0
20
40 60 Time (s)
80
100
0.5
0
p prob
4.14e-03 4.46e-03 4.83e-03 5.23e-03 5.66e-03 6.12e-03
-0.5
-1
6
0
20
40 60 Time (s)
Iz (kg m2 )
80
100
80
100
×10−3 estimate
real
5.5 5 4.5
175
0
20
40 60 Time (s)
Fig. 6. Experimental results: first - (a) yaw measurement, second - (b) actuation, third - (c) posterior probabilities, fourth - (d) inertia estimate. sensors used for the proposed solution were an indoor camera indoor multi-camera motion capture system and a gyroscope. The solution was studied in a simulation and experimentally using the Parrot Ar.Drone 2.0. The control and estimation systems provided very low settling times in simulation, while in testing it was much slower to ensure that the x, y and z controllers would be capable of accompanying the rotation. The controller was capable 175
Baram, Y. (1976). Information, Consistent Information and Dynamic System Identification. Ph.D. thesis, Massachusetts Institute of Technology. Cabecinhas, D., Naldi, R., Silvestre, C., Cunha, R., and Marconi, L. (2016). Robust landing and sliding maneuver hybrid controller for a quadrotor vehicle. IEEE Transactions on Control Systems Technology, 24(2), 400–412. doi:10.1109/TCST.2015.2454445. Chang, C.B. and Athans, M. (1978). State estimation for discrete systems with switching parameters. IEEE Transactions on Aerospace and Electronic Systems, AES-14(3), 418–425. doi:10.1109/TAES.1978.308603. Fekri, S. (2002). Robust Adaptive MIMO Control Using Multiple-Model Hypothesis Testing and Mixed-µ Synthesis. Ph.D. thesis, Instituto Superior T´ecnico. Gaspar, T., Oliveira, P., and Silvestre, C. (2015). Modelbased filters for 3-d positioning of marine mammals using ahrs- and gps-equipped uavs. IEEE Transactions on Aerospace and Electronic Systems, 51(4), 3307–3320. doi:10.1109/TAES.2015.140748. Hassani, V., Pedro Aguiar, A., Pascoal, A.M., and Athans, M. (2009). Further results on plant parameter identification using continuous-time multiple-model adaptive estimators. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, 7261–7266. doi:10.1109/CDC.2009.5400434. Lee, D. (2017). Ar.drone 2.0 support from embedded coder. https://www.mathworks.com/hardwaresupport/ar-drone.html. Loianno, G., Brunner, C., McGrath, G., and Kumar, V. (2017). Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and imu. IEEE Robotics and Automation Letters, 2(2), 404– 411. Mahony, R., Kumar, V., and Corke, P. (2012). Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor. IEEE Robotics Automation Magazine, 19(3), 20–32. doi:10.1109/MRA.2012.2206474. Notter, S., Heckmann, A., Mcfadyen, A., and Gonzalez, L. (2016). Modelling, simulation and flight test of a model predictive controlled multirotor with heavy slung load. volume 49. doi:10.1016/j.ifacol.2016.09.032. Outeiro, P., Cardeira, C., and Oliveira, P. (2018a). Lqr/mmae height control system of a quadrotor for constant unknown load transportation. In 2018 13th APCA International Conference on Automatic Control and Soft Computing (CONTROLO), 389–394. doi: 10.1109/CONTROLO.2018.8514545. Outeiro, P., Cardeira, C., and Oliveira, P. (2018b). Mmac height control system of a quadrotor for constant unknown load transportation. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 4192–4197. doi: 10.1109/IROS.2018.8594215.