Practical implementation of Differential Flatness concept for quadrotor trajectory control

Transcription

1 Practical implementation of Differential Flatness concept for quarotor trajectory control Abhishek Manjunath 1 an Parwiner Singh Mehrok 2 Abstract This report ocuments how the concept of Differential Flatness was implemente to fly an AR Drone from the paper [1] Differential Flatness Base Control of a Rotorcraft For Aggressive Maneuvers by Dr. Bear, et all,. The AR Drone was flown successfully by using the ROS-Cortex interface with goo control for trajectories with fixe altitue. I. INTRODUCTION The primary objectives of this project was, To implement the Differential Flatness Control paper by Dr. Bear, et all., using RISC MAAP by esigning an appropriate LQR controller for the AR Drone moel To fly ifferent trajectories using ROS an Cortex to valiate the performance in harware The above mentione implementation was carrie out at the Robust Intelligent Sensing an Control (RISC) Laboratory, Utah State University using the Multi-Agent Analysis Platform (MAAP). II. THEORY AND MATH The Linear Quaratic Regulator (LQR) control process flow consists of an external feeback loop with an internal control loop as shown in the figure below. For the quacopter, we efine the flat output vector as a function of only the reference states x r as p r n y tra j = p r e p r (2) ψ r We efine the state an control inputs calculate from the ifferential flatness as the reference states an the reference inputs to be x r = g x (y tra j,ẏ tra j ) (3) u r = g u (y tra j,ẏ tra j,ÿ tra j ) (4) The reference inputs are efine as a matrix of accelerations in north, east an own irections with the fourth component being the yaw rate while accounting for the acceleration ue to gravity in the ownwar irection. [ ] u u r r = p (5) where, u r ψ u r p = p n r p r e 0 0 (6) p r g an u r ψ = ψ (7) Fig. 1. Differential quacopter control process flow Since there was an on-boar attitue controller on the AR Drone, the states of the quacopter are the positions in north, east an own irections, the velocities in north, east an own irections an the yaw angle. x = [ p n p e p p n ṗ e p ψ ] T 1 Grauate stuent, Mechanical an Aerospace Engineering, Utah State University, Logan, UT 84322, USA krmabhishek@gmail.com 2 Grauate stuent, Mechanical an Aerospace Engineering, Utah State University, Logan, UT 84322, USA parwiner.mehrok@aggi .usu.eu (1) Hence, the inputs to the quacopter were the esire thrust, roll angle, pitch angle an the yaw rate. T v = φ θ (8) r The iagonal Q an R matrices to calculate the LQR gain K was formulate using Bryson s Rule which is given by, Q ii = R ii = 1 maximum acceptable (states error) 2 1 maximum acceptable (input error) 2 i (1,2,...,l) (9) i (1,2,...,m) (10)

2 III. RISC MAAP It abbreviates to Robust Intelligent Sensing an Control Multi-Agent Analysis Platform. It is a test-be to valiate new or previously existing control laws using actual harware. This is very useful as the robustness of the control theory can be observe in real worl conitions. IV. APPROACH Six IR markers were attache to the AR Drone as shown in the figure below. This six marker template was use to calculate the centroi (x,y an z positions) an the roll, pitch an yaw angles. Fig. 2. RISC MAAP Components A. ROS Robot Operating System (ROS) is a collection of software frameworks for robot software evelopment, proviing operating system-like functionality on a heterogeneous computer cluster. ROS provies stanar operating system services such as harware abstraction, low-level evice control, implementation of commonly use functionality, messagepassing between processes, an package management. The greatest functionality of ROS is that it can be implemente as a real-time Operating System. ROS has reaily available, open-source, compatible packages for the AR Drone, Arucopter, Mikrocopter, etc., with eicate support communities. B. MoCap System - Cortex The motion capture system consists of 16 IR camera, IR reflective markers an the software Cortex. The cameras have an IR LED ring aroun the lens that beams IR light on the reflective markers while the reflecte IR beam enters the camera lens. Using patente, sub-pixel analysis algorithms, the x, y an z positions of each marker is epicte on Cortex in a graphical way at approximately 200 Hz. The MoCap system works similar to an inoor GPS system proviing ata at superior rates for quick maneuvers. C. AR DRONE The AR Drone by Parrot SA, France was chosen as the quacopter of choice for the following reasons. Inbuilt attitue control ROS an AR Drone compatibility High ata rates ( 80 Hz) No external communication moule neee (WiFi connectivity) Resilient buil Safe for inoor flight because of lower rotor spee Fig. 3. AR Drone marker template Markers B an C form a vector while the vector from marker A when projecte on the BC vector, intersecte at the centroi. These vector pairs were rotate in the z-axis by 45 to efine the pitch an roll axis. The rotation vector is use was, Rotation matrix = cos(45 ) sin(45 ) 0 sin(45 ) cos(45 ) 0 (11) Using the AR Drone moel given in the table below, the Q an R matrices were formulate to obtain the LQR gain K for the circular an figure eight trajectory respectively. The lqr comman was use in MATLAB to obtain the values of K. Fig. 4. AR Drone parameters The controller an trajectory files were written in Python which was irectly compile an run from ROS to fly the AR Drone. The input to the AR Drone was in the form of altitue, roll, pitch an yaw rate instea of thrust as it was transforme from NED system of co-orinates to XYZ system of co-orinates. The feeback of x, y an z

3 positions, velocities in x, y an z irection an the yaw rate was obtaine from the Mocap system. The velocities were foun from the positions using linear regression for the most accurate approximation values. The gamepa was use for manual take-off an laning an also to switch between manual an autonomous moes. The trajectory was moifie using the following formula. n e ψ tra j a cos(ω 2 time) = b sin(ω 1 time) n (12) 0 Here, a an b efine the size of the trajectory. If ω 2 = ω 1, then a circular trajectory is efine. Similarly, if ω 2 = ω 1 2, then an eight figure trajectory is efine. The value n efines the change in altitue. This value of n was kept a constant 1 in this project for optimum performance. The time efines the lap time for each trajectory. The LQR controller esign was as follows. For circular trajectory, Q= R= K = For figure eight trajectory, Q= R= K= V. RESULTS AND DISCUSSION The AR Drone was flown in both circular an figure eight trajectories an the ata was save an extracte on ROS an then plotte on MALTAB for analysis. Fig. 5. Fig. 6. Fig. 7. Circular Trajectory. a = b = 0.8, lap time = 10sec Circular Trajectory. a = b = 0.8, lap time = 7sec Circular Trajectory. a = b = 0.8, lap time = 20sec

4 Fig. 8. Figure Eight Trajectory. a = 1, b = 0.75, lap time = 8sec Fig. 10. Error plot - Circular Trajectory. a = b = 0.8, lap time = 10sec Fig. 11. Error plot - Figure Eight Trajectory. a = 1, b = 0.75, lap time = 6sec Fig. 9. Figure Eight Trajectory. a = 1, b = 0.75, lap time = 3sec The AR Drone was initially positione at 180 yaw angle, while the esire heaing was 0. It can be observe in the error plots above that the heaing was controlle very accurately with almost critical amping an settling time of a respectable 15 secons. VI. CONCLUSION The circular trajectory with a lap time of 10 secons (Fig. 5) ha the least error as observe from the graph while the figure eight trajectory with a lap time of 6 secons (Fig. 8) ha the least error. This ifference of 4 secons for the best lap time was ue to the fact that the circular trajectory i not have abrupt movements like the figure eight where the quarotor ha to rapily change irection an angles to stay on the esire trajectory. Hence the error plots were obtaine for only the lap time when the error was minimum between the esire an actual trajectory. The following conclusions were reache after obtaining an plotting ata for repeate runs. The LQR controller esign was easy but the gain value K ha to be fine tune for optimal performance Since the response of the AR Drone is slow, only trajectories with higher lap times coul be achieve with low error values Because of the low power-to-weight ratio of the AR Drone, the quacopter gains reache saturation As the battery raine, the errors between the commane an actual trajectory increase The thrust comman ha to be converte to altitue comman for the AR Drone

5 VII. FUTURE WORK Implementation of the concept on a more agile Quacopter Comparison of Differential Flatness Concept with Pure Pursuit an/or Trajectory Shaping for optimal performance using the same inverse mapping technique Optimizing the current Mo-Cap setup for better utilization of its potential, since the system ha to be recalibrate time an again as the resiual errors grew with time. ACKNOWLEDGMENT Thanks to Dr. Rajnikant Sharma an Spencer Maughan for their help an support in the realization of this project. REFERENCES [1] Differential Flatness Base Control of a Rotorcraft For Aggressive Maneuvers, Jeff Ferrin, Robert Leishman, Rany Bear, an Tim McLain, 2011 IEEE/RSJ International Conference on Intelligent Robots an Systems September 25-30, San Francisco, CA, USA [2] Grey-Box System Ientification of a Qua rotor Unmanne Aerial Vehicle, Qianying Li, Master of Science thesis, Delft University of Technology, Netherlans [3] Rigi-boy ynamics, Carlos Murgua, Department of Mechanical Engineering, Dynamics an Control Group, Einhoven University of Technology, 5612 AZ Einhoven, Netherlans