Robot Dynamics - Rotary Wing UAS: Control of a Quadrotor

Transcription

1 Robot Dynamics Rotary Wing AS: Control of a Quadrotor V Marco Hutter, Roland Siegwart and Thomas Stastny Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

2 Contents Rotary Wing AS. Introduction - Design and Propeller Aerodynamics. Propeller Analysis and Dynamic Modeling 3. Control of a Quadrotor. Rotor Craft Case Study Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

3 Multi-body Dynamics General Formulation T T M b, g J F S Generalized coordinates M Mass matrix b, Centrifugal and Coriolis forces g Gravity forces act Actuation torque F External forces (end-effector, ground contact, propeller thrust,...) ex J ex S Jacobian of external forces Selection matrix of actuated joints ex ex act 7..6 Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 3

4 Controller Synthesis Rotary Wing AS: Control of a Quadrotor Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

5 Stability Analysis Dynamic stability Calculate the operational point (equilibrium point) Derivative of the states must be zero (static point) x f x, u x and u Linearize the dynamic equations Calculate the eigenvalues of the state matrix A Exponentially stable if Stable if df x Axu ; A, dx eig( A) i i x, u x, u o df du o i i Stable equilibrium Exponentially stable equilibrium nstable equilibrium Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

6 Control of a Quadrotor General Overview Goal: Move the Quadrotor in space Six degree of freedom Four motor speeds as system input nder-actuated system Not all states are independently controllable Fly maneuvers Control using full state feedback Assume all states are known Needs an additional state estimator Assumption Neglect motor dynamics Low speeds only dominate aerodynamic forces considered linearization around small attitude angles Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

7 Change of momentum and spin in the body frame Position in the inertial frame and the Attitude Forces and moments Aerodynamics and gravity 7..6 Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7 Modeling of Rotorcraft ody Dynamics M F ω I ω v ω ω v I x m me 3 3 v C x E E ω J r Aero Aero G M M F F F T G E Aero i i E F C F mg T E 3x3 : Identity matrix 3 Aero T i i i l T T M M Q l T T Q

8 Modeling of Rotorcraft Rotational Velocity Relation between Tait-ryan angles and angular velocities J ω r J r Linearized relation at hover J ω r, Trust and drag of Propellers T b Q d i i pi, i i pi, sin cos sin cos sin cos cos Singularity for = 9 o Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

9 Control of a Quadrotor Analysis of the Dynamics me x v 3 3 ω m v I ω ωi ω Rotational dynamics near hover I p qr( I I ) lb( ) xx yy zz p, p, I q rp( I I ) lb( ) yy zz xx p, p,3 Izzr d( p, p, p,3 p, ) p J r q r pi, Full control over all rotational speeds Not dependent on position states F M p Ixx p qr I zz I yy I q I q rpi I r Izz r pqiyy Ixx yy xx zz b : thrust constant d : drag constant l : distance of propeller from the CoG : rotational speed of propellor i Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

10 Control of a Quadrotor Analysis of the Dynamics me 3x3 v ω m v I ω ωi ω Translational dynamics in hover mu m( rv qw) sin mg mv m( pw ru) sincos mg mw m( qu pv) coscos mg b( ) u x C E v CE v w F p u mqw rv m v q m v mru pw M r w mpv qu Only direct control of the body z velocity se attitude to control full position p, p, p,3 p, b : thrust constant : rotational speed of propellor i pi, sin T F C sincos mg mg coscos E G E Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

11 Control of a Quadrotor Virtual Control Input Define virtual control inputs to simplify the equations Get decoupled and linear inputs Moments along each axis I xx p qr( Iyy Izz ) I q rp( I I ) yy zz xx I r zz Total thrust mu m( rv qw) sin mg mv m( pw ru) sincos mg mw m( qu pv) coscos mg 3 lb( ) p, p, lb( ) 3 p, p,3 d( ) p, p, p,3 p, b( p, p, p,3 p, ) Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

12 Control of a Quadrotor Hierarchical Control Rotational dynamics independent of translational dynamics se a cascaded control structure Inner loop: Control of the attitude Input:, 3, Outer loop: Control of the position Input:, ϕ, θ 3,, p q r X Y Z u v w Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

13 Control of a Quadrotor Linearization of Attitude Dynamics Linearize attitude dynamic at hover Equilibrium point: p q r ; ; mg 3 I p qr( I I ) xx yy zz I q pr( I I ) yy zz xx I r J zz r p q r 3 I xx 3 I yy I zz This model has no coupling We use 3 individual controller Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

14 Control of a Quadrotor PID Control PID control strategy Generic non model based control Consist of three terms P - Proportional I - Integral D - Derivative Different tuning methods Ziegler-Nichols method Pole placement Widely used r(t) e(t) C u(t) t ut () ket () k etdt () k Plant p i d det () dt y(t) Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

15 Control of a Quadrotor PID Attitude Control Consider roll subsystem I xx P controller can t stabilize the system Harmonic oscillation I xx ( des ) se a PD controller k p kp( des ) kd( des ) I xx Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

16 Control of a Quadrotor Attitude Control Design 3 separate control loops additional thrust input Tdes ( ) k k ( ) k k 3 ( ) k k des proll droll des ppitch dpitch des pyaw dyaw Aspect on the implementation Get angle derivatives from transformation of the body angular rates Avoid integral element in controller d d d 3 real real real Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

17 Control of a Quadrotor Parameter Tuning k p is chosen to meet desired convergence time k d is chosen to get desired damping In theory any convergence time can be reached Actuator dynamics allow for a limited control signal bandwidth Closed loop system dynamics must respect the bandwidth of actuator dynamics Actuator signals shall not saturate the motors Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

18 Transformation of virtual control inputs to motor speeds Rewrite virtual control inputs in matrix form Invert matrix to get motor speeds Limit motor speeds to feasible (positive) values 7..6 Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 8 Control of a Quadrotor Control Allocation 3,,3,, d lb b d lb b d lb b d lb b p p p p,,3,, 3 p p p p d d d d lb lb lb lb b b b b

19 Control of a Quadrotor Altitude Control A possible control flow for altitude control ϕ des,θ des,ψ des, 3, ω i,des z des T z Control attitude and the altitude Attitude controller to control the angles Parallel control loop. Altitude controller to control the horizontal thrust and nonlinear transformation to calculate the corresponding thrust input Robot Dynamics - Rotary Wing AS: Control of a Quadrotor

20 Control of a Quadrotor Altitude Control Rewrite translational dynamics in the inertial frame X V V C g E E m Only look at the altitude dynamics Virtual Control PD controller for the height Transformation to the body frame z gcoscos m z g TZ TZ cos cos m T k ( z z) k z mg z p des d T z cos cos Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

21 Control of a Quadrotor Position Control Possible control structure for full position control Ψ des x des y des z des T x T y T z ϕ, θ 3 ω i,des Control position and yaw angle Hierarchical controller Outer loop: Position control Thrust vector control. Desired thrust vector in inertial frame Transform to thrust and roll and pitch angles Inner loop: Attitude control Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6

22 Control of a Quadrotor Position Control Thrust vector control Assume that the control input is the thrust in the inertial frame Inertial position system dynamics x y z T T m T x y z g se 3 separate PD controller for each direction Transform to desired thrust, roll and pitch Tx sin cos T T Tx Ty Tz C ( z, ) T sin E y T Tz cos cos Robot Dynamics - Rotary Wing AS: Control of a Quadrotor 7..6