ドローンの2Dシミュレーション

初めてのドローンシミュレーション 2Dマルチコプタシミュレーション⽤⽅程式 Motor 𝑄 = 𝐶# 𝜔" 𝑑𝑖 𝐿 + 𝑅𝑖 + 𝐾𝜔 = 𝑢 𝑑𝑡 𝑑𝜔 J! + 𝐷𝜔 + 𝑄 = 𝐾𝑖 𝑑𝑡 初めての⼈はオイラー法を慣れた⾔語で書いたりSimulinkを使い、これら8本の 微分⽅程式を解いて計算（シミュレーション）してみることをお勧めします ※式の形は左右共通 ※記号の説明はP.3参照 𝑞 𝜔B 𝜔A 𝜃 インダクタンスを無視 𝑇$ = 𝐶' 𝜔$ " 𝑇% = 𝐶' 𝜔% " 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 𝑈 𝑋 𝑑𝜔 𝐾" 𝐾 J! + 𝐷+ 𝜔 + 𝐶# 𝜔" = 𝑢 𝑑𝑡 𝑅 𝑅 Copter 𝑥 𝑧 𝑊 𝑌 Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 1

初めてのドローンシミュレーション つりあい(Trim)の計算 モータ・機体 変化がない状態、微分が0がつりあいです Trim状態の状態量は添字0を付けて表します。 定常直線⾶⾏状態を考えた場合 𝑞) = 0 そのほかの状態量は何らかの値を持ちます。（0を含む） Motor ※式の形は左右共通 Copter 𝑇$ = 𝐶' 𝜔$ " 𝑇% = 𝐶' 𝜔% " 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 𝑇$) − 𝑇%) =0 𝑑𝜔 𝐾" 𝐾 J! + 𝐷+ 𝜔 + 𝐶# 𝜔" = 𝑢 𝑑𝑡 𝑅 𝑅 𝐾" 𝐾 " 𝐷+ 𝜔) + 𝐶# 𝜔) = 𝑢) 𝑅 𝑅 𝑅 𝑢) = 𝐾 𝐾" 𝐷+ 𝜔) + 𝐶# 𝜔) " 𝑅 𝜔) = 𝜔$) = 𝜔%) 0 = −𝑀𝑔 sin 𝜃) + 𝐶& 𝑈) 0 = − 𝑇$) + 𝑇%) + 𝑀𝑔 cos 𝜃) + 𝐶( 𝑊) 𝑀𝑔 sin 𝜃) 𝐶& 𝑀𝑔 cos 𝜃) + 𝐶( 𝑊) 𝑇) = 𝑇$) = 𝑇%) = 2 " 𝑇) = 𝐶' 𝜔) 𝑈) = 𝜔) = 𝑀𝑔 cos 𝜃) + 𝐶( 𝑊) 2𝐶' 2

初めてのドローンシミュレーション つりあい(Trim)の計算 キネマティクス Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 𝑞) = 0 V*) = 𝑈) cos 𝜃) + 𝑊) sin 𝜃) V+) = − 𝑈) sin 𝜃) + 𝑊) cos 𝜃) 記号 𝑄：プロペラトルク[Nm] 𝐶! ：プロペラトルク係数 𝐿：モータコイルインダクタンス [H] 𝑅：モータコイル抵抗 [H] 𝑖：モータコイル電流 [A] 𝜔" ,𝜔# ：モータ⾓速度 [rad/s] 𝐾：モータトルク定数 [Nm/A][Vs/rad] 𝑖：モータコイル電流 [A] 𝐽$ ：モータ慣性モーメント [kgm2] 𝐷：モータ動粘性抵抗係数 [Nm rad/s] 𝑇" ,𝑇# ：プロペラ推⼒ [N] 𝐶% ：プロペラ推⼒係数 𝐽：機体慣性モーメント [kgm2] 𝑞 ：機体⾓速度(ピッチレート) [rad/s] 𝜃 ：機体姿勢⾓(ピッチ⾓) [rad] 𝑀 ：機体質量 [kg] 𝑙：モータ配置半径 [m] 𝑈 ：機体座標系横速度 [m/s] 𝑊 ：機体座標系縦速度 [m/s] 𝑀 ：機体質量 [kg] 𝐶& , 𝐶& ：空⼒係数 𝑋 , 𝑍 ：慣性座標系位置(横，縦)[m] 3

初めてのドローンシミュレーション 微⼩擾乱法による線形化 モータ 微⼩擾乱法とは、例えば、モータの⾓速度𝜔は トリム値𝜔) と微⼩変化量∆𝜔の和と考え 𝜔 = 𝜔) + ∆𝜔 として⽅程式を展開し微⼩量を無視し近似する Motor ※式の形は左右共通 𝑑𝜔 𝐾" 𝐾 " J! + 𝐷+ 𝜔 + 𝐶# 𝜔 = 𝑢 𝑑𝑡 𝑅 𝑅 𝑑∆𝜔 𝐾" J! + 𝐷+ (𝜔) + ∆𝜔) + 𝐶# 𝜔) + ∆𝜔 𝑑𝑡 𝑅 " 𝐾 = (𝑢) + ∆𝑢) 𝑅 𝑑∆𝜔 𝐾" 𝐾 J! + 𝐷+ (𝜔) + ∆𝜔) + 𝐶# (𝜔)" + 2𝜔) ∆𝜔 + ∆𝜔" ) = (𝑢) + ∆𝑢) 𝑑𝑡 𝑅 𝑅 トリム計算が反映さ 𝑑∆𝜔 𝐾" 𝐾 J! + 𝐷+ ∆𝜔 + 2𝐶# 𝜔) ∆𝜔 = ∆𝑢 れ簡単になる 𝑑𝑡 𝑅 𝑅 微少量の2乗は⾮常に⼩さ いとして無視する 𝑑∆𝜔 𝐾" 𝐾 J! + 𝐷+ + 2𝐶# 𝜔) ∆𝜔 = ∆𝑢 𝑑𝑡 𝑅 𝑅 4

初めてのドローンシミュレーション 微⼩擾乱法による線形化 機体⾓速度 Copter 𝑇$ = 𝐶' 𝜔$ " 𝑇% = 𝐶' 𝜔% " 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 推⼒を展開してモータ⾓速 𝑑∆𝑞 " " J = 𝐶' 𝜔$) + ∆𝜔$ − 𝜔%) + ∆𝜔% 𝑙 度で表している 𝑑𝑡 𝑑∆𝑞 微少量の2乗は⾮常に⼩さ J = 𝐶' 𝜔$) " + 2𝜔$) ∆𝜔$ + ∆𝜔$ " − 𝜔%) " + 2𝜔%) ∆𝜔% + ∆𝜔% " 𝑙 いとして無視する 𝑑𝑡 トリム計算が反映さ 𝑑∆𝑞 J = 𝐶' 2𝜔$) ∆𝜔$ − 2𝜔%) ∆𝜔% 𝑙 れ簡単になる 𝑑𝑡 J 𝑑∆𝑞 = 2𝜔) 𝐶' 𝑙 ∆𝜔$ − ∆𝜔% 𝑑𝑡 5

初めてのドローンシミュレーション 微⼩擾乱法による線形化 機体並進速度 Copter " " 𝑇$ = 𝐶' 𝜔$ 𝑇% = 𝐶' 𝜔% 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 計算メモ 𝑞) + ∆𝑞 𝑊) + ∆𝑊 = 𝑞) 𝑊) + 𝑞) ∆𝑊 + ∆𝑞𝑊) + ∆𝑞∆𝑊 ≈ 𝑊) ∆𝑞 sin/cosは加法定理を 𝑑∆𝑈 𝑀 + 𝑊) ∆𝑞 = −𝑀𝑔 sin 𝜃) + ∆𝜃 + 𝐶& 𝑈) + ∆𝑈 ⽤いて展開 𝑑𝑡 ※計算メモ参照 𝑑∆𝑈 sin/cosは近似 𝑀 = −𝑀𝑔 sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃 + 𝐶& 𝑈) + ∆𝑈 − 𝑀𝑊) ∆𝑞 𝑑𝑡 𝑑∆𝑈 𝑀 = −𝑀𝑔 sin 𝜃) + cos 𝜃) I ∆𝜃 + 𝐶& 𝑈) + ∆𝑈 − 𝑀𝑊) ∆𝑞 𝑑𝑡 𝑀 𝑑∆𝑈 = −𝑀𝑔 cos 𝜃) I ∆𝜃 + 𝐶& ∆𝑈 − 𝑀𝑊) ∆𝑞 𝑑𝑡 6

初めてのドローンシミュレーション 微⼩擾乱法による線形化 機体並進速度 Copter " " 𝑇$ = 𝐶' 𝜔$ 𝑇% = 𝐶' 𝜔% 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 計算メモ 𝑞) + ∆𝑞 U) + ∆𝑈 = 𝑞) 𝑈) + 𝑞) ∆𝑈 + ∆𝑞𝑈) + ∆𝑞∆𝑈 ≈ 𝑈) ∆𝑞 𝑑∆𝑊 𝑀 − 𝑈) ∆𝑞 = −𝐶' 𝜔$) " + 2𝜔$) ∆𝜔$ + ∆𝜔$ " − 𝜔%) " + 2𝜔%) ∆𝜔% + ∆𝜔% " 𝑑𝑡 ※計算メモ参照 +𝑀𝑔 cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃 + 𝐶( 𝑊) + ∆𝑊 𝑀 𝑀 𝑑∆𝑊 = −𝐶' 𝜔$) " + 2𝜔$) ∆𝜔$ − 𝜔%) " + 2𝜔%) ∆𝜔% 𝑑𝑡 +𝑀𝑔 cos 𝜃) + sin 𝜃) I ∆𝜃 + 𝐶( 𝑊) + ∆𝑊 + 𝑀𝑈) ∆𝑞 𝑑∆𝑊 = −2𝜔) 𝐶' ∆𝜔$ − ∆𝜔% + 𝑀𝑔 sin 𝜃) I ∆𝜃 + 𝐶( ∆𝑊 + 𝑀𝑈) ∆𝑞 𝑑𝑡 7

初めてのドローンシミュレーション 微⼩擾乱法による線形化 ピッチ⾓ Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 J 𝑑∆𝜃 = ∆𝑞 𝑑𝑡 8

初めてのドローンシミュレーション 微⼩擾乱法による線形化 ⽔平位置 Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 𝑑∆𝑋 𝑉*) + = 𝑑𝑡 𝑑∆𝑋 𝑉*) + = 𝑑𝑡 𝑑∆𝑋 𝑉*) + = 𝑑𝑡 𝑑∆𝑋 𝑉*) + = 𝑑𝑡 メモ ∆𝑋や∆𝑍に関しては微少量ではなく初期状態からの変化 量と捉えた⽅が良い 計算メモ 𝑑𝑋 𝑑(𝑋) + ∆𝑋) 𝑑𝑋) 𝑑∆𝑋 𝑑∆𝑋 = = + = 𝑉*) + 𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑈) + ∆𝑈 cos(𝜃) + ∆𝜃) + (𝑊) + ∆𝑊) sin(𝜃) + ∆𝜃) 𝑈) + ∆𝑈 cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃 + (𝑊) + ∆𝑊) sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃 𝑈) + ∆𝑈 cos 𝜃) + sin 𝜃) I ∆𝜃 + (𝑊) + ∆𝑊) sin 𝜃) + cos 𝜃) I ∆𝜃 𝑈) cos 𝜃) + 𝑈) sin 𝜃) I ∆𝜃 + cos 𝜃) I ∆𝑈 + (𝑊) sin 𝜃) + 𝑊) cos 𝜃) I ∆𝜃 + sin 𝜃) I ∆𝑊) 𝑑∆𝑋 = cos 𝜃) I ∆𝑈 + sin 𝜃) I ∆𝑊 + 𝑈) sin 𝜃) + 𝑊) cos 𝜃) ∆𝜃 𝑑𝑡 9

初めてのドローンシミュレーション 微⼩擾乱法による線形化 垂直位置 Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 メモ ∆𝑋や∆𝑍に関しては微少量ではなく初期状態からの変化 量と捉えた⽅が良い 𝑑∆𝑍 = − 𝑈) + ∆𝑈 sin(𝜃) + ∆𝜃) + (𝑊) + ∆𝑊) cos(𝜃) + ∆𝜃) 𝑑𝑡 𝑑∆𝑍 = − 𝑈) + ∆𝑈 sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃 + (𝑊) + ∆𝑊) cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃 𝑑𝑡 𝑑∆𝑍 = − 𝑈) + ∆𝑈 sin 𝜃) + cos 𝜃) I ∆𝜃 + (𝑊) + ∆𝑊) cos 𝜃) + sin 𝜃) I ∆𝜃 𝑑𝑡 𝑑∆𝑍 = − 𝑈) sin 𝜃) + 𝑈) cos 𝜃) I ∆𝜃 + sin 𝜃) I ∆𝑈 + (𝑊) cos 𝜃) + 𝑊) sin 𝜃) I ∆𝜃 + cos 𝜃) I ∆𝑊) 𝑑𝑡 𝑑∆𝑍 = − sin 𝜃) I ∆𝑈 + cos 𝜃) I ∆𝑊 + − 𝑈) cos 𝜃) + 𝑊) sin 𝜃) ∆𝜃 𝑑𝑡 10

初めてのドローンシミュレーション 微⼩擾乱法による線形化 まとめ 線形化前 Motor ※式の形は左右共通 " 𝑑𝜔 𝐾 J! + 𝐷+ 𝑑𝑡 𝑅 𝜔 + 𝐶# 𝜔" 𝐾 = 𝑢 𝑅 Copter 𝑇$ = 𝐶' 𝜔$ " 𝑇% = 𝐶' 𝜔% " 𝑑𝑞 J = 𝑇$ − 𝑇% 𝑙 𝑑𝑡 𝑑𝑈 𝑀 + 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶& 𝑈 𝑑𝑡 𝑑𝑊 𝑀 − 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊 𝑑𝑡 Kinematics 𝑑𝜃 J =𝑞 𝑑𝑡 𝑑𝑋 = 𝑈 cos 𝜃 + 𝑊 sin 𝜃 𝑑𝑡 𝑑𝑍 = −𝑈 sin 𝜃 + 𝑊 cos 𝜃 𝑑𝑡 線形化された微分⽅程式から状態空間表現でドローンモデルを表し現代制御理 論等での解析や設計を多なうことができる 線形化後 Motor " ※式の形は左右共通 𝑑∆𝜔 𝐾 𝐾 J! + 𝐷+ + 2𝐶# 𝜔) ∆𝜔 = ∆𝑢 𝑑𝑡 𝑅 𝑅 Copter 𝑑∆𝑞 J = 2𝜔) 𝐶' 𝑙 ∆𝜔$ − ∆𝜔% 𝑑𝑡 𝑑∆𝑈 𝑀 = −𝑀𝑔 cos 𝜃) I ∆𝜃 + 𝐶& ∆𝑈 𝑑𝑡 𝑑∆𝑊 𝑀 = −2𝜔) 𝐶' ∆𝜔$ − ∆𝜔% + 𝑀𝑔 sin 𝜃) I ∆𝜃 + 𝐶( ∆𝑊 𝑑𝑡 Kinematics 𝑑∆𝜃 J = ∆𝑞 𝑑𝑡 𝑑∆𝑋 = cos 𝜃) I ∆𝑈 + sin 𝜃) I ∆𝑊 + 𝑈) sin 𝜃) + 𝑊) cos 𝜃) ∆𝜃 𝑑𝑡 𝑑∆𝑍 = − sin 𝜃) I ∆𝑈 + cos 𝜃) I ∆𝑊 + − 𝑈) cos 𝜃) + 𝑊) sin 𝜃) ∆𝜃 𝑑𝑡 11