Guidance and Trajectory Following of an Autonomous Vision-Guided Micro QuadRotor

Transcription

1 Guidance and Trajectory Following of an Autonomous Vision-Guided Micro QuadRotor S. Fernandes Instituto Superior Técnico - Lisboa / Technion - Israel Institute of Technology sofiabat@aero.ist.utl.pt January, 2011 Abstract The present thesis documents the dynamics modelling and control design development for an autonomous visionguided micro-quadrotor. Linear models were determined for Roll, Pitch, Yaw and Altitude and PID controllers for attitude and position were designed and tested. The system identification software CIFER was used to determine the frequency domain models from the previously recorded time-domain flight data. The flight tests were conducted with a human pilot. Matlab and Simulink were used for the development and simulation of the PID attitude, position and altitude controllers. A testbed was built for tests with the QuadRotor platform. The realtime controllers were implemented in Matlab code. A C++ program was developed to handle the communications, close the loop with the QuadRotor platform and manage the experiments. Successful attitude tests were conducted and the results are presented. 1 Introduction The present work was developed in the scope of a project at the Technion - Israel Institute of Technology which aims to develop a fully autonomous micro QuadRotor guided by vision, and with the hability to fly indoors as well as outdoors. The QuadRotor platform, a X-3d-BL [1] was enhanced with an XSens unit, which contains an IMU and a GPS receiver. The QuadRotor is a very agile ship with hover hability. It is propelled by four engines displayed as shown in the diagram of Figure 1 and the attitude can be actuated by creating differentials between the engines velocities. [2] Tilting the vehicle in any direction causes a component of the thrust vector to point in that direction, so commanding pitch and roll is directly analogous to commanding accelerations in the X-Y plane [2]. Position control is cur- Figure 1: QuadRotor MAV scheme [2] rently implemented using a PID controller design which actuates the vehicle s roll and pitch as control inputs. An advantage of QuadRotors in what concerns camera surveillance is that they can fly with any chosen yaw angle, in any given direction. 2 Dynamics Modelling The QuadRotor s dynamic models for attitude were determined using the method of system identification. Given a set of flight data, the CIFER software was used to determine the attitude frequency models of the platform. 2.1 CIFER software overview CIFER receives the recorded time domain flight data series and uses these data to perform spectral analysis and a variety of specialized behavior extractions. The blockdiagram in Figure 2 shows the algorithm structure for system identification in CIFER. CIFER s software structure is organized in functions or software blocks, being FRESPID and COMPOSITE the core ones. CIFER begins with some checks of each sample on the integrity of the data, to find those cases where data corruption has occurred but is not self-evident. Next 1

2 Figure 3: Flight data: attitude measurements for Pitch identification / Figure 2: Frequency response identification process in CIFER [3] FRESPID uses a flexible form of the fast Fourier transform (FFT) that was developed in CIFER facility, known as the Chirp-Z transform (CZT). The COMPOSITE routine does the automatic sliding window processing and optimization. [4] [5] In summary, the capabilities of the CZT allow the extraction of high-resolution spectra in a narrow frequency band and an increase in the identified dynamic range, especially at the low-frequency end [4]. Furthermore this is accomplished with simultaneously more accurate results (because CIFER can interactively narrow the range of interest) at greatly reduced computational cost (because only the needed points are calculated). [5] 2.3 Attitude models Equations 1 to 3 present the attitude linear models obtained with CIFER. TF ψ (s) = TF φ (s) = TF θ (s) = s (1) s s (2) s s (3) Figure 4 shows the coherence data reported by CIFER for the Pitch model. Among other relevant data, it shows the model s bandwidth, which is approximately 10rad/s, and should therefore be enough to include all relevant dynamics of the QuadRotor s motion. 2.2 Data acquisition The dynamics identification flight tests were conducted with a human pilot. Flight test planning requires previous knowledge about the system to be identified. The flight testing for frequency domain data acquisition is designed to provide frequency sweeps for selected input-ouput pairs, about a given trimmed flight condition over a frequency range of interest. [4] No detailed information on rotors, servos or other subsystems was of interest because the desired models should regard the system as a whole. Having in mind these identification objectives, the recorded quantities were the measurements from the XSens unit as well as the pilot inputs. The angle rate measurements were used for attitude (Roll, Pitch and Yaw) together with the respective commands from the remote control (pilot input). Figure 3 shows the recorded attitude angles measured in a flight test for Pitch dynamics identification. Figure 4: Pitch model coherence data The three Euler angles - Roll, Pitch and Yaw - models were considered to be independent due to the fact that during the system identification process no cross-relations were found between the Euler angles dynamic responses. 2.4 Position model According to [6], the translational motion of a quadrotor can be approximated by Equations 4, where the Euler an- 2

3 gles represent the platform s orientation with respect to the inertial reference frame. mẍ = Tsin(θ) mÿ = Tcos(θ)sin(ϕ) m z = Tcos(θ)cos(ϕ) mg Assuming that (θ, ϕ) (0, 0), Equations 4 can be linearized to Equations 5, which coincide with those derived for the flat disk by me. mẍ = Tθ mÿ = Tϕ m z = T mg For the vertical position model a slightly different model formulation was used, in accordance with [7]. The model is shown in Equation 6. ḣ(t) = dh dt = g + cos(φ)cos(θ)t (t) m After linearization for (θ, φ) 10, Equation 6 becomes ḣ(t) = T (t) m g = T n(t) (7) In the frequency domain, the obtained models become: X(s) θ(s) = g 1 s 2 Y (s) φ(s) =g 1 s 2 H(s) T n (s) =1 s This very simple translation model based on Newton s equations served as the basis for the development of the position controllers. 3 Attitude PID Control Digital PID controllers were designed for attitude control following the algorithm described in [8]. This algorithm specifies the set of steps to generate a stable and robust digital PID controller that meets the design specifications for a second order transfer function. The design specifications include phase margin 60deg, minimal settling time, zero SSE and peak overshoot under 10%. The sample time is T s =0, 04s due to restrictions in the wireless communication with the QuadRotor. Matlab implementation of the referred algorithm led to the results summarized on Table 1. (4) (5) (6) (8) Angle Discrete model Discrete controller Roll Pitch Yaw z z z(z 1) z z z z z2 z(z 1) z z z z2 1.85z z(z 1) Table 1: Attitude discrete models and PID controllers 3.1 Fine tunning and simulation results Simulink models were created to simulate and fine tune the designed controllers. The step responses achieved after tunning were quite satisfying, having been adjusted only the global controller gains. Table 2 shows the tunned controller gains and settling times obtained. Angle Tunned gain Settling time Roll 1 0.5s Pitch s Yaw < 2s Table 2: Controller gains and settling times after fine tunning All step responses matched the defined design specifications. As expected, the yaw response is the slower when compared with the other Euler angles - Pitch and Roll. The global gain for Roll s controller was not changed. 3.2 Stability Analysis The values of the stability margins (Gain and Phase margin) and bandwidth after fine tunning are summarized on Table 3. The closed-loop Bode plot for Yaw is shown in Figure 5. The corresponding plots for Roll and Pitch aren t displayed here because they are very similar and the badnwidths are already displayed in Table 3. Please note as well that the Gain and Phase margins computed on the closed-loop Bode plot aren t meaningful; the stability margins are retrieved from the open-loop Bode plot intead. 3

4 Angle Gm (db) Pm (deg) Bandwidth (rad/s) Roll Pitch Yaw > 10 models (T s =0.04s) and the digital phase-lead compensators obtained. Axis Discrete model Phase-lead Inner-loop Table 3: Attitude s stability margins and bandwidth X X(z) θ(z) (z+1) = g (z 1) z 0.64 z 0.56 Pitch Y Y (z) φ(z) = g (z+1) (z 1) z z Roll Table 4: Horizontal position models and controllers Figure 5: Bode plot of the Yaw closed loop system 4 Horizontal position control With an attitude controller of sufficient bandwidth defined, it is possible to develop a path tracker that generates attitude reference commands, ignoring the fast dynamics of the attitude angles [9]. Hence, the simple position models derived in Subsection 2.4 were enhanced by the addition of the corresponding attitude control systems designed in Section 3, according to the block diagram in Figure 6. It should be clear that each horizontal position controller takes the respective position error as input to compute the output, which is the corresponding attitude reference angle. After gainning some insight about the behavior of the system in the simulation environment, it was decided to feed the measurement of the angle rate to the position controller instead of the angle itself. This has shown to improve the response since it basically removes one integrator from the system s transfer function, thus making it faster. The compensators weren t changed. The open-loop for the X axis can thus be written as in Equation 9. X(z) X ref (z) = C θ X(z) lead(z) θ ref θ(z) The Y axis open-loop can be similarly defined, simply substituting θ by φ. 4.1 Stability analysis (9) The values of the stability margins and bandwidth for the horizontal position control systems are shown on Table 5. Axis Gm (db) Pm (deg) Bandwidth (rad/s) X Y Figure 6: Diagram of position control with attitude innerloop A phase-lead design was chosen for the horizontal position (X and Y axes) controllers. Having introduced the full models with the attitude inner-loops in Simulink, the controllers were tunned inside Simulink s environment. The following Table 4 shows the discretized position Table 5: Horizontal position s stability margins and bandwidth Figure 7 shows the step response of the full X axis control system, that is, with the Pitch attitude control system as an inner-loop. Again, since it s very similar to the Y axis response with Roll inner-loop, the later was omitted. 4

5 Figure 7: Simulated position step response 5 Vertical position control The discretized vertical position model is shown in Equation 10. H (z) = 0.04 T n z 1 (10) For this very simple model, a PI controller was developed and tunned in the Simulink environment. The controller obtained can be described as in Equation 11. T (z) g H ref (z) H(z) =5+ 2 z 1 (11) Figure 8: Altitude response to attitude changes 6 Test results The attitude control system was extensively and successfully tested on the testbed developed for that purpose. Figure 9 shows the QuadRotor mounted on the testbed during an experiment. The overall simulation results were satisfying given the simplcity of the models at hand. Unfortunately, the applicability of these simple control systems could not be studied or further developed by means of flight testing for the reasons mentioned in Section Stability analysis The values of the stability margins and bandwidth obtained for the vertical position control system are shown on Table 6. Gm (db) Pm (deg) Bandwidth (rad/s) > 10 Table 6: Vertical position s stability margins and bandwidth The full position control system (horizontal and vertical) with attitude control inner-loop was simulated in Simulink with good results. Figure 8 shows the altitude step response in the presence of attitude step changes, the attitude step responses and the corresponding throttle plot. This shows that the simulated altitude control system is robust to agressive attitude conditions, following it s reference with zero SSE. Figure 9: QuadRotor on the testbed during an attitude test Figures 10 and 11 present examples of attitude test results. The plots show angle (deg) versus time (seconds). It can be observed from the results that the implemented PID controllers are able follow the fast dynamics of the QuadRotor s platform, although only free flight tests can accurately determine that. Improvements in the responses might be achieved with robust control techniques. No satisfactory flight test results could be achieved for the full position control algorithm due to the lack of accuracy of the position sensors. The QuadRotor s autonomous flight should be aided by vision data, which is not yet available. Having the XSens IMU as the only measurement source posed great problems due to the XSens limitations: not only the position readings had above 1000% errors as the attitude measurements 5

6 showed too big drifts. Note that for tethered flight in the testbed the drifts aren t a big problem because the testbed prevents translational movement. Nevertheless the results that were achieved are promising, since the inner-loop, the attitude control system, presents a satisfying behaviour. 7 Concluding remarks and future work Figure 10: Hovering test Figure 11: Very hard conditions: steps on all angles A dynamics model for the attitude and altitude of the QuadRotor was determined. A set of PID controllers for attitude was developed and fully tested in tethered flight. Also position controllers were developed and the full position control system was simulated with the attitude control system as an inner-loop. The attitude flight test results were satisfying, although free flight tests would be advisable, when the vision data is available. However, the current control implementation has little ability to reject disturbances from wind and translational velocity effects. For this scale aircraft, even mild winds can cause disturbances. There is signifant coupling between the velocity and the attitude dynamics, so assuming that free stream velocity and attitude control are decoupled is a key weakness of this approach. [2] Because the QuadRotor is meant to fly in multiple kinds of environments, great flexibility will be required and an important feature of the control system for this project is that it should be robust to changes in the platform s dynamics. This feature wasn t tested, but it should be considered in the future. There are two clear conflicting project design goals: the hability of the algorithms to deal with the fast dynamics of the QuadRotor against the objective of having a controller that is robust to model changes. Regarding the vision algorithms implementation, it is expected that these will have much lower (around 10 to 20 times) sample rates than the common IMU sensors. This poses a challenge to the control algorithms and the development of a dedicated algorithm block to account for the vision positioning data handling should be studied. Another possible subject for future research is the study of correlations between the dynamics about the different axis of the QuadRotor s platform. Since no correlations were detected with the CIFER software, the current control system was built on the assumption that the correlation between axis is neglectible. Although, the experimental results with the tethered QuadRotor suggest otherwise. It is still to determine wether this was an effect produced by the testbed tethering system or an actual feature of the platform s dynamics. 6

7 8 Aknowledgments I would like to express my appreciation to my advisors, Afzal Suleman and Pini Gurfil, and also to Yuval Shalgi, Sergey Danielian, Aram Movsisian and Yair Atzmon for their invaluable help and contributions. 9 Note For extensive information on the work hereby described, please refer to the respective bibliography items or to my MSc thesis: S. Fernandes. Guidance and Trajectory Following of an Autonomous Vision-Guided Micro QuadRotor. Instituto Superior Técnico - Universidade Técnica de Lisboa, This thesis also documents the development of a communication system and realtime code necessary for the experimental tests. References [1] A. T. GmbH. X-3D-BL User s Manual. [2] G. M. Hoffman, H. Huang, S. L. Waslander, and C. J. Tomlin. Quadrotor helicopter flight dynamics and control: Theory and experiment. In AIAA Guidance, Navigation and Control Conference and Exhibit. American Institute of Aeronautics and Astronautics, August [3] CIFER User s Guide, student version edition. [4] M. B. T. Jeffrey N. Williams, Johnie A. Ham. Flight Test Manual - Rotorcraft Frequency Domain Flight Testing. U.S. Army Test and Evaluation Command, U.S. Army Aviation Technical Test Center - Airworthiness Qualification Test Directorate, September [5] CIFER Background and Application. [6] L. Beji and A. Abichou. Streamlined rotors mini rotorcraft: Trajectory generation and tracking. International Journal of Control, Automation and Systems, 3(1):87 99, March [7] S. Bouabdallah and R. Siegwart. Advances in Unmanned Aerial Vehicles, chapter 6 - Design and Control of a Miniature Quadrotor, pages Springer, Netherlands, [8] A. B. J. Hetthéssy, R. Bars. Discrete PID Control. Budapest University of Technology and Economics, [9] G. M. Hoffman, S. L. Waslander, and C. J. Tomlin. Quadrotor helicopter trajectory tracking control. In AIAA Guidance, Navigation and Control Conference and Exhibit. American Institute of Aeronautics and Astronautics, August [10] W. E. Green and P. Y. Oh. A mav that flies like an airplane and hovers like a helicopter. Proceedings of the 2005 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, July [11] K. Ogata. Modern Control Engineering. Prentice Hall International, Inc., 3rd edition, [12] G. C. Goodwin, S. F. Graebe, and M. E. Salgado. Control System Design. Universidad Técnica Federico Santa María, [13] J. Kamman. Me 360 control systems - system type and steady-state error. [14] K. Astrom and B. Wittenmark. Computer Controlled Systems. Prentice Hall International, Inc., [15] S. Bouabdallah, A. Noth, and R. Siegwart. Pid vs lq control techniques applied to an indoor micro quadrotor. In IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan,