Implementation of Estimation and Control Solutions in Quadcopter Platforms

Transcription

1 Implementation of Estimation and Control Solutions in Quadcopter Platforms Flávio de Almeida Justino Instituto Superior Técnico, Universidade de Lisboa, Lisboa, Portugal May 216 Abstract The main objective of this work is the comparative experimental evaluation of different methods of attitude estimation in order to test and evaluate the performance of one of these methods, the Trace- Based Filter. To that effect, an implementation of this solution is made in two autopilots: Paparazzi and Pixhawk. First, the concept of a quadcopter is introduced. Two quadcopter platforms used for the purposes of this work are presented: QuaVIST and QuadR-ANT. Then, the two mentioned autopilots are introduced in order to make the process of implementation clear and objective. Finally, the results of the implementation in each autopilot are presented and discussed. For the case of Paparazzi, a comparison is made between the implementation of the proposed estimator and one of the autopilot solutions, the complementary filter. The Trace-Based Filter is also implemented in Matlab based on data provided offline by QuaVIST sensors. Posteriorly, an evaluation of the proposed estimator on Pixhawk is made based on data obtained in real-time flight with QuadR-ANT. As a reference for the validation of the estimator in both autopilots, attitude data provided by Robotics, an infrastructure containing a motion capture system capable of returning true ground attitude values, is considered. Keywords: Nonlinear Attitude Estimation, Trace-based Filter, Paparazzi, Complementary Filter, Pixhawk. 1. Introduction Unmanned Aerial Vehicles (UAV) are becoming very important in our daily life, even if sometimes we have no idea about it. What started to be a military weapon, commonly known as drone, today is way more than that. Drones have a wide range of applications, such as military weapons, surveillance and security, archaeological surveying, climate study, search and rescue missions [8] or even for entertainment purposes. Drones have been taking an important role in the aircraft technology field as they allow the development of new ideas that can be applied and tested without any harm to the pilot, that being an important advantage when developing new control and estimation algorithms in order to improve efficiency, flexibility and robustness. This work focuses on the problem of attitude estimation. For feedback control, it is necessary that all variables are observable. It is not always possible to measure variables due to, for example, lack of sensors, and they need to be estimated. Estimation also allows to attenuate noise coming from the sensors. Nowadays, there are already estimation methods in the market with proven good performances. Amongst the most common are the Extended Kalman Filter [2]-[5] and the Complementary Filter (CF) [6]. This work intends to analyse the performance of an attitude estimation solution, the Trace-based Filter (TBF), proposed in [7]. Implementation of the TBF is done in two Autopilots (AP): Paparazzi and Pixhawk. Validation of the implementation is made through two quadcopter platforms, the Qua- VIST, which uses Paparazzi AP, and the QuadR- ANT, which uses Pixhawk AP. The next chapter introduces both platforms, the kinematic model of a quadcopter and the two AP s. 2. Quadcopters Quadcopters configuration may vary depending on what is their prime mission or application. However, there are some generic components that constitute the basics of a quadcopter, such as the frame, motors, propellers, speed controllers, batteries, IMU and flight controllers. IMU is the most relevant component for the purpose of attitude estimation, since it provides sensors such as gyroscopes, accelerometers and magnetometers that can measure angular velocity, acceleration and magnetic field, respectively, thus providing enough data for attitude estimation. 1

2 2.1. QuaVIST QuaVIST (fig.1) is a commercial custom designed quadcopter. It serves as a research instrument mainly in the fields of systems control and computational vision. QuaVIST has a robust structure as it features several extra components and provides a wide range of functionalities. It can be controlled manually or fly autonomously as it is equipped with path following algorithms. The user can monitor its activities and control some flight parameters in real time. brushless motors and four speed controllers. The onboard AP is Pixhawk, an open source hardware and software flight controller. The landing gear was made using 3D printing methods. Communication with the GCS is made through MAVLink protocol using a 3DR radio to provide telemetry. Telemetry can also be accessed through an onboard SD card. Figure 1: QuaVIST platform Additionally, the QuaVIST platform is custom designed to feature a computer, PC-14. This machine carries out high level tasks and allows the introduction of other control and estimation solutions. Telemetry is available in QuaVIST via Paparazzi and PC-14. Information about the sensors, motors, trajectories, etc, can be accessed via the Ground Control Station (GCS). For this purpose, QuaVIST is equipped with a Xbee series datalink, a wireless communication device. Also, the PC-14 can interact directly with the information received by QuaVIST and send back new information given by the user. The IMU consists, mainly, of a 3-axis ADXRS62 gyroscope, 3-axis ADXL335 accelerometer and a 3- axis HMC5883L magnetometer. Fig.2 shows the flux of communication between QuaVIST-PC14 and the GCS. Figure 3: QuadR-ANT platform QuadR-ANT is perfect for testing new controllers and estimators in real time flight because most of the components are easy and cheap to replace if some misbehaviour occurs, opposite to QuaVIST, which has a higher cost and requires full attention Reference Frames Let I = {O; I x ; I y ; I z } denote the inertial frame North-East-Down (NED), with center in O, and {B} the body-fixed frame with center in O b as in fig.4. The matrix that defines the rotation from the Figure 2: Communication links with the QuaVIST 2.2. QuadR-ANT QuadR-ANT (fig.3) is a quadcopter prototype designed by IST students. It is equipped with four Figure 4: Body-fixed frame centered in O b body-fixed frame to the inertial frame, and using Euler angles description with Ψ = [φ; θ; ψ] T, is characterized as: 2

3 I BR Ψ = 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 θ (1) 2.4. Inertial Measurement Unit modelling As described before, IMU consists of a set of sensors, namely gyroscope, accelerometer and magnetometer, that provide measurements in order to estimate attitude Gyroscope Currently, gyroscopes usually are Micro Electro- Mechanical Systems (MEMS), which are low-cost, small but still very efficient. The gyroscopes are used to measure the angular velocities of the quadcopter in the body-fixed frame, Ω B. Most of the gyroscopes can be modelled considering two kind of disturbances: a stochastic Gaussian noise and a slowly time-variant non-stochastic bias that can be assumed, most of the time, as constant [3]. The model that governs the gyroscope can be described as: Ω B = [ ḡ p ḡ q ḡ r ] T = Ω B + σ ω + µ ω (2) where Ω B R 3 is the measured angular rate, Ω B R 3 is the real angular rate, σ ω R 3 is the stochastic Gaussian noise and µ ω R 3 is the bias Accelerometer Accelerometers are quite useful to measure the acceleration relative to free-fall of a body. This acceleration is commonly known as g-force and it is different from the acceleration relative to rate of change of velocity. An accelerometer at rest will measure a positive acceleration of g = 9.81 m/s 2 (1G) upwards while an accelerometer free-falling out of the sky will measure zero acceleration. The model of the accelerometer is as follows: ā B = a B R T Ψ.g + σ a + µ a (3) where ā B R 3 is the measured acceleration expressed in the body-fixed frame, a B R 3 is the real acceleration expressed in the body-fixed frame, g = [ 9.81 ] T R 3 is the constant gravitational vector, σ a R 3 is the stochastic Gaussian noise and µ a R 3 is a bias term Magnetometer Magnetometers are used to measure the local Earth magnetic field. They are also affected by some disturbances, such as stochastic Gaussian noise, and also local magnetic disturbances such as iron disturbances, electromagnetic disturbances, etc. The magnetometer model can be characterized as: n = R T Ψ.n + σ n + e n (4) where n R 3 is the measured magnetic field vector, n R 3 is the real specific local magnetic field vector, σ n R 3 is the stochastic Gaussian noise and e n R 3 represents general disturbances affecting the magnetometer. 3. Attitude Estimation Two attitude estimation methods are now introduced, the Trace-based filter (TBF) and the Complementary filter (CF). For that, it is necessary to introduce some mathematical identities in Lie-Algebra. The cross map operation, ( ) : R 3 so(3) is a Lie Algebra isomorphism defined by: ν = ν 3 ν 2 ν 3 ν 1 (5) ν 2 ν 1 for any ν R 3, such that any a R 3 implies ν a = ν a. The inverse of the cross map operation is the vee map, ( ) : so(3) R 3. The trace map operation, tr( ) : R n n R, is defined by: tr(a) = {A R n n tr(a) = n a ii } (6) i= Direct Attitude Measurements There are different methods to obtain attitude measurements from the sensors. The one used for the purposes of this work consists of using accelerometer measurements directly to calculate pitch and roll, and the magnetometer measurements to calculate yaw. Let ā = [ā x ; ā y ; ā z ] define the accelerometer measurements in each axis. Then: φ = atan2 ( ā y, ā z ) (7) ( ) θ = atan2 ā x, ā 2 y + ā 2 z (8) Now, let M = [ M x ; M y ; M z ] define the measurements of the magnetometer in each axis. To obtain the measured yaw angle, the following equation is used [1]: ψ = atan2 ( M y c φ + M z s φ; M x c θ + ( M y s φ + M ) z c φ)s θ (9) The rotation matrix R is then obtained by replacing the measured angles in (1). 3

4 3.2. Complementary Filter The main idea behind a CF is to combine the outputs of the gyroscope and accelerometer in order to obtain good angular estimation results. As described in Chapter 2, both the gyroscopes and accelerometers suffer from several types of disturbances that have to be filtered somehow. The accelerometer measures all accelerations present in the aircraft, which means that any change of the wind or the gravitational field, among others, will disturb the measurements. Therefore, the accelerometer data is reliable only on the long term. So, the information must go through a Low- Pass Filter to be more reliable. When it comes to the gyroscope, because of the integration over time, the measurements tend to drift over time, which means that the data is more reliable on the short term. It is necessary to use a High-Pass Filter to remove the drifting mean. The function of the CF is to fuse these information in order to reconstruct the signal. The CF used by Paparazzi includes a bias estimation and its dynamics is given by: ˆR = ( R( ω ˆµ) + K p ˆRλ) ˆR (1) Continuous Time Formulation For a known measured rotation matrix R and known angular velocity measurements ω (gyroscope measurements), and based on (13)-(15), the estimator dynamics is: for ė ω = e ω d 2 [D R T ˆR ˆRT RD ] (16) ˆR = ˆR [ˆω] (17) ˆω = e ω + ˆR T R ω (18) where is a positive definite diagonal weighting matrix and d R + is a positive scalar parameter Discrete Time Formulation For the discrete formulation (16)-(17) can be rewritten as: e ω,k+1 = e ω,k t s e ω,k t s de R (19) with and ˆµ = K I λ (11) λ = 1 2 ( ˆR T R ( ˆRT R) T ) (12) ˆR k+1 = ˆR k exp ts[ˆω] (2) where the angular velocity estimation, ˆω k, is given by: ˆω k = e ω,k + ˆR T k R k ω k (21) for a sampling time t s, and: where λ is a correction factor that drives the rotation error R = ˆR T R and Kp, K I are positive gains Trace-based Filter The core of the TBD approach consists in considering an attitude error function Υ : SO(3) SO(3) R, an attitude error vector e r R 3 and an angular velocity error vector e ω R 3, where SO(3) represents the Special Orthogonal Group in three dimensions, [4] and [7]: Υ( R, ˆR) = 1 2 tr ( D(I R T ˆR) ) e R = 1 2 [D R T ˆR ˆRT RD ] (13) (14) e ω = ˆω ˆR T R ω (15) where Υ( R, ˆR) is a locally positive-definite about ˆR = R and D R 3 3 is a positive definite diagonal weighting matrix. exp ts[ˆω] = I + [ˆω] sin (t s ) + [ˆω] 2 [1 cos (t s)] (22) 3.4. Gyroscope Bias Estimation As explained previously in Chapter 2, low cost MEMS gyroscopes usually have small disturbances, one of them being a slowly time varying bias, µ ω, described by (2). It is possible to estimate the gyroscope bias in order to reduce its impact on the attitude results. The used bias formulation is the following: ˆω = ω + α ˆµ, ˆµ = kµ α (23) where α is a correction term based on the vectorial measurements and < k µ < 1, [s 1 ]. One should notice that the TBF has a rotation between the frames of ˆω and ω, so the correction term α can be defined as: α = e ω + ˆR ( T Rˆµ ˆµ = kµ e ω + ˆR ) T Rˆµ (24) In the next section, results of the different implementations are shown and some conclusions are drawn about the performance of the estimators. 4

5 4. Results As mentioned earlier, the implementation of the TBF was made in two AP s: Paparazzi and Pixhawk. For the case of Paparazzi, first, some data from QuaVIST sensors were taken for a known trajectory in order to reconstruct the TBF in Matlab, with and without bias estimation. Then, a comparison was made between the Matlab results, the implemented TBF on Paparazzi and the CF on Paparazzi, using the true ground attitude values provided by a motion capture system mounted in IST, called. For the case of Pixhawk, results from the TBF implementation were taken during a real-time flight and compared against data provided by. A validation was not made in Matlab for this case due to technical difficulties in obtaining valid data from the Pixhawk IMU. In the next subsections, these results are presented and discussed to obtain the proper conclusions Paparazzi For the purpose of this validation, data from Qua- VIST sensors, as well attitude estimation from the CF and the implemented TBF, were saved on a SD card. A simple trajectory was described with Qua- VIST turned off and it consists on small roll and pitch rotations in both directions and yaw rotations of about 9, also in both directions. The TBF parameters were adjusted manually until good performance was achieved. Table 1 summarizes the values of the TBF used in the implementations, where diag represents a diagonal matrix R 3 3. The frequency of the is 179Hz. At the time of implementation on Paparazzi, was not available. For that reason, bias estimation was not implemented since its impact could not be evaluated relatively to the true attitude values. Values of the TBF parameters Matlab Paparazzi Frequency [Hz] d 1 4 D diag(25) diag(25) diag(45) diag(45) k µ.1 Table 1: Values of the TBF parameters for the Matlab and Paparazzi implementations Fig.5 illustrates a comparison for all estimators for roll, pitch and yaw rotations, individually. As the frequencies of, Paparazzi and implementation in Matlab are different, a resampling was made using methods of interpolation and decimation, in order to get the same number of samples. Roll (º) Pitch (º) Yaw (º) TBF on Matlab TBF on Paparazzi TBF w/ Bias on Matlab CF Paparazzi Sensors Attitude (a) Roll TBF on Matlab TBF on Paparazzi TBF w/ Bias on Matlab CF Paparazzi Sensors Attitude (b) Pitch TBF on Matlab 1 TBF on Paparazzi TBF w/ Bias on Matlab CF Paparazzi Sensors Attitude (c) Yaw Figure 5: Roll, Pitch and Yaw results for Matlab and Paparazzi implementations In order to obtain a detailed analyse of the performance of each estimator, a calculation of the Root- Mean-Square Error (RMSE) was made: n i=1 RMSE = (Ψ i ˆΨ i ) 2 (25) n where Ψ i represents the true ground values of atti- 5

6 tude, in this case provided by, ˆΨ i represents the estimated values of each solution and n in the number of samples. Table.2 summarizes the RMSE of each implementation. RMSE[ ] φ θ ψ TBF in Matlab TBF on Paparazzi TBF bias on Matlab CF Table 2: RMSE for Matlab and Paparazzi implementations It is possible to observe that, from all the presented solutions, the TBF in Matlab is the one with the best performance, achieving a smaller error when compared to the CF. The implementation of the TBF on Paparazzi presents good results. However, attitude worsens when compared to the implementation in Matlab. Also, this solution appears to contain more noise. A better adjustment of the parameters may improve its performance. The yaw presents a higher error when compared to pitch and roll. This is most likely due to lack of calibration of the magnetometer. It is visible that for roll and pitch movements the yaw tends to follow their dynamic, increasing the error. Testing this solution in a real-time flight would provide more information about its behaviour. It is important to see the effect of the bias parameter, k µ, in the implementation with bias estimation. Table.3 shows the RMSE for different k µ. The best performance achieved with bias estimation RMSE[ ] φ θ ψ k µ = k µ = k µ = k µ = k µ = Table 3: RMSE for TBF on Matlab, for different bias parameter, k µ was for k µ =.1. The estimated attitude tends to get worse with the increase of k µ, except for the case of φ, that improves slightly. In the next section, an analysis of attitude estimation is presented in real-time flight with QuadR- ANT, on Pixhawk Pixhawk In order to test the performance of the TBF in real scenarios, a flight with random trajectories was conducted using QuadR-ANT. The attitude data was taken in real-time and saved into a log file. True ground values were also taken from to conclude about TBF performance. However, reliable data from the sensors could not be obtained in order to implement the TBF solution on Matlab and compare it with the default quaternion estimator of Pixhawk. Table.4 resumes the values of the parameters implemented on Pixhawk for the TBF solution. The whole Pixhawk system runs at 25Hz.. Values of the TBF parameters Pixhawk implementation Frequency [Hz] 25 d 4 D diag(25, 25, 25) diag(45, 45, 45) k µ.1 Table 4: Values of the TBF parameters for the Pixhawk implementation The estimated and true ground values of attitude obtained during flight are illustrated in fig.6. The first conclusion that can be drawn from these results is that stabilization was achieved in flight mode, i.e, the quadcopter effectively was able to fly. As it would be expected, during flight the presence of noise is stronger when compared to the previous validations. Noise coming from the motors are a huge contribution to that fact. However, the estimator seems to perform well under these conditions. We may detect some delay of the TBF compared to the true attitude. In fact, during the flight experiment, it was noted that the quadcopter would perform well for hover movements but would become unstable for more aggressive and sudden movements. This delay provokes instability since the system is trying to compensate estimated results from a previous action. Despite this fact, the dynamics of TBF estimator was according to what was expected. The reason for this delay is most likely related to small adjustments of the parameters and the frequency of the system. Further improvements on this theme may include an optimization of the parameters to achiever better stability. Like the case described in the previous section, a comparison analysis between the estimated and the true attitude values was made by calculating the RMSE, as shown in table.5. RMSE[ ] φ θ ψ TBF on Pixhawk Table 5: RMSE for Pixhawk TBF implementation A direct conclusion based on the RMSE values is 6

7 Roll (º) Pitch (º) TBF on Pixhawk (a) Roll TBF on Pixhawk took place before the flight experiment in order to attenuate discrepancies with direct attitude measurements. Pixhawk provides a simple and intuitive way to perform this calibration, opposed to Paparazzi. The worst performing rotation is clearly roll, which presents a considerable error of almost 3. As seen in fig.6(a), the quadcopter was submitted to constant variations in the roll axis. This happens because of the delay explained before that cause instability to the system. Also, it makes stabilization depending too much on the pilot, which is not recommendable. In this implementation, particularly, bias estimation was of major importance. From the previous section, a conclusion that the bias estimation has a small impact on the performance of the estimator may be evident. However, stabilization during the flight was only achieved for values of k µ very close to.1. On an overall basis, it was verified that the TBF estimator could be a suitable solution for attitude estimation, providing new alternatives to the current estimators on the market. It is for sure necessary to improve the implementation by adjusting and optimizing its parameters. Yaw (º) (b) Pitch TBF on Pixhawk (c) Yaw Figure 6: Roll, Pitch and Yaw results for Pixhawk implementation that TBF performs slightly worse when submitted to flight conditions. That was, somehow, expected because of the vibrations coming from the motors. Yaw rotation presents a significant decrease of the error, which is directly related with the calibration of the magnetometer. Calibration procedures 5. Conclusions During the course of this work some results were reached and some final conclusions can be drawn. The implementations of the TBF in both studied AP s revealed good overall performances. However, further improvements should be made in order to get more accurate and systematic results. Improvements should focus on the optimization of parameters. Results of the implementation on Paparazzi show very good performances for roll and pitch rotations, beating the CF. For yaw rotation, poor results were obtained considering magnetometer miscaliberation. Estimation of bias was proven to be important for small values of K µ and tends to worsen when increased. The bias estimation was crucial for the Pixhawk implementation, since stability conditions for the flight were only achieved when bias estimation was present, with K µ =.1. A well succeeded flight experiment was achieved for the Pixhawk implementation, which is, by itself, a good indicator. In this case, the TBF estimator performs worse and becomes unstable for sudden and aggressive movements. Although reasonable results were obtained and flight was possible, this does not discard future improvements, specially for purposes of computational vision and aerial mapping, where stabilization is of maximum importance. Another interesting improvements that could be made in the future include combining the TBF 7

8 solution with new control solutions. Substituting the PID controllers being used by both AP s by LQR based solutions would provide new perspectives about attitude control and estimation. In the case of Paparazzi, this improvement may help with the high level control tasks taking place in PC-14, giving the user more autonomy and flexibility. Over the course of this work, Pixhawk revealed to be a simpler and more user friendly tool, not only for the user but also for developers. Thus, installing Pixhawk on QuaVIST may prove to be a good choice. Some of the work done here may help in this transition. References [1] J. D. Barton. Fundamentals of Small Unmanned Aircraft Flight. Johns Hopkins APL Technical Digest, 31(2): , 212. [2] P. Batista, C. Silvestre, and P. Oliveira. Sensor-Based Complementary Globally Asymptotically Stable Filters for Attitude Estimation: Analysis, Design, and Performance Evaluation. Automatic Control, IEEE Transactions on, 57(8):295 21, February 212. [3] M. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel. A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV. In Intelligent Robots and Systems, 28. IROS 28. IEEE/RSJ International Conference, pages , Nice, September 28. [4] M. Figueirôa. Nonlinear Attitude Estimation in SO(3): Application to a Quadrotor UAV. Master Thesis, Instituto Superior Tcnico, Universidade de Lisboa, May 214. [5] B. Henriques. Estimation and Control of a Quadrotor Attitude. Master Thesis, Instituto Superior Tcnico, Universidade Tcnica de Lisboa, June 211. [6] R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group. Automatic Control, IEEE Transactions on, 53(5): , June 28. [7] A. Moutinho, M. Figueirôa, and J. R. Azinheira. Attitude Estimation in SO(3): A Comparative UAV Case Study. Journal of Intelligent & Robotic Systems, 8(3-4): , 215. [8] S. Waharte and N. Trigoni. Supporting Search and Rescue Operations with UAVs. In Emerging Security Technologies (EST), 21 International Conference on, pages IEEE, 21. 8