세종대학교 항공우주공학과 유도항법제어연구실

Presentation on theme: "세종대학교 항공우주공학과 유도항법제어연구실"— Presentation transcript:

1 세종대학교 항공우주공학과 유도항법제어연구실
LEC05. 드론의 자세제어기 설계 -2 세종대학교 항공우주공학과 유도항법제어연구실

2 Contents Drone Modeling Review Controller Design Strategy
Step 1 : Nonlinear Model Linearization PX4 Attitude Controller Structure Step 2 : Attitude Controller Design ( Analytical Approach ) Step 3 : Checking Control Performance ( Simulation ) Implementation & SIL/PIL/HIL Step 4 : Checking Control Performance ( Flight Test ) Step 5 : Verification of the Simulator ( Control + Model ) last week

3 Nonlinear model linearization
𝑢 = 1 𝑚 𝑣𝑟 −𝑤𝑞 −𝑔𝑠𝑖𝑛𝜃 𝑣 = 1 𝑚 (𝑤𝑝 −𝑢𝑟 )+𝑔𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 𝑤 = 1 𝑚 (𝑢𝑞 −𝑣𝑝− 𝑈 𝑇 )+𝑔𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃 𝑝 = 𝐼 𝑌𝑌 − 𝐼 𝑍𝑍 𝐼 𝑋𝑋 𝑞𝑟+ 1 𝐼 𝑋𝑋 𝑈 𝜙 + 1 𝐼 𝑋𝑋 𝐽 𝑅 𝑞 Ω 𝑟 𝑞 = 𝐼 𝑍𝑍 − 𝐼 𝑋𝑋 𝐼 𝑌𝑌 𝑝𝑟+ 1 𝐼 𝑌𝑌 𝑈 𝜃 + 1 𝐼 𝑌𝑌 𝐽 𝑅 𝑝 Ω 𝑟 𝑟 = 𝐼 𝑋𝑋 − 𝐼 𝑌𝑌 𝐼 𝑍𝑍 𝑝𝑞+ 1 𝐼 𝑍𝑍 𝑈 𝜓 Thrust from Rotor Gravity Rotor Gyro Effect Torque from Rotor Linearized Equation of Motion Δ 𝑢 =−𝑔Δ𝜃 Δ 𝑣 = 𝑔Δ𝜙 Δ 𝑤 =− Δ𝑈 𝑇 𝑚 Δ 𝑝 = 1 𝐼 𝑋𝑋 Δ𝑈 𝜙 Δ 𝑞 = 1 𝐼 𝑌𝑌 Δ 𝑈 𝜃 Δ 𝑟 = 1 𝐼 𝑍𝑍 Δ 𝑈 𝜓

4 Attitude Controller Design -1
∅𝑑 ∅ 𝟏 𝑰 𝑿𝑿 𝟏 𝒔 𝟐 𝐷 𝑠 = 𝑘 𝑝 𝐷 𝑠 = 𝑘 𝑝 + 𝑘 𝑑 𝑠 𝐷 𝑠 = 𝑘 𝑝 + 𝑘 𝑑 𝑠+ 𝑘 𝑖 1 𝑠 P controller can’t stabilize the system : Harmonic oscillation

5 Attitude Controller Design -2
Example : Roll Controller - 1 Design Criteria natural freq. : 𝜔 𝑛 ≈7𝑟𝑎𝑑/𝑠 damping ratio : 𝜁≈0.6 Δ 𝜙 = 1 𝐼 𝑋𝑋 Δ 𝑈 2 ( 𝐽 𝑥 =0.0086𝑘𝑔∙ 𝑚 2 ) Δ 𝑈 2 = 𝑘 1 𝑘 2 𝜙 𝑑 −𝜙 − 𝜙 Δ 𝜙 + 𝑘 1 𝐼 𝑋𝑋 Δ 𝜙 + 𝑘 1 𝑘 2 𝐼 𝑋𝑋 Δ𝜙= 𝑘 1 𝑘 2 𝐼 𝑋𝑋 Δ 𝜙 𝑑 Δ 𝜙 + 𝐾 𝑑 Δ 𝜙 + 𝐾 𝑝 Δ𝜙= 𝐾 𝑝 Δ 𝜙 𝑑 if) 2𝜁 𝜔 𝑛 = 𝐾 𝑑 , 𝜔 𝑛 2= 𝐾 𝑝 𝐾 𝑝 =49, 𝐾 𝑑 =8.4 𝒌 𝟏 =𝟎.𝟎𝟕, 𝒌 𝟐 =𝟔.𝟎𝟐 𝒌 𝟐 𝒌 𝟏 𝟏 ∅ ∅𝑑 𝟏 𝑰 𝑿𝑿 𝟏 𝒔 𝟏 𝒔 Δ𝑝=Δ 𝜙 Δ𝑞=Δ 𝜃 Δ𝑟=Δ 𝜓

6 Controller & Model Simulation -1
Example : Roll Controller - 2 (linear Model) 𝒌 𝟐 𝒌 𝟏 𝟏 ∅ ∅𝑑 𝟏 𝑰 𝑿𝑿 𝟏 𝒔 𝟏 𝒔

7 Controller & Model Simulation -2
Example : Roll Controller Performance Check -1 (linear Model) Dynamic Performance Specifications transient settling time : 1sec Overshoot ≤ 15% Design Criteria natural freq. : 𝜔 𝑛 ≈7𝑟𝑎𝑑/𝑠 damping ratio : 𝜁≈0.6 0.5 0.7 𝑀 𝑝 = 𝑒 −𝜋𝜁/ 1− 𝜁 2 𝑡 𝑟 ≅ 1.8 𝜔 𝑛 𝑡 𝑠 ≅ 4 𝜁 𝜔 𝑛

8 Controller & Model Simulation -3
Example : Roll Controller Performance Check -2 (linear Model) Dynamic Performance Specifications transient settling time : 1sec Overshoot ≤ 15% 𝑴 𝒑 =11.06 [%] ≤ 1% 𝒕 𝒔 = [sec] Overshoot : [%] Settling time : [sec] 𝒕 𝒓 = [sec]

9 Controller & Model Simulation -4
Example : Roll Controller Performance Check -3 (nonlinear Model)

10 Controller & Model Simulation -5
Example : Roll Controller Performance Check -4 (nonlinear Model) 𝜏=0.025 1st order System 2nd order System 𝜏=0.010 𝜏=0.025 𝝉(motor) 를 줄임. Gain Tuning

11 Implementation & Simulation
HIL 완료 될 시, 비행 시험 가능! SIL : 모든 Dynamic 및 제어기를 시뮬레이션. PIL : FCC의 프로그램 검증. - HIL : Dynamic을 제외한 실 H/W를 시뮬레이션 안에 포함하여 시뮬레이션.

12 Flight Test -1 Example : Control performance checking Prediction Error Method(PEM) -1 Prediction Error Method 를 통한 동적 시스템 모델링  Least Square Method를 이용하여 를 최소화 Black box Model

13 Flight Test -2 Example : Control performance checking Prediction Error Method(PEM) -2

14 Flight Test -3 Example : Hexa-copter Test (Tarot 680)
using 1st or 2nd order Model Comparison -1 Desired Attitude ( Flight Test Data ) 1st order Model OUT Flight Test Simulation Model Output 비교 1. Dynamic 모델 및 시뮬레 이션 환경 verification. 2. Controller Performance verification

15 Flight Test -4 Example : Hexa-copter Test (Tarot 680)
using 1st or 2nd order Model Comparison -2

16 Flight Test -5 Example : Hexa-copter Test (Tarot 680)
using 1st or 2nd order Model Comparison -3

17 Flight Test -6 Example : Hexa-copter Test (Tarot 680)
using 1st or 2nd order Model Comparison -4

18 Verification of the simulator
if) PIL/HIL 수행이 끝났다면, 1. Model 검증 확인 2. Model Parameter 수정 및 Model의 추가적인 Term 고려 3. Model이 달라졌다면, SIL/PIL 을 다시 수행. 경우에 따라서 HIL 수행. HW 또는 SW가 바뀌거나, 추가 되었을때…