Part 02: Dynamic Modeling of Quadcopters Unmanned Aerial Systems Multicopter UAS : Guidance, Navigation, and Control Hong Sung-Kyung GNCLab. Goals - In this lecture, we will model a quadcopter (DJI F450) - Steps for the creation of the model: GNCLab. Why dynamic modeling? GNCLab. Simulations? Mission Simulation Aerodynamic Simulation Dynamic Simulation GNCLab. Modeling of Quadcopters (Overview) ▪ The model of quadcopter consist of three main parts 𝑽𝒊 Motor Dynamics Aerodynamics 𝝎𝒎 T: Thrust (N) D: Drag Moment (N·m) 𝐹, 𝑀 Body Dynamics & Kinematics 𝑉,ሶ 𝝎ሶ 𝟏 𝒔 𝑉, 𝝎 GNCLab. 1.Motor Dynamics Torque 𝝉 = 𝑲𝒕 𝒊 Back emf 𝒆 = 𝑲𝒆 𝝎 𝑲𝒕 : torque constant (N〮m/A) 𝑲𝒆 : electromotive force(emf) constant (V〮s or V/RPM) Ex : Parameter Value Motor & Propeller Inertia J = 2.8 × 10−5 [𝑘𝑔 ∙ 𝑚2 ] Motor resistance R = 0.117 [𝑂ℎ𝑚] Motor constant K = 960[𝑅𝑃𝑀/𝑉] GNCLab. 1. Motor Dynamics… + − + 𝛕 − 𝑱𝒎 𝒅𝝎 = 𝛕 − 𝒃𝒇 𝛚 𝒅𝒕 𝑱𝒎 𝒅𝝎 = 𝑲𝒕 𝒊 − 𝒃𝒇 𝛚 𝒅𝒕 where, 𝒊 = Solving i using KVL assume that the electrical dynamics are fast with respect to the mechanical dynamics 𝑳≈𝟎 𝒗𝒂 −𝑲𝒆 𝝎 𝑹 GNCLab. Supplementary 1. Motor Dynamics… 𝝉 = 𝑱𝒎 𝒅𝝎 + 𝒃𝒇 𝝎 𝒅𝒕 ▪ Mechanical and Electrical equivalents Parameter Mechanical Electrical Applied Torque 𝜏 VTORQ Velocity 𝜔 i Inertia 𝐽𝑚 L Friction 𝑏𝑓 R GNCLab. Activity #1 1. Motor Dynamics… 𝒗𝒊 − 𝑲𝒆 𝝎 𝑹 𝒅𝝎 = (𝑲𝒕 𝒊 − 𝒃𝒇 𝛚)/𝑱𝒎 𝒊= 𝒗𝒊 𝒅𝒕 𝟏 𝒔 𝝎 function MotorSpeedsDer = fcn(InputVoltage, Drag, KMotor, IMotor, RMotor, MotorSpeeds) % Calculate the Current Uind = KMotor*MotorSpeeds; i = (InputVoltage-Uind)/RMotor; % Calculate the derivative of the motor speed MotorSpeedsDer = (KMotor*i-Drag)/IMotor; 𝑲𝒕 = 𝑲𝒆 ? ? GNCLab. Supplementary 1. Motor Dynamics… ▪ Electrical Power in 𝑷 = 𝒗𝒂 𝒊 = 𝑲𝒆 𝝎 + 𝒊𝑹 𝒊 Conservation of energy ▪ Mechanical Power out 𝑷 = 𝝉𝝎 = 𝑲𝒕 𝒊𝝎 Electrical Power in = Mechanical power out +Losses This is “True” if and only if 𝑲𝒆 = 𝑲𝒕 GNCLab. Activity #2 1. Motor Dynamics… 𝒗𝒊 − 𝑲𝒆 𝝎 𝑹 𝒅𝝎 = (𝑲𝒕 𝒊 − 𝒃𝒇 𝛚)/𝑱𝒎 𝒊= 𝑱𝒎 𝒅𝒕 𝒅𝝎 𝑲𝒕 𝑲𝒆 𝑲𝒕 = − 𝒃𝒇 + 𝝎+ 𝒗 𝒅𝒕 𝑹 𝑹 𝒊 𝑱𝒎 𝑹 𝒅𝝎 𝑲𝒕 = −𝝎 + 𝒗 𝑹𝒃𝒇 + 𝑲𝒕 𝑲𝒆 𝒅𝒕 𝑹𝒃𝒇 + 𝑲𝒕 𝑲𝒆 𝒊 What happen if pwm(1ms~2ms) input? 𝝉 𝒌𝒔 𝝉 𝝉 𝒅𝝎 = −𝝎 + 𝒌𝒔 𝒗𝒊 𝒅𝒕 𝒅𝝎 = −𝝎 + 𝒌𝒑𝒘𝒎 △ 𝒑𝒘𝒎𝒊 𝒅𝒕 𝒗𝒊 𝑮𝒎𝒐𝒕𝒐𝒓 (𝒔) = 𝒌𝒔 𝝉𝒔 + 𝟏 𝝎 GNCLab. Supplementary 1. Motor Dynamics… 𝝉 𝒅𝝎 = −𝝎 + 𝒌𝒔 𝒗𝒊 𝒅𝒕 1) Homogeneous 2) Particular 𝝉𝝎ሶ + 𝝎 = 𝟎 0 = −𝝎 + 𝒌𝒔 𝒗𝒊 assume 𝜔 = 𝐶𝑒 𝑠𝑡 , assume 𝜔 = 𝐴 for t → ∞ 𝜏𝐶𝑠𝑒 𝑠𝑡 + 𝐶𝑒 𝑠𝑡 = 0, ∴ 𝝎 = 𝑪𝒆−𝒕/𝝉 𝑠 = −1Τ𝜏 0 = −𝐴 + 𝑘𝑠 𝑣𝑖 , 𝐴 = 𝑘𝑠 𝑣𝑖 ∴ 𝝎 = 𝒌𝒔 𝒗𝒊 3) Total 𝝎 = 𝑪𝒆−𝒕/𝝉 + 𝒌𝒔 𝒗𝒊 , if 𝜔 0 = 0, then 𝐶 = − 𝑘𝑠 𝑣𝑖 𝒕 ∴ 𝝎(𝒕) = 𝒌𝒔 𝒗𝒊 𝟏 − 𝒆−𝝉 𝝎𝒔𝒔 = 𝝎(𝒕)ȁ𝒕→∞ = 𝒌𝒔 𝒗𝒊 GNCLab. ▪ Supplementary First Order System (t ) = 1 − e − (t / ) − t (t ) = 1 − e 99.3% 98.2% output signal 0.632 95% 86.5% ( ) = 1 − e −1 = 0.632 (2 ) = 1 − e −2 = 0.865 (4 ) = 1 − e −4 = 0.982 63.2% t time constant : how fast the system approaches a new steady state condition after being disturbed GNCLab. Supplementary 1. Motor Dynamics… 𝒗𝒊 − 𝑲𝒆 𝝎 𝒊= 𝑹 𝒅𝝎 = (𝑲𝒕 𝒊 − 𝒃𝒇 𝛚)/𝑱𝒎 𝒅𝒕 𝑑 Drag=𝟐𝒅𝒘𝟎 ∆𝒘 𝑏 GNCLab. Activity #3 1. Motor Dynamics… ▪ How to generate input voltage to achieve desired speed? SpeedVoltage Conversion 𝝎𝒅 𝒗𝒊 𝒗𝒊 − 𝑲𝒆 𝝎 𝑹 𝒅𝝎 = (𝑲𝒕 𝒊 − 𝒃𝒇 𝛚)/𝑱𝒎 𝒊= 𝒅𝒕 𝟏 𝒔 𝝎 𝒗𝒊 − 𝑲𝒆 𝝎 − 𝐃𝐫𝐚𝐠 𝑹 𝑹 𝒗𝒊 = 𝑲𝒆 𝝎𝒅 + 𝐃𝐫𝐚𝐠 𝑲𝒕 𝟎 = 𝑲𝒕 function Voltage = VoltageConversion(DesiredMotorSpeed,KMotor,RMotor,Drag_coef) % Convert DesiredMotorSpeed to Voltage Voltage = KMotor*DesiredMotorSpeed + RMotor/KMotor*Drag_coef*DesiredMotorSpeed^2; GNCLab. 1. Motor Dynamics… Write speed set point Read actual speed Identified dynamics of all rotor components (ESC/BLDC-Motor/ Propeller) GNCLab. 1. Motor Dynamics… Motor Dynamics Block Diagram GNCLab. Modeling of Quadcopters (Overview) 𝑽𝒊 Motor Dynamics Aerodynamics 𝝎𝒎 𝐹, 𝑀 Body Dynamics & Kinematics 𝑣,ሶ 𝝎ሶ 𝟏 𝒔 GNCLab. 2. AeroDynamics Blade Configuration/ Parameter 𝝎𝒎 𝑇, 𝑄 𝑄 = 𝑑𝜔2 T Thrust 𝑻= 𝟏 𝟏 𝝆𝑪𝑻 𝑨𝒗𝟐 = 𝝆𝑪𝑻 𝑨(𝑹𝝎)𝟐 = 𝒃𝝎𝟐 𝟐 𝟐 𝑸= 𝟏 𝟏 𝑹𝝆𝑪𝑫 𝑨𝒗𝟐 = 𝑹𝝆𝑪𝑫 𝑨(𝑹𝝎)𝟐 = 𝒅𝝎𝟐 𝟐 𝟐 Drag Moment 19 𝑇 = 𝑏𝜔2 Q GNCLab. 2. AeroDynamics… ▪ How to get thrust coefficient and drag torque coefficient ? Rotor Test (using loadcell sensor) PWM signal : 1.15-1.80 Result : b=1.17678e-05 d=1.85419e-07 Q= 𝒅𝝎𝟐 𝑻 = 𝒃𝝎𝟐 GNCLab. Activity #4 1. Motor Dynamics… ▪ How to generate input voltage to achieve desired speed? function Voltage = VoltageConversion(DesiredMotorSpeed,KMotor,RMotor,Drag_coef) % Convert DesiredMotorSpeed to Voltage Voltage = KMotor*DesiredMotorSpeed + RMotor/KMotor*Drag_coef*DesiredMotorSpeed^2; For hovering, set DesiredMotorSpeed as. 𝝎𝒅 = 𝒔𝒒𝒓𝒕 𝟏 𝒎𝒈/𝐛 𝟒 GNCLab. 2. AeroDynamics… ▪ Forces & Moments acting on the Quadcopter come from propeller Blade Configuration/ Parameter 𝑇, 𝑄 𝝎𝒎 Conversion to Forces & Moments acting on the Quadcopter Forces Moments Gravity Sum of thrust forces Sum of drag torques Thrust induced moments 22 𝐹𝑜𝑟𝑐𝑒𝑠 (F) 𝑀𝑜𝑚𝑒𝑛𝑡𝑠 (M) GNCLab. 2. AeroDynamics… cross configuration Aerodynamics 𝝎1 𝝎𝒎 l 𝐹, 𝑀 𝝎1 𝝎4 𝝎2 𝝎3 𝒇𝒙 𝟎 𝟎 𝑭𝒃 = 𝒇 𝒚 = 𝟐 𝟐 −𝒃(𝝎𝟏 + 𝝎𝟐 + 𝝎𝟐𝟑 + 𝝎𝟐𝟒 ) 𝒇𝒛 𝒍𝒃(𝝎𝟐𝟒 − 𝝎𝟐𝟐 ) 𝒎𝒙 𝑴𝒃 = 𝒎𝒚 = 𝒍𝒃(𝝎𝟐𝟏 − 𝝎𝟐𝟑 ) 𝒎𝒛 𝒅(−𝝎𝟐𝟏 + 𝝎𝟐𝟐 − 𝝎𝟐𝟑 + 𝝎𝟐𝟒 ) 23 Sum of thrust forces Thrust induced moments Sum of drag torques GNCLab. Activity #5 2. AeroDynamics… 𝒇𝒙 𝟎 𝒃 𝟎 𝑭 = 𝒇𝒚 = 𝟐 𝟐 −𝒃(𝝎𝟏 + 𝝎𝟐 + 𝝎𝟐𝟑 + 𝝎𝟐𝟒 ) 𝒇𝒛 24 𝒍𝒃(𝝎𝟐𝟒 − 𝝎𝟐𝟐 ) 𝒎𝒙 𝑴𝒃 = 𝒎𝒚 = 𝒍𝒃(𝝎𝟐𝟏 − 𝝎𝟐𝟑 ) 𝒎𝒛 𝒅(−𝝎𝟐𝟏 + 𝝎𝟐𝟐 − 𝝎𝟐𝟑 + 𝝎𝟐𝟒 ) GNCLab. Supplementary 2. AeroDynamics… AeroDynamics Block Diagram 25 GNCLab. Supplementary 2. AeroDynamics… ▪ Flight time estimation with given propeller Flight Time ▪ Thrust of one propeller in hover ? Battery Capacity (Wh or Ah) Propeller Radius 26 ▪ Ideal Power used to produce that thrust ? ; Momentum Theory ▪ Actual Power ? ; Figure of Merit GNCLab. Supplementary ▪ Momentum Theory ▪ ▪ ▪ Conservation of Mass - No change of speed across rotor/propeller disc - Change in pressure Conservation of Momentum - Thrust from Prop. in function of inlet and exit velocity Conservation of Energy Induced velocity 𝑽𝒊 = ▪ Thrust force formula : 𝑻 = 𝟐𝝆𝑨𝑹 𝑽 + 𝑽𝒊 𝑽𝒊 𝑻 = 𝟐𝝆𝑨𝑹 𝑽𝒊 𝟐 ▪ Ideal Power used to produce thrust : 𝑷= 𝟑 𝑻𝟐 𝟐𝝆𝝅𝑹𝟐 𝑻 𝟐𝝆𝝅𝑹𝟐 (in hover case) 𝑷 = 𝑻(𝑽 + 𝑽𝒊 ) (in hover case) 𝑽 + 𝑽𝒊 GNCLab. Supplementary ▪ Figure of Merit Defining the efficiency of a rotor ▪ Figure of Merit : - Ideal Power : Can be calculated using the momentum theory - Actual Power : Includes profile drag, blade-tip vortex, … ▪ Can be used to compare different propellers with same disc loading GNCLab. Activity #6 Ex: The Quadcopter(F450) under consideration has the following parameters. Parameter Value Mass m= 1.762 [kg] Propeller radius R= 0.119 [m] Figure of Merit 0.4 Battery Capacity 𝐶𝑏𝑎𝑡𝑡 = 1500 [mAh] @ 14.8 V 𝐸𝑏𝑎𝑡𝑡 = 22.2 [Wh] (Energy) ※ 𝝆 = 𝟏. 𝟐 𝒌𝒈/𝒎𝟑 𝒈 = 𝟗. 𝟖𝟏[𝒎/𝒔𝟐 ] GNCLab. Problem : 1. Calculate the thrust of one propeller in hover. 2. Calculate the ideal power consumption of one propeller using the momentum theory. 3. Calculate the real power of the Quadcopter using the figure of merit and all propellers. 4. Calculate the maximum flight time with the given battery. (𝐸𝑏𝑎𝑡𝑡 = 10000 [mAh] @ 14.8 V) a. Calculate the energy in the battery. b. Calculate the time until the energy from the battery is used. GNCLab. Solution : 1) Thrust of one propeller in hover : (the total thrust = gravity force) 4.32 N 2) Ideal power consumption of one propeller (momentum theory) : 27.49W 3) The real power of the Quadcopter (FOM=0.8) 34.36W 4) Maximum flight time (with given battery) 𝑡𝑖𝑚𝑒 = 𝐸𝑏𝑎𝑡𝑡 /𝑃𝑇 = 0.16(ℎ) = 9.69 min 137.46W GNCLab. clear all clc %% Load parameters m=1.762; % kg rho=1.2; % kg/m^3 g=9.81; % m/s^2 R=0.119; %m %% real power % Power at one propeller Preal = Pideal/FoM % Power for quadrotor Pt = 4*Preal FoM = 0.8; Cap = 1500; VBat = 14.8; %% Flight time % Battery energy Ebat = VBat*Cap/1000 % Wh % flight time until battery is empty time = Ebat/Pt % hour % time in minutes timemin = time*60 % min % Capacity (mAh) % Voltage Level (V) %% Thrust of one propeller Tprop = 1/4*m*g %N %% Ideal power from momentum theory % propeller disc Ap = R^2*pi; % ideal power at hover Pideal = Tprop^(3/2)/(sqrt(2*rho*Ap)) % Watt GNCLab. ▪ 쿼드콥터의 운동 GNCLab. GNCLab. GNCLab. GNCLab. GNCLab. Additional thrust necessary to provide the same vertical lift while moving Amount of thrust necessary to hover GNCLab. Modeling of Quadcopters (Overview) 𝝎𝒎𝒅 Motor Dynamics Aerodynamics 𝝎𝒎 𝐹𝑎𝑒𝑟𝑜 𝑀 𝐹𝑔𝑟𝑎𝑣𝑖𝑡𝑦 𝐹𝑡𝑜𝑡𝑎𝑙 Body Dynamics & Kinematics 𝑣,ሶ 𝝎ሶ 𝟏 𝒔 GNCLab. Ex: The Quadcopter under consideration has the following parameters. Parameter Value Mass m=1.762 Inertia I=diag( 0.02,0.02 ,0.025)[kgm2] Arm length L= 0.225 [m] Propeller radius R= 0.119 [m] Figure of Merit 0.8 Battery Capacity 𝐸𝑏𝑎𝑡𝑡 = 1500[Ah] @ 14.8 V Air density 𝜌= 1.2 [kg/m3] Gravity constant g=9.81 [m/s2] [kg] GNCLab. 4. Quadcopter Body Dynamics F, M Position Attitude input output GNCLab. ▪ Properties • Arm length l : Planar distance between CoG and rotor plane • Mass m : Total lift of mass • Inertia I : Assume symmetry in xz- and yz- plane l GNCLab. ▪ 6-DOF EOM ✓ Why EOM formulated in the body-fixed frame ? ▪ Body Frames - Newton’s EOM (with Transport Theorem) - Aerodynamic forces and torques - On-board sensors (like accelerometers and gyros) ▪ Earth Frames - GPS (position, ground speed, course angle) - Most mission requirements (loiter points, flight trajectories) - Map information GNCLab. ▪ Coordinate System ① ② ④ ③ GNCLab. ▪ Coordinate Transformation File:Plane.svg 𝝍 → 𝜽 → 𝝓 회전 for E to B 𝝓 → 𝜽 → 𝝍 회전 for B to E GNCLab. ▪ Coordinate Transformation xB 𝜃 XE YE XE yB xB zB yB YE ZE According to Associative law of matrix multiplication, ABC=A(BC)=(AB)C zB ZE GNCLab. ▪ Coordinate Transformation - The inverse transformation : 𝑬 𝑹𝑩 𝑬 = 𝑹𝑩 −𝟏 = 𝑹𝑬𝑩 𝑻 유도 & Orthonormal 증명 Supplementary p u A= q v p 2 + q 2 = 1, u 2 + v 2 = 1 normal pu + qv = 0 orthogonal GNCLab. ▪ Coordinate Transformation B to E 𝑬 𝑹𝑩 𝑬 = 𝑹𝑩 E to B 𝑻 𝑻 𝑻 𝑹𝑩 𝑬 = 𝑹𝒙 (𝝓) ∙ 𝑹𝒚 (𝜽) ∙ 𝑹𝒛 (𝝍) 𝑻 GNCLab. ▪ Coordinate Transformation Ex) Gravity Components g x 0 B g = R E 0 y g z g body 𝑹𝑩 𝑬 earth a x = − g sin a y = g sin cos a z = g cos cos 기속도계를 이용하여 자 세를 계산 할 수 도…. GNCLab. Activity #7 ▪ Coordinate Transformation Ex) Gravity Forces (body component) 𝒇𝒈𝒙 , 𝒇𝒈𝒚 , 𝒇𝒈𝒛 GNCLab. Activity #8 ▪ Coordinate Transformation 𝒅 𝒑𝒏 𝒑𝒆 𝒅𝒕 −𝒉 𝒖 𝒗 𝒘 𝒖 𝒗 𝒘 GNCLab. ▪ Coordinate Transformation GNCLab. ▪ Angular Velocity - Sprit angular velocity into three basic rotations 𝑹𝑻𝒙 (𝝓) 𝑹𝑻𝒚 (𝜽) 𝒙𝑬 𝒚𝑬 𝒛𝑬 𝒙𝟐 𝒚𝟐 𝒛𝟐 𝒙𝟏 𝒚𝟏 𝒛𝟏 𝝍ሶ 𝜽ሶ 𝒙𝑩 𝒚𝑩 𝒛𝑩 𝝓ሶ 𝟎 𝒑 𝟎 𝝓ሶ 𝒒 = 𝟎 +𝑹𝑻𝒙 𝝓 𝜽ሶ + 𝑹𝑻𝒙 (𝝓) ∙ 𝑹𝑻𝒚 (𝜽) 𝟎 𝒓 𝝍ሶ 𝟎 𝟎 𝝎𝒓𝒐𝒍𝒍 𝑩 𝝎𝒑𝒊𝒕𝒄𝒉 𝑩 𝝎𝒚𝒂𝒘 𝑩 GNCLab. ▪ Angular Velocity… 𝟎 𝒑 𝟎 𝝓ሶ 𝒒 = 𝟎 +𝑹𝑻𝒙 𝝓 𝜽ሶ + 𝑹𝑻𝒙 (𝝓) ∙ 𝑹𝑻𝒚 (𝜽) 𝟎 𝒓 𝝍ሶ 𝟎 𝟎 GNCLab. ▪ Angular Velocity… 𝝓ሶ 1 sin tan cos 𝜽ሶ = 0 𝝍 ሶ 0 sin sec cos tan p − sin q cos sec r rate gyro 출력 By integrating these equations, one can determine the Euler angles …. GNCLab. Activity #9 ▪ Angular Velocity… GNCLab. ▪ 6-DOF EOM 2) Rigid Body Dynamics Transport Theorem Rigid body can be completely described by the velocity at the CoG and the rotational velocity GNCLab. ▪ 6-DOF EOM ▪ Rigid Body Dynamics : Translation 𝑚 𝑑𝑽 =𝑭 𝑑𝑡𝑖 𝑑𝑳 =𝑭 𝑑𝑡𝑖 (L : Linear Momentum) 𝑑𝑽 𝑚 +𝝎×𝑽 =𝑭 𝑑𝑡𝑏 𝑽 = 𝑢𝐢 + 𝑣𝐣 + 𝑤𝐤 𝒊 𝝎×𝐯= 𝑝 𝑢 𝑑𝐕 = 𝑢𝐢ሶ + 𝑣𝐣ሶ + 𝑤𝐤 ሶ 𝑑𝑡𝑏 ※ 58 𝒋ሶ 𝑞 𝑣 𝒌 𝑟 𝑤 𝒇 = 𝒇𝑎𝑒𝑟𝑜 + 𝒇𝑔𝑟𝑎𝑣𝑖𝑡𝑦 GNCLab. Activity #10 2. AeroDynamics… 𝒇 = 𝒇𝒂𝒆𝒓𝒐 + 𝒇𝒈𝒓𝒂𝒗𝒊𝒕𝒚 59 GNCLab. Activity #11 𝑚 𝑭𝒕𝒐𝒕𝒂𝒍 → 𝑑𝒗 +𝝎×𝒗 =𝑭 𝑑𝑡𝑏 𝑑𝒗 𝐹 = −𝝎×𝒗 𝑑𝑡𝑏 𝑚 60 𝒗ሶ 𝟏 𝒔 𝒗 GNCLab. ▪ 6-DOF EOM ▪ Rigid Body Dynamics : Rotational 𝑑𝒉 =𝐌 𝑑𝑡𝑖 (h : Angular Momentum) 𝒉=𝐉∙𝛚 𝝎 = 𝑝𝒊 + 𝑞𝒋 + 𝑟𝒌 𝒉 = 𝐽𝑥 𝑝𝒊 + 𝐽𝑦 𝑞𝒋 +𝐽𝑧 𝑟𝒌 61 GNCLab. ▪ 6-DOF EOM ▪ Rigid Body Dynamics : Rotational 𝑑𝒉 =𝐌 𝑑𝑡𝑖 𝑑𝒉 = 𝐽𝑥 𝑝𝒊ሶ + 𝐽𝑦 𝑞𝒋 ሶ +𝐽𝑧 𝑟𝒌 ሶ 𝑑𝑡𝑏 𝑑𝒉 +𝝎×𝒉 = 𝑴 𝑑𝑡𝑏 𝒊 𝝎×𝒉= 𝑝 𝐽𝑥 𝑝 62 𝒋ሶ 𝑞 𝐽𝑦 𝑞 𝒌 𝑟 𝐽𝑧 𝑟 GNCLab. Activity #12 𝒅𝒉 +𝝎×𝒉 =𝑴 𝒅𝒕𝒃 𝑴 𝝎ሶ = 𝑱−𝟏 𝑴 − 𝝎 × 𝑱 ∗ 𝝎 63 𝝎ሶ 𝟏 𝒔 𝝎 GNCLab. ▪ 6-DOF EOM 3) 6 DOF Model for the quadcopter 64 GNCLab. ▪ 6-DOF EOM 1) Quadcopter State Variables Position x (North) Earth Frame Position y (East) altitude Body Frame න Roll angle Earth Frame Pitch angle Yaw angle Roll rate Body Frame Pitch rate Yaw rate 65 න GNCLab. Simplified 6-DOF EOM (in hover) 1) Translation 𝑢ሶ = −𝑔𝑠𝑖𝑛𝜃 𝑣ሶ = 𝑔𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 4 𝒇 = 𝒇𝑎𝑒𝑟𝑜 + 𝒇𝑔𝑟𝑎𝑣𝑖𝑡𝑦 1 𝑤ሶ = 𝑔𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃 + −𝑇𝑖 𝑚 𝑖=1 4 0 𝒇𝑎𝑒𝑟𝑜 = 0 𝑖=1 −𝑇𝑖 −𝑠𝑖𝑛𝜃 0 𝒇𝑔𝑟𝑎𝑣𝑖𝑡𝑦 = 𝑅𝐸𝐵 0 = 𝑚𝑔 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 𝑚𝑔 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃 𝑢 𝑿ሶ = 𝑅𝐸𝐵 𝑣 𝑤 GNCLab. Simplified 6-DOF EOM (in hover) 1) Rotation 1 𝑝ሶ = 𝜏𝜙 𝐽𝑥 𝑞ሶ = 1 𝜏 𝐽𝑦 𝜃 𝑟ሶ = 1 𝜏 𝐽𝑧 𝜓 𝜏𝜙 = 𝑙𝑏(𝜔42 − 𝜔22 ) 𝜏𝜃 = 𝑙𝑏(𝜔12 − 𝜔32 ) 𝜏𝜓 = 𝑑(−𝜔12 + 𝜔22 − 𝜔32 + 𝜔42 ) GNCLab. ▪ How to get attitude ? Gyro or Accelerometer Integrator Gyro sensor X 축 기준 0deg-> -45 deg-> 0deg ->45deg -> 0 deg-> -45deg -> 0deg Acceler ometer 𝒂𝒚 ) 𝒂𝒛 −𝒂𝒙 𝝓 = 𝒔𝒊𝒏−𝟏 ( ) 𝒈 𝜽 = 𝒕𝒂𝒏−𝟏 ( ACCELEROMETER : 힘에 민감하게 반응 GYRO SENSOR : 시간이 지남에 따라 적분 오차 발생 GNCLab. 6-DOF Modeling (Matlab/Simulink) GNCLab. QuadcopterModel GNCLab. Landing detection Z–axis Position simulation result Z-axis Velocity simulation result GNCLab. Landing detection GNCLab. Landing detection 𝒛≥𝟎 𝒗𝒛 > 𝟎 GNCLab. Landing detection Z–axis Position simulation result Z-axis Velocity simulation result GNCLab. GNCLab. GNCLab. Supplementary Dynamical Systems : Systems where the effects of actions do not occur immediately State : A collection of variables that completely characterizes the motion of a system • 𝒙(𝑡) gives the value of these states over time • Evolution of these states over time is often given by a set of governing ordinary differential equations 𝑥ሷ 𝑡 = 𝑢(𝑡) Second-order system GNCLab. Supplementary Example : Mass Spring System 𝑢(𝑡) 𝑚𝑥ሷ 𝑡 + 𝑘𝑥(𝑡) = 𝑢(𝑡) 𝑥1 = 𝑥 𝑥2 = 𝑥ሶ State Equation 𝑘 1 𝑥ሷ 𝑡 = − 𝑥 𝑡 + 𝑢(𝑡) 𝑚 𝑚 0 𝑥ሶ 1 = −𝑘 𝑥ሶ 2 𝑚 1 𝑥1 0 1 0 𝑥2 + 𝑚 u GNCLab. Homework #11 Exercise Assignment (quadcopter modeling) 1. Write in the field Omega_1 the formula of the propeller speed such that hovering is achieved. This formula depends on the quadcopter mass: m, the thrust factor b and the gravity g. Insert also the formulas for Omega_2,3,4. 2. Run the simulation again. See the results by plotting all the states. You can also open the different scopes in Simulink 3. Double-click on the “Motor Dynamics Switch” to include the dynamics of the motors, run the simulation once again and plot all the states Explain what you observe. 4. Bring the switches back to their original position 5. Change the “Initial conditions”. Start the simulation with that initial condition. (ex: attitudeInit = [20*pi/180;0;0];) Explain what you observe.
0
advertisement
Download
advertisement
Add this document to collection(s)
You can add this document to your study collection(s)
Sign in Available only to authorized usersAdd this document to saved
You can add this document to your saved list
Sign in Available only to authorized users