「マルチコプタの運動と制御」基礎のきそ

「マルチコプタの 運動と制御」 基礎のきそ こうへい Ver 0.26 2023.7.8 1

※マルチコプタ
と表記していま
すが。基本のク
アッドコプタに
ついて解説して
います。

𝑦

2023.7.8 こうへい

𝑥

𝑖=

𝑇:Thrust
𝑧

motor

𝜔：Angular velocity
𝑄：Propeller Torque

※

𝐿は⾮常に⼩さいので無視すると。

𝑒 − 𝐾𝜔
(2)
𝑅

よって、モータ＋プロペラ系の回転の運動⽅程式は次になります

𝐾"
𝐾
"
𝐽𝜔̇ + 𝐷 +
𝜔 + 𝐶# 𝜔 + 𝑄$ = 𝑒 (3)
𝑅
𝑅

※機体座標についてです。ホバリ

剛体としての機体の運動方程式 ング時を想定して移動時の空⼒は
考慮していません
FL
FR
𝑥
並進運動
𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = −𝑚𝑔 sin 𝜃 − 𝐶% 𝑢"
推力・トルクと角速度の関係
"
𝑚
𝑣
̇
+
𝑟𝑢
−
𝑝𝑤
=
𝑚𝑔
cos
𝜃
sin
𝜙
−
𝐶
𝑣
𝑦
(4)
&
"
RR
RL
𝑇 = 𝐶! 𝜔
𝐶! ：推⼒係数
𝑚 𝑤̇ + 𝑝𝑣 − 𝑞𝑢 = 𝑚𝑔 cos 𝜃 cos 𝜙 − 𝐶' 𝑤 "
(1)
𝐶" ：トルク係数
𝑙
𝑄 = 𝐶# 𝜔"
−𝑇() − 𝑇(* − 𝑇)) − 𝑇)*
𝑦
回転運動 (5)
推⼒とトルクは⾓速度の２乗に⽐例します。
𝐼+ 𝑝̇ + 𝐼, − 𝐼- 𝑞𝑟 = 0.5𝑙(𝑇(* + 𝑇)* − 𝑇() − 𝑇)) )
𝑧
𝐼- 𝑞̇ + 𝐼+ − 𝐼, 𝑟𝑝 = 0.5𝑙(𝑇() + 𝑇(* − 𝑇)) − 𝑇)* )
モータ+プロペラ系の運動方程式と回路の微分方程式 前後、左右対称
なため慣性乗積
𝐼, 𝑟̇ + 𝐼- − 𝐼+ 𝑝𝑞 = 𝐾 𝑖() + 𝑖)* − 𝑖(* − 𝑖))
𝐽𝜔̇ + 𝐷𝜔 + 𝐶# 𝜔" + 𝑄$ = 𝐾𝑖 マルチコプタのモータはBLDC
は０とします。
モータが⼀般的ですがブラシ付
きモータと⾒做します。
𝐿𝚤̇̇ + 𝑅𝑖 + 𝐾𝜔 = 𝑒
2
2
𝐽:慣性モーメント[kgm2]
𝐿：コイルインダクタンス[H]
𝐷：動粘性抵抗係数[Nms/rad]
𝑅：コイル抵抗[Ω]
𝑄# ：摩擦トルク[Nm]
𝑒：印加電圧[V]
𝐾：トルク定数・逆起電圧定数[Nm/A][Vs/rad]

𝐼$ , 𝐼% ,𝐼& :慣性モーメント[kgm ]
𝑢, 𝑣, 𝑤：機体の速度[m/s]
𝑝, 𝑞, 𝑟：機体の⾓速度[rad/s]
𝑚：機体の質量[kg]
𝐶* ,𝐶+ ,𝐶, ：空⼒係数

「マルチコプタの運動と制御」基礎のきそ ver 0.26

𝑔：重⼒加速度 [m/s ]
𝑇'( , 𝑇') , 𝑇(( , 𝑇() ：推⼒[N]※下向き正
𝑖'( , 𝑖') , 𝑖(( , 𝑖() ：モータトルク[Nm]
𝜙, θ, 𝜓：オイラー⾓[rad]

2／？

2023.7.8 こうへい 先に𝑞!を求めた場合 オイラー角の微分 𝜙̇ = 𝑝 + 𝑞 sin 𝜙 tan 𝜃 + 𝑟 cos 𝜙 tan 𝜃 𝜃̇ = 𝑞 cos 𝜙 − 𝑟 sin 𝜙 𝜓̇ = (𝑞 sin 𝜙 + 𝑟 cos 𝜙)⁄cos 𝜃 ピッチ Pitch クォータニオンの微分 𝑞!̇ = 0.5(−𝑝𝑞" − 𝑞𝑞# + 𝑟𝑞$) 𝑞"̇ = 0.5(𝑝𝑞! + 𝑟𝑞# − 𝑞𝑞$) 𝑞#̇ = 0.5(𝑞𝑞! − 𝑟𝑞" + 𝑝𝑞$) 𝑞$̇ = 0.5(𝑟𝑞! + 𝑞𝑞" − 𝑟𝑞#) 𝜙 ロール 𝑝 Roll 𝑥 (6) (7) 𝜃 𝑞 𝑟𝜓 𝑦 ヨー Yaw 𝐸!" 𝐸"" 𝐸#" 𝑧 cos 𝜃 cos 𝜓 𝐸!# 𝐸"# = sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 𝐸## cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓 𝜙 = tan%" 𝐸#$⁄𝐸$$ # + 𝐸# 𝜃 = tan%" − 𝐸"$9 𝐸#$ $$ %" 𝜓 = tan 𝐸"#⁄𝐸"" cos 𝜃 sin 𝜓 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓 𝐸!! 𝑬 = 𝐸"! 𝐸#! 𝐸!" 𝐸"" 𝐸#" 𝐸!# 𝐸"# 𝐸## 𝑞$" + 𝑞!" − 𝑞"" − 𝑞#" = 2(𝑞! 𝑞" − 𝑞$ 𝑞# ) 2(𝑞! 𝑞# + 𝑞$ 𝑞" ) − sin 𝜃 sin 𝜙 cos 𝜃 cos 𝜙 cos 𝜃 (9) 方向余弦行列とクォータニオン (10) 2(𝑞! 𝑞" + 𝑞$ 𝑞# ) " 𝑞$ − 𝑞!" + 𝑞"" − 𝑞#" 2(𝑞" 𝑞# − 𝑞$ 𝑞! ) 方向余弦行列からクォータニオン (11) 𝑞/ = (𝐸01 −𝐸10 )⁄4𝑞. 𝑞0 = (𝐸/1 +𝐸1/ )⁄4𝑞. 𝑞1 = (𝐸/0 +𝐸0/ )⁄4𝑞. 先に𝑞#を求めた場合 𝑞0 = 0.5 1 − 𝐸// + 𝐸00 − 𝐸11 𝑞. = (𝐸1/ −𝐸/1 )⁄4𝑞0 𝑞/ = (𝐸/0 +𝐸0/ )⁄4𝑞0 𝑞1 = (𝐸01 +𝐸10 )⁄4𝑞0 慣性座標系→機体座標系の座標変換行列𝑬 （方向余弦行列:DCM）とオイラー角 (8) 𝐸!! 𝐸 𝑬 = "! 𝐸#! 𝑞. = 0.5 1 + 𝐸// + 𝐸00 + 𝐸11 先に𝑞"を求めた場合 𝑞/ = 0.5 1 + 𝐸// − 𝐸00 − 𝐸11 𝑞. = (𝐸01 −𝐸10 )⁄4𝑞/ 𝑞0 = (𝐸/0 +𝐸0/ )⁄4𝑞/ 𝑞1 = (𝐸/1 +𝐸1/ )⁄4𝑞/ 先に𝑞$を求めた場合 𝑞1 = 0.5 1 − 𝐸// − 𝐸00 + 𝐸11 𝑞. = (𝐸/0 −𝐸0/ )⁄4𝑞1 𝑞/ = (𝐸/1 +𝐸1/ )⁄4𝑞1 𝑞0 = (𝐸01 +𝐸10 )⁄4𝑞1 速度の慣性空間への座標変換(オイラー角表現) 𝑋̇ - = 𝑢 cos 𝜃 cos 𝜓 + 𝑣 sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 +𝑤(cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓) 𝑌-̇ = 𝑢 cos 𝜃 sin 𝜓 + 𝑣 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 +𝑤(cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓) 𝑍̇- = −𝑢 sin 𝜃 + 𝑣 sin 𝜙 cos 𝜃 + 𝑤 cos 𝜙 cos 𝜃 (12) 速度の慣性空間への座標変換(クォータニオン表現) 2(𝑞! 𝑞# − 𝑞$ 𝑞" ) 2(𝑞" 𝑞# + 𝑞$ 𝑞! ) " 𝑞$ − 𝑞!" − 𝑞"" + 𝑞#" (10)の対⾓項どうしの和差でクォータニオンを⼀つ特定できる。その際、０になら ないものを選択する。実際は、対⾓項の全ての組み合わせを試して、求められた物 の中から最⼤のものを選んだのち、他のクォータニオンを以下のように算出する。 𝑋̇ - = 𝑞.0 + 𝑞/0 −𝑞00 −𝑞20 𝑢 + 2 𝑞/ 𝑞0 + 𝑞. 𝑞1 𝑣 + 2 𝑞/ 𝑞1 − 𝑞. 𝑞0 𝑤 𝑌-̇ = 2 𝑞/ 𝑞0 − 𝑞. 𝑞1 𝑢 + 𝑞.0 − 𝑞/0 + 𝑞00 − 𝑞10 𝑣 + 2 𝑞0 𝑞1 + 𝑞. 𝑞/ 𝑤 𝑍̇- = 2 𝑞/ 𝑞1 + 𝑞. 𝑞0 𝑢 + 2 𝑞0 𝑞1 − 𝑞. 𝑞/ 𝑣 + 𝑞.0 − 𝑞/0 − 𝑞00 + 𝑞10 𝑤 (13) ※(12)(13)は⽅向余弦⾏列の逆⾏列（⽅向余弦⾏列の場合転置で済む）と速度ベク トルの積という形で簡単に記述できますが、実際にコーディングするときは展開形 式が使いやすいと思い上記のように記述しました。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 3／？

2023.7.8 こうへい 微小擾乱法等による運動方程式等の線形化 本資料では定常状態（平衡状態）の状態量（定常値）に添字０をつけて表現します。 マルチコプタの⽔平速度が𝑉$ の⽔平定常⾶⾏を想定しているので、多くの定常値は ０になります。そうすると、以下のように、各状態量は定常値に微⼩な変化（擾 乱）が増えたものと表現できます。微⼩な変化にはΔをつけて表記します。 𝑢 = 𝑢. + ∆𝑢 𝑣 = 𝑣. + ∆𝑣 𝑤 = 𝑤. + ∆𝑤 𝑒 = 𝑒. + ∆𝑒 𝜔 = 𝜔. + ∆𝜔 𝑋- = 𝑋- . + ∆𝑋𝑌- = 𝑌- . + ∆𝑌Z- = 𝑍- . + ∆𝑍- 𝜔) 𝑝 = 𝑝. + ∆𝑝 𝑞 = 𝑞. + ∆𝑞 𝑟 = 𝑟. + ∆𝑟 𝜙 = 𝜙. + ∆𝜙 𝜃 = 𝜃. + ∆𝜃 𝜓 = 𝜓. + ∆𝜓 (14) 𝑋 . 𝑤. 𝑞 𝑌 𝑉/ 𝜃. 𝑢. = 𝑉0 cos 𝜃. 𝑤. = 𝑉0 sin 𝜃. 𝑢. 𝑧 図 定常⽔平⾶⾏ 𝜔( . 𝑥 釣り合いの式 定常状態では多く微分値は０となるので各運動⽅程式から次の釣り合いの⽅程式が 導かれる。ただし、𝑋% の微分値は⽔平速度の𝑉$ となります。 モータ＋プロペラ 𝐷+ 3& ( 3 𝜔. + 𝐶" 𝜔. 0 + 𝑄# = ( 𝑒. ※モータ＋プロペラは四組ありま すが、ここでは１本だけ⽰します。 以降、必要に応じて場所を⽰す添 字FR,FL,RR,RLをつけ区別します。 機体並進運動 𝑚 𝑞. 𝑤. − 𝑟. 𝑣. = −𝑚𝑔 sin 𝜃. − 𝐶* 𝑢. 0 𝑚(𝑟. 𝑢. − 𝑝. 𝑤. ) = 𝑚𝑔 cos 𝜃. sin 𝜙. − 𝐶+ 𝑣. 0 𝑚 𝑝. 𝑣. − 𝑞. 𝑢. = 𝑚𝑔 cos 𝜃. cos 𝜙. 0 0 0 0 −𝐶! 𝜔'(. + 𝜔'). + 𝜔((. + 𝜔(). − 𝐶, 𝑤. 0 機体回転運動 0 0 0 0 𝐼& − 𝐼% 𝑞. 𝑟. = 0.5𝑙𝐶! (𝜔'(. + 𝜔((. − 𝜔'). − 𝜔(). ) 0 0 0 0 𝐼$ − 𝐼& 𝑟. 𝑝. = 0.5𝑙𝐶! (𝜔((. + 𝜔(). − 𝜔'(. − 𝜔'). ) 𝐾 𝐼% − 𝐼$ 𝑝. 𝑞. = 𝑒 + 𝑒() . − 𝑒') . − 𝑒(( . − 𝐾 𝜔'( . + 𝜔() . − 𝜔') . − 𝜔(( . 𝑅 '( . オイラー⾓ 𝑝. + 𝑞. sin 𝜙. tan 𝜃. + 𝑟. cos 𝜙. tan 𝜃. = 0 𝑞. cos 𝜙. − 𝑟. sin 𝜙. = 0 (𝑞. sin 𝜙. + 𝑟. cos 𝜙. )⁄cos 𝜃. = 0 ⽔平位置 𝑉4 = 𝑢. cos 𝜃. cos 𝜓. + 𝑣. sin 𝜙. sin 𝜃. cos 𝜓. − cos 𝜙. sin 𝜓. +𝑤. (cos 𝜙. sin 𝜃. cos 𝜓. + sin 𝜙. sin 𝜓. ) 0 = 𝑢. cos 𝜃. sin 𝜓. + 𝑣. sin 𝜙. sin 𝜃. sin 𝜓. + cos 𝜙. cos 𝜓. +𝑤. (cos 𝜙. sin 𝜃. sin 𝜓. − sin 𝜙. cos 𝜓. ) 0 = −𝑢. sin 𝜃. + 𝑣. sin 𝜙. cos 𝜃. + 𝑤. cos 𝜙. cos 𝜃. 「マルチコプタの運動と制御」基礎のきそ ver 0.26 4／？

2023.7.8 こうへい ⽔平定常⾶⾏状態では 𝑣' = 𝑝' = 𝑞' = 𝑟' = 𝜙' = 𝜓' = 0 となるので、これらの式に反映させる。 また、編微分を⽤いて線形化する⽅法もある。関数𝑓(𝑥)を線形化すると以下となる 0 0 0 0 𝜔'(. + 𝜔'). + 𝜔((. + 𝜔(). = (𝑚𝑔 + 𝐶, 𝑤. 0 )⁄𝐶! 𝑓 𝑥 = 𝑓 𝑥. モータの⾓速度は全て同じ⾓速度𝜔' とすると / 𝜔. = 𝜔'(. = 𝜔'). = 𝜔((. = 𝜔(). = 0 8(73 & 5674( ,) & 03 4* + 4+ ((5674( ,) & ) 234* . ∆𝑥 以上を⽤いても同じ結論が得られます。微分を⽤いますが場合によってはこの⽅法 の⽅が簡単です。 5674( ,) & 4* したがって、⽔平定常⾶⾏時のモータへの⼊⼒電圧は以下となる。 𝑒. = 𝜕𝑓(𝑥) + 𝜕𝑥 ( 以上が、線形化されたモータ＋プロペラの運動⽅程式になります。同様にして他の 運動⽅程式等も線形化します。 + 𝑄# 3 ※この値は全てのモータで同⼀となる 微⼩擾乱法による線形化 これまで明らかになった運動⽅程式等に(14)を代⼊する。例えばモータ＋プロペラ であれば、次のようになる。 & ̇ + 𝐷 + 3 (𝜔. + ∆𝜔) + 𝐶" 𝜔. + ∆𝜔 𝐽∆𝜔 ( 0 3 + 𝑄# = ( (𝑒. + ∆𝑒) 上式に、釣り合いの式を適⽤すると & ̇ + 𝐷 + 3 ∆𝜔 + 𝐶" (2𝜔. ∆𝜔 + ∆𝜔0 ) = 3 ∆𝑒 𝐽∆𝜔 ( ( 更に、変化量の積や２乗は他の項に⽐して微⼩であるので無視すると 𝐾0 𝐾 ̇ + 𝐷+ 𝐽∆𝜔 + 2𝐶" 𝜔. ∆𝜔 = ∆𝑒 𝑅 𝑅 ※モータ＋プロペラは四組ありま すが、ここでは１本だけ⽰します。 必要に応じて場所を⽰す添字 FR,FL,RR,RLをつけ区別します。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 5／？

2023.7.8 こうへい

４つの舵面（入力）のミキシング
モータの線形化運動方程式
𝐾0
𝐾
̇
𝐽∆𝜔 + 𝐷 +
+ 2𝐶" 𝜔. ∆𝜔 = ∆𝑒
𝑅
𝑅

以上まで⾒てきたモデルについては４⼊⼒１出⼒システムに⾒えますが、以下のよ
うに⼊⼒として航空機の舵⾯に⾒⽴てた、スロットル、ロール舵、ピッチ舵、ヨー
舵を導⼊すると１⼊⼒１出⼒システムとなり古典制御理論で扱えます。またこれら
４つの⼊⼒からモータへの⼊⼒は⼀意に決定できます。

並進運動の線形化運動方程式
∆𝑢̇ = −𝑤. ∆𝑞 − 𝑔 cos 𝜃. ∆𝜃 − 2𝐶* 𝑢. ∆𝑢/𝑚
∆𝑣̇ = −𝑢. ∆𝑟 + 𝑤. ∆𝑝 + 𝑔 cos 𝜃. ∆𝜙 − 2𝐶+ 𝑣. ∆𝑣/𝑚
̇ = 𝑢. ∆𝑞 − 𝑔 sin 𝜃. W ∆𝜃 − 2𝐶! 𝜔. ∆𝜔'( + ∆𝜔') + ∆𝜔(( + ∆𝜔() ⁄𝑚
∆𝑤
+2𝐶, 𝑤. ∆𝑤/𝑚
回転運動の線形化運動方程式
𝐼$ ∆𝑝̇ = 𝑙𝐶! 𝜔. (∆𝜔') + ∆𝜔() − ∆𝜔'( − ∆𝜔(( )
𝐼% ∆𝑞̇ = 𝑙𝐶! 𝜔. (∆𝜔'( + ∆𝜔') − ∆𝜔(( − ∆𝜔() )

𝐼& ∆𝑟̇ =

𝐾
∆𝑒'( + ∆𝑒(; − ∆𝑒') − ∆𝑒(( − 𝐾 ∆𝜔'( + ∆𝜔(; − ∆𝜔') − ∆𝜔((
𝑅

スロットル

𝛿! = ∆𝑒'( +∆𝑒') + ∆𝑒(( + ∆𝑒()

ロール舵

𝛿< = −∆𝑒'( + ∆𝑒') − ∆𝑒(( + ∆𝑒()

ピッチ舵

𝛿- = ∆𝑒'( + ∆𝑒') − ∆𝑒(( − ∆𝑒()

ヨー舵

𝛿= = ∆𝑒'( − ∆𝑒') − ∆𝑒(( + ∆𝑒()

※スロットルは舵ではあ
りませんが、話の流れ上
舵として話させてもらい
ます。T,a,e,rの添字記号
はスロットル、エルロン、
エレベータ、ラダーを表
しています

以上を連⽴してモータ⼊⼒について解くと以下となる

右後モータ

∆𝑒'( = 𝛿! − 𝛿< + 𝛿- + 𝛿= ⁄4
∆𝑒') = 𝛿! + 𝛿< + 𝛿- − 𝛿= ⁄4
∆𝑒(( = 𝛿! − 𝛿< − 𝛿- − 𝛿= ⁄4

左後モータ

∆𝑒() = 𝛿! + 𝛿< − 𝛿- + 𝛿= ⁄4

右前モータ
左前モータ

※これをミキシングと⾔
います。

オイラー角の線形化微分方程式
∆𝜙̇ = ∆𝑝 + tan 𝜃. W ∆𝑟

∆𝜃̇ = ∆𝑞
1
̇ =
∆𝜓
∆𝑟
cos 𝜃.
位置に関する線形化微分方程式

∆𝑋̇ / = cos 𝜃! < ∆𝑢 + sin 𝜃! ∆𝑤 + 𝑤! cos 𝜃! + 𝑢! sin 𝜃! ∆𝜃 + 𝑉0
∆𝑌/̇ = 𝑢! cos 𝜃 < ∆𝜓 − 𝑤!∆𝜙 + 𝑤! sin 𝜃! < ∆𝜓 + ∆𝑣
∆𝑍̇/ = − sin 𝜃! < ∆𝑢 + cos 𝜃! < ∆𝑤 − (𝑢! cos 𝜃! + 𝑤! sin 𝜃!)∆𝜃

「マルチコプタの運動と制御」基礎のきそ ver 0.26

6／？

2023.7.8 こうへい

マルチコプタの伝達関数

並進運動

求めた線形化された運動⽅程式等を変動量の初期値０でラプラス変換して伝達関数
を求めます。通常変数は⼤⽂字で表しますが、元の⼩⽂字をそのまま使⽤します。

モータ＋プロペラ系

∆𝜔 =

𝐾
∆𝑒
𝐽𝑅𝑠 + 𝐷𝑅 + 𝐾 # + 2𝑅𝐶1 𝜔!

このモータの伝達関数を𝐺(𝑠)とします

𝐺(𝑠) =

𝐾
𝐽𝑅𝑠 + 𝐷𝑅 + 𝐾 # + 2𝑅𝐶1 𝜔!

1
−𝑤!∆𝑞 − 𝑔∆𝜃
𝑠 + 2𝐶9 𝑢!/𝑚
1
∆𝑣 =
−𝑢!∆𝑟 + 𝑤!∆𝑝 + 𝑔 cos 𝜃! ∆𝜙
𝑠 + 2𝐶: 𝑣!/𝑚
1
{𝑢 ∆𝑞 − 𝑔 sin 𝜃! < ∆𝜃
∆𝑤 =
𝑠 + 2𝐶2 𝑤!/𝑚 !
−2𝐶3 𝜔!𝐺(𝑠)∆𝛿3 /𝑚}

∆𝑢 =

回転運動

1 𝑙𝐶3 𝜔!
𝐺(𝑠)∆𝛿5
𝑠 𝐼4
1 𝑙𝐶3 𝜔!
∆𝑞 =
𝐺(𝑠)∆𝛿/
𝑠 𝐼6

𝐾
𝐷𝑅 + 𝐾 # + 2𝑅𝐶1 𝜔!
=
𝐽𝑅
𝑠+1
#
𝐷𝑅 + 𝐾 + 2𝑅𝐶1 𝜔!

∆𝑝 =

𝐾;
=
𝜏𝑠 + 1

∆𝑟 =

ここでは𝜏と𝐾, はモータ＋プロペラ系の時定数とゲインとなります。

𝜏=

𝐽𝑅
𝐷𝑅 + 𝐾 # + 2𝑅𝐶1 𝜔!

𝐾; =

𝐾
𝐷𝑅 + 𝐾 # + 2𝑅𝐶1 𝜔!

1 2𝐶1 𝜔!
𝐺(𝑠)∆𝛿8
𝑠 𝐼7

オイラー⾓と位置の伝達関数はそれぞれの線形化⽅程式の右辺を積分したものとな
る。線形化モデルのまとめに⽰す

「マルチコプタの運動と制御」基礎のきそ ver 0.26

7／？

2023.7.8 こうへい

マルチコプタの線形化モデル（まとめ）
モータ＋プロペラ系

𝐾;
𝑒
𝜏𝑠 + 1
𝐾;
𝐺(𝑠) =
𝜏𝑠 + 1
𝜔=

並進運動

姿勢
𝐽𝑅
𝜏=
"
𝐷𝑅 + 𝐾 + 2𝑅𝐶% 𝜔$
𝐾
𝐾& =
𝐷𝑅 + 𝐾 " + 2𝑅𝐶% 𝜔$

1
−𝑤!∆𝑞 − 𝑔∆𝜃
𝑠 + 2𝐶9 𝑢!/𝑚
1
∆𝑣 =
−𝑢!∆𝑟 + 𝑤!∆𝑝 + 𝑔 cos 𝜃! ∆𝜙
𝑠 + 2𝐶: 𝑣!/𝑚
1
{𝑢 ∆𝑞 − 𝑔 sin 𝜃! < ∆𝜃
∆𝑤 =
𝑠 + 2𝐶2 𝑤!/𝑚 !
−2𝐶3 𝜔!𝐺(𝑠)∆𝛿3 /𝑚}

∆𝑢 =

回転運動

1 𝑙𝐶3 𝜔!
𝐺(𝑠)∆𝛿5
𝑠 𝐼4
1 𝑙𝐶3 𝜔!
∆𝑞 =
𝐺(𝑠)∆𝛿/
𝑠 𝐼6

∆𝑝 =

∆𝑟 =

1 2𝐶1 𝜔!
𝐺(𝑠)∆𝛿8
𝑠 𝐼7

1
∆𝑝 + tan 𝜃! < ∆𝑟
𝑠
1
∆𝜃 = ∆𝑞
𝑠
1 1
∆𝜓 =
∆𝑟
𝑠 cos 𝜃!
∆𝜙 =

位置

1
cos 𝜃! < ∆𝑢 + sin 𝜃! ∆𝑤 + 𝑤! cos 𝜃! + 𝑢! sin 𝜃! ∆𝜃 + 𝑉0
𝑠
1
∆𝑌/ = 𝑢! cos 𝜃 < ∆𝜓 − 𝑤!∆𝜙 + 𝑤! sin 𝜃! < ∆𝜓 + ∆𝑣
𝑠
1
∆𝑍/ = − sin 𝜃! < ∆𝑢 + cos 𝜃! < ∆𝑤 − (𝑢! cos 𝜃! + 𝑤! sin 𝜃!)∆𝜃
𝑠
∆𝑋/ =

ホバリング状態を考える時は0がついている平衡状態での変数はすべて0のなるので線形化
モデルは⼤変簡単いなります。

「マルチコプタの運動と制御」基礎のきそ ver 0.26

8／？

2023.7.8 こうへい

マルチコプタのホバリング時の線形化モデル
モータ＋プロペラ系

𝐾;
𝑒
𝜏𝑠 + 1
𝐾;
𝐺(𝑠) =
𝜏𝑠 + 1
𝜔=

姿勢

ここで

𝐽𝑅
𝐷𝑅 +
+ 2𝑅𝐶% 𝜔$
𝐾
𝐾& =
𝐷𝑅 + 𝐾 " + 2𝑅𝐶% 𝜔$
𝜏=

𝐾"

並進運動

1
∆𝑢 = − 𝑔∆𝜃
𝑠
1
∆𝑣 = 𝑔∆𝜙
𝑠
𝐾,
∆𝑤 = −
𝛿
𝑠 𝜏𝑠 + 1 !

位置

ここで
2𝐶! 𝜔. 𝐾
𝐾, =
𝐷𝑅 + 𝐾 0 + 2𝑅𝐶" 𝜔. 𝑚

回転運動

∆𝑝 =

𝐾C
∆𝛿5
𝑠 𝜏𝑠 + 1

𝐾D
∆𝑞 =
𝛿
𝑠 𝜏𝑠 + 1 /
𝐾8
∆𝑟 =
𝛿
𝑠 𝜏𝑠 + 1 1

1
∆𝑝
𝑠
1
∆𝜃 = ∆𝑞
𝑠
1
∆𝜓 = ∆𝑟
𝑠

∆𝜙 =

ここで
𝐾' =
𝐾* =
𝐾, =

1
∆𝑢
𝑠
1
∆𝑌/ = ∆𝑣
𝑠
1
∆𝑍/ = ∆𝑤
𝑠
∆𝑋/ =

𝑙𝐶( 𝜔$ 𝐾

𝐷𝑅 + 𝐾 " + 2𝑅𝐶% 𝜔$ 𝐼)
𝑙𝐶( 𝜔$ 𝐾
𝐷𝑅 + 𝐾 " + 2𝑅𝐶% 𝜔$ 𝐼+
2𝐶% 𝜔$ 𝐾
𝐷𝑅 + 𝐾 " + 2𝑅𝐶% 𝜔$ 𝐼-

「マルチコプタの運動と制御」基礎のきそ ver 0.26

9／？

2023.7.8 こうへい 制御システム構築の際の注意点 簡単な姿勢制御システムの一例 線形化する際に微⼩擾乱法を⽤いていますが、以上のモデルで制御系を設計した場 合制御される量は変化量になることに注意してください。操作量として得られる量 も平衡状態を⽣み出す定常値からの変化量が算出されます。実際にマルチコプタに ⼊⼒する際は定常値との和を加える必要があります。通常制御系設計の際に描かれ るブロック線図ではこれは⽰されていないかもしれません。 姿勢制御システムの簡単な例を以下に⽰します。インナーループで⾓速度をPID制 御で安定化し、さらにアウターループは⽐例制御で姿勢⾓を制御します。以下は ピッチ制御のみですが、他の姿勢の制御も基本的には同じです。制御⼊⼒は最終的 にはミキシングして各モータに分配します。スロットルに関しては操縦装置からの 信号に適当にゲインをかけて𝛿- とします。 コントローラ ⽬標からの誤差 コント ローラ ∆𝑒 ＋ ＋ 𝑒. 𝐾>@ マルチ コプタ 角度指令値 定常値を加える − 𝐾A@ − 機体 𝐾>? 𝑠 𝛿- 𝐾> 𝑠(𝜏𝑠 + 1) 𝑞 1 𝑠 𝜃 𝐾*. 𝑠 ピッチ角制御系（ロール、ヨーも同様） スロットル指令値 𝐾!@ 𝛿! スラスト制御系 「マルチコプタの運動と制御」基礎のきそ ver 0.26 10／？

2023.7.8 こうへい 12 7 マルチコプタの数値例（ノミナルモデル） 以下に、机上で検討するため、実機を想定して（⾃分のマルチコプタを測定して） モデルとなる数値例を⽰したいと思います。 152 16 厚さ 20 38 質量 プロペラとモータ フレーム フレームの 慣性モーメント Ix= 8.64x10-4 kgm2 Iy= 8.64x10-4 kgm2 Iz= 1.47x10-3 kgm2 厚さ 180 36 46 18 28 180 30 脚（円柱） マルチコプタのレイアウト プロペラとモータの 慣性モーメント Ix= 5.02x10-6 kgm2 Iy= 5.02x10-6 kgm2 Iz= 8.12x10-6 kgm2 29 2 モータ 0.035×4=0.140kg プロペラ 0.01×4=0.040kg フレーム 0.190kg 脚 0.02×4=0.080kg 機体総質量 0.450kg バッテリー 0.260kg 全備重量 0.710kg 脚の 慣性モーメント Ix= 1.14x10-5 kgm2 Iy= 1.14x10-5 kgm2 Iz= 1.67x10-5 kgm2 ※プロペラの⼤きさは無視してます 16 50 バッテリーの 慣性モーメント Ix= 5.97x10-5 kgm2 Iy= 4.93x10-4 kgm2 Iz= 5.24x10-4 kgm2 150 バッテリー 慣性モーメント計算に関するメモ / マルチコプタの慣性モーメント Ix= 6.10x10-3 kgm2 Iy= 6.53x10-3 kgm2 Iz= 1.16x10-2 kgm2 ※Githubに計算のためのスクリプトを公 開してます。 https://github.com/kouhei1970/fundam ental_of_multicopter_control/blob/main /Numerical_sample.ipynb 一様な長方形の慣性モーメント /0 𝑚 𝑎0 + 𝑏0 一様な円柱の慣性モーメント 円の中心周り 一様な円柱の慣性モーメント 平行軸の定理 𝐼 = 𝐼B6 + 𝑚𝑑 0 「マルチコプタの運動と制御」基礎のきそ ver 0.26 / 0 𝑚𝑟 0 長手方向の中心周り / / 𝑚𝑟 0 + /0 𝑚𝐿0 2 11／？

• https://github.com/kouhei1970/fundam

2023.7.8 こうへい モータのパラメータ 152 16 38 プロペラとモータ 巻線インダクタンス L：3.7μH 巻線抵抗 R：0.12Ω トルク定数 K：3.28x10-3 Nm/A 動粘性抵抗係数 D：0.0 摩擦トルク Qf：0.0 マルチコプタの速度と角速度のボード線図 伝達関数のパラメータが決定されたので、それぞれの伝達関数のボード線図を描い てみます。各伝達関数の分⺟多項式は同⼀なので、分⼦は定数ですので、位相曲線は 同⼀のものになっています。ゲインの⼤きさのみ違うので、ゲイン曲線が上下に平⾏ 移動した形です。それぞれのシステムには０の極が存在するので、⾃ら平衡状態に戻 ることはないため、フィードバックによる安定化が必要です。 プロペラのパラメータ 推力係数 CT：8.3x10-7 Ns2/ rad2 トルク係数 CQ：3.0x10-8 Nms2/rad2 モータ＋プロペラ系の時定数 τ：1.93x10-2 s 上下運動のゲイン Kw：0.524 ロール運動のゲイン Kroll(Kp)：10.3 ピッチ運動のゲイン Kpitch(Kq)：9.11 ヨー運動のゲイン Kyaw(Kr)：2.15 以上の数値例から、マルチコ プタの伝達関数のパラメータを 算出すると左の値が求まります。 モータとプロペラのトルク応 答に関する時定数が約0.02sと なり、その逆数は５０Hzなので、 少なくともその10倍の周波数５ ００Hzぐらいで制御は回したい ところです。 各ゲインは、質量や各運動の 軸に関する慣性モーメントが⼤ きくなると⼩さくなる値です。 それぞれの値が⼩さいほど、 速度が⼤きくなるので応答が早 いように感じられます。 マルチコプタの速度・角速度系のボード線図 「マルチコプタの運動と制御」基礎のきそ ver 0.26 12／？

2023.7.8 こうへい マルチコプタの上下位置と角度のボード線図 角速度制御：比例制御のみ ⾓度に関するボード線図は積分器が⼆つ⼊ってくるので、位相余裕は全くなく。単 純に⾓度フィードバックしても、マルチコプタは不安定になってしまいます。PID補 償などで、位相を進めてやって、位相余裕を確保して、安定化を図りたいところです。 前に⽰したように、⾓速度制御ループと⾓度制御ループの２重ループが有効です。 ピッチレート制御系を例にとって、マルチコプタの制御を考え始めてみます。最初 は最も単純な⽐例制御だけを施してみます。後々、問題があることがわかってきます が簡単なものからです。ボード線図を⾒ると、位相余裕が⼤変⼤きく、⽐例制御だけ だとゲイン線図が上に上がるだけなので⽐例ゲインは相当な⼤きさまで⾏けそうです。 機体 目標 − 𝐾>@ 𝛿- 𝐾> 𝑠(𝜏𝑠 + 1) 𝑞 1 𝑠 𝜃 コントローラ 比例制御によるピッチレート制御系 数値例も整ったので、ここから机上制御実験をやっていこうと思います。⽐例制御 からはじまり、PID制御等々について検討と設計のあらましなどについて触れていこ うと思います。陽にロバスト制御は扱わない予定ですが、平衡点からズレた場合の挙 動や無駄時間を考えた場合の挙動などのノミナルモデルとの⽐較もしてみます。 ピッチレート伝達関数のボード線図 「マルチコプタの運動と制御」基礎のきそ ver 0.26 13／？

2023.7.8 こうへい 比例制御の開ループ伝達関数 𝐾23 𝐾2 𝐿 𝑠 = 𝑠(𝜏𝑠 + 1) 以上の様な開ループ伝達関数となります。⼀⾒、１型（積分器が⼀個ある）なので、 ⽬標値追随性も良さそうです。⽐例制御（以下、P制御）だけでも良いかも︕と期待 させてくれます。まあ、実際はそう簡単には進まないのが残念なところです。 比例制御の設計 マルチコプターと⽐例制御器のボード線図 を重ねたものが左上の図です。⽐例制御の⼀ 巡伝達関数はオレンジ⾊の制御器のゲイン分 だけ制御対象のゲイン線図が上にスライドす るだけで位相線図は変化しません。従って⼀ 巡伝達関数のボード線図は右上のようになり ます。左下は閉ループのボード線図を⽰して います。この例はゲインが3.7の場合で。位相 余裕60.5deg、ゲイン交差周波数29.3rad で す。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 ⽐例制御の場合の、過渡応答をみてみま す。左上が単位ステップ応答です。元々の マルチコプタの伝達関数が１型ですので、 定常偏差なく追随します。右上はインパル ス応答を⽰していますが、０に素早く戻っ ていることが確認できます。 しかし、左下の定常外乱に対する応答は ０に収束せず⽐例制御では外乱を抑制する ことはできていません。 左の３つのグラフのうち1番上 のグラフは⽐例ゲインの変化に 対しての位相余裕の変化です。2 番⽬のグラフは⽐例ゲインの変 化に対してのゲイン交差周波数 の変化です。ゲインを⼤きくす ると位相余裕は減少し、ゲイン 交差周波数は増⼤します。 3番⽬はゲイン交差周波数と位 相余裕の関係を⽰しています。 位相余裕を⼤きくしたいときは ゲイン交差周波数が⼩さくなる ため、両⽅を同時に⼤きくする ことはできません。これらのグ ラフから位相余裕などの仕様が 決まれば⽐例ゲインの⼤きさを 決められます。 14／？

2023.7.8 こうへい 古典制御での設計の勘所 ゲインを調整しながら、時間応答を⾒て良さそうな値を探しても良いのですが、それ だと良い値を⾒つけるための試⾏錯誤時間が割と⻑くなることが多いです。その⽅法で も、経験を積むと割と勘が働くようになりますが、古典制御では周波数応答の世界であ たりをつけてから微修正するというのが良い⽅法だと思います。 制御対象と制御器を含めた開ループ伝達関数（これを⼀巡伝達関数と呼びます。）こ の⼀巡伝達関数の周波数応答をボード線図で確認して図★の様になる様に形を整えます。 これをループ整形と呼びます。各種の制御器とそのゲインの調整で形がどの様に変化す るかは法則性がある程度あるので、それを基にループ整形します。 目標 − 𝛿- 𝐾>@ 𝐾> 𝑠(𝜏𝑠 + 1) 比例制御器 𝑞 機体 フィードバックを切る 𝐾>@ 𝛿- 比例制御器 gc付近の傾きを緩く -20dB/dc 高周波のゲインを下げる 安定性向上 ノイズの影響減・ロバスト性 ０dB低周波のゲインを上げる gc -20dB/dcより傾ける 定常偏差が減少 ゲイン交差周波数gcを大きくする 速応性向上（バンド幅大になる） 𝐾> 𝑠(𝜏𝑠 + 1) -180deg 𝑞 機体 これが一巡伝達関数（開ループ伝達関数） 𝐾23 𝐾2 𝐿 𝑠 = 𝑠(𝜏𝑠 + 1) 位相余裕を大きく 安定度増大 図★ 望ましいボード線図の形 ⼤雑把にまとめると以下の様になるかと思います 安定性︓位相余裕を⼤きくする 速応性︓ゲイン交差周波数を⼤きくする 定常性︓低周波のゲインを上げる ロバスト性・対ノイズ性︓⾼周波のゲインを下げる 「マルチコプタの運動と制御」基礎のきそ ver 0.26 15／？

2023.7.8 こうへい フィードバック制御における外乱の影響 マルチコプタの制御において⽐例制御では外乱の影響が残る結果となりましたが、 フィードバック制御における外乱の影響について簡単におさらいします。 マルチコプタの比例積分（PI）制御の場合 制御器に積分器が入るPI制御の場合の外乱の影響 𝑑 外乱 目標 0 − 𝐶(𝑠) 制御器 𝑢 1 𝑃(𝑠) 𝑒𝑟𝑟 𝑞 1 𝑇? 𝑠 機体 PI制御器： 𝐶(𝑠) = 𝐾>@ 1 + 外乱から出力までの伝達関数 𝑃(𝑠) 𝐺24 (𝑠) = 1 + 𝐶(𝑠)𝑃(𝑠) 比例ゲイン： 𝐾>@ 1 𝑇? 𝑠 外乱から出力までの応答 𝑞 𝑠 = 𝐾> 𝑠 𝑑(𝑠) 𝑇? 𝑠 0 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝜏𝑠 + 1 外乱が定常外乱の場合 𝐾> 𝑠 1 𝑇? 𝑠 0 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝜏𝑠 + 1 𝑠 𝐾> = 𝑇? 𝑠 0 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝜏𝑠 + 1 𝑞 𝑠 = 最終値の定理を用いると 𝑞 ∞ = lim 𝑠 D→. 𝑢 PI制御器 積分時間： 𝑇? マルチコプタの比例制御の場合 𝐾> 外乱が定常外乱の場合 コプタ： 𝑃(𝑠) = 𝐾> 𝑠(𝜏𝑠 + 1) 1 𝑞 𝑠 = 比例制御器：𝐶(𝑠) = 𝐾>@ 𝑠 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝑠 𝐾> 最終値の定理を用いると 𝑠(𝜏𝑠 + 1) 𝐾> 1 𝐺>C (𝑠) = 𝑞 ∞ = lim 𝑠 𝐾>@ 𝐾> D→. 𝑠 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝑠 1+ 𝑠(𝜏𝑠 + 1) 𝐾> = lim 𝐾> D→. 𝑠 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝐺>C (𝑠) = 1 𝑠(𝜏𝑠 + 1) + 𝐾>@ 𝐾> = 𝐾>@ 𝐾> 𝑞 𝑠 = 𝑑(𝑠) 外乱の影響が残る 𝑠 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝐾>@ 𝐾> 𝑇? 𝑠 0 𝜏𝑠 + 1 + 𝐾>@ 𝐾> 𝜏𝑠 + 1 ⼀⾒すると、マルチコプタの 伝達関数には積分器が含まれて いるので、定常外乱をも抑制し てくれると勘違いしそうですが、 ⽐例制御ではステップ⼊⼒には 定常偏差なく追随してくれます が、定常外乱を０にする効果は ありません。ただし、⽐例ゲイ ンを⼤きくすることで外乱の影 響を⼩さくする事はできますが、 ハイゲインにすることにより制 御⼊⼒が飽和したりノイズの影 響が⼤きくなったりと限界があ ります。 そこで、この様に制御器に積 分器を含ませてることによって、 外乱を抑制する効果が現れるこ とがわかります。 =0 PI制御では外乱の影響は０となる 「マルチコプタの運動と制御」基礎のきそ ver 0.26 16／？

2023.7.8 こうへい 角速度制御：比例積分（PI）制御 PI制御の開ループ伝達関数 𝐾23 𝐾2 𝑇5 𝑠 + 1 𝐿 𝑠 = 𝑇5 𝑠 " (𝜏𝑠 + 1) 比例ゲイン： 𝐾>@ 積分時間： 𝑇? 積分時間・比例ゲインとゲイン交差周波数と位相余裕 総当たり的で⼿計算では不可能でありますが、積分ゲインを決めて、⽐例ゲインを変 えていくとゲイン交差周波数と位相余裕がどの様になるかみていきます。ゲイン交差周 波数と位相余裕がどちらも⼤きくなると速応性と安定性が増⼤しますが、どちらも同時 に確実に⼤きくするのは難しく、当てずっぽうではかなりくたびれるので、実際にグラ フ化して確認します PI制御の設計 安定の条件を調べる ピーク がある ⼀巡伝達関数（開ループ伝達関数）𝐿(𝑠)から、閉ループの伝達関数𝑊(𝑠)を求め、閉 ループの特性⽅程式を求め、フルビッツの安定判別法から、安定の条件が明らかになる か⾒てみます。 閉ループ伝達関数 𝐾23 𝐾2 (𝑇5 𝑠 + 1) 𝐿(𝑠) 𝑊 𝑠 = = 1 + 𝐿(𝑠) 𝑇5 𝜏𝑠 6 + 𝑇5 𝑠 " + 𝐾23 𝐾2 𝑇5 𝑠 + 𝐾23 𝐾2 特性方程式 𝑇5 𝜏𝑠 6 + 𝑇5 𝑠 " + 𝐾23 𝐾2 𝑇5 𝑠 + 𝐾23 𝐾2 = 0 フルビッツ行列 𝑇5 𝑇5 𝜏 0 制御ゲインによるゲイン交差周波数と位相余裕 𝐾23 𝐾2 𝐾23 𝐾2 𝑇5 𝑇5 安定の条件: 0 0 𝐾23 𝐾2 フルビッツの安定判別法を適⽤していくと、 特性⽅程式の各係数は全て正でなければなら ないのですが、それは満たされます。次にフ ルビッツ⾏列の⼩⾏列式を求めていくと次の 安定の条件についての結論が得られます。 𝑇5 > 𝜏 積分時間を固定して、⽐例ゲインを変えていき、その時のゲイン交差周波数と位相余 裕を横軸ゲイン交差周波数、縦軸位相余裕でプロットすると、位相余裕の値にピークが ある曲線が得られます。ざっくりいうと、ゲイン交差周波数と位相余裕はどちらも⼤き くとりたいのでピークのところが位相余裕の最適値と⾔うことになります。ピークのゲ イン交差周波数と位相余裕を連ねた線が、将に制御設計の分⽔嶺と⾔えそうです。 PI制御の積分時間はモータ＋プロペラ系の時定数より⼤きくなければならない 「マルチコプタの運動と制御」基礎のきそ ver 0.26 17／？

2023.7.8 こうへい 制御器のゲインの決定 赤い点線 より右側 には設計 できない 青い点線に 近く右側で 設計するの が望ましい 位相余裕が 60degの線 比例ゲイン 1.5 図A ゲイン交差周波数と位相余裕 位相余裕の最適線と比例制御の場合を重ねた図 図Aにおいて、各積分時間における位相余裕のピークを連ねた曲線が太い⻘い点線で す。また⽐例制御においてゲインを変化させた際に、ゲイン交差周波数と位相余裕は太 い⾚い点線の上を移動します。 PI制御において、⻘い点線よりも左側は、速応性も安定性ももっとよくできる点があ ることを意味するので選択する事は合理的ではありません。⼀⽅の、⻘い点線よりも右 側は安定性が⼩さくなることを許容して速応性を上げるということになりそうです。ま た、⾚い点線に近いところでは⽐例制御に性質が似たものとなるためわざわざPI制御を かけて定常性や外乱抑制を期待しているのに、その効果はあまり無いことになりそちら も選択する合理的な理由はありません。 結論的には⻘い点線に近くかつその右側の領域で、ゲイン交差周波数と位相余裕を満 ⾜する⽐例ゲインと積分時間を⾒つけることになります。 図B 横軸が比例ゲイン縦軸が位相余裕のグラフ 制御器の⽐例ゲインと積分時間を決定してみましょう。 図Aから⻘い点線の上からゲイン交差周波数と位相余裕の設計値を選びます。ここで は位相余裕を60degとします。そうすると⻘い点線からゲイン交差周波数は13.9rad/s とグラフから読み取れます。また、積分ゲインは0.27s程度とも分かります。 次に、図Bから位相余裕60degの時の⽐例ゲインを読み取ります。図Bは図Aと同じ様に ⾒えますが、横軸は⽐例ゲインになっています。縦軸は図Aと同じ位相余裕です。図B からは⽐例ゲインが1.5程度である事が読み取れます。 設計値が以下の様に決まりました。 ゲイン交差周波数︓13.9rad/s 位相余裕︓60deg ⽐例ゲイン︓1.5 積分時間︓0.27s 図Aと図Bの描画について 図Aや図Bについては積分時間を決めて⽐例ゲインを決めるとゲイン交差周波数と位相余裕は求める事ができます。⽐例ゲインを少しずつ変えると図Aや図Bの１本の曲線が描けます。積 分時間を変えて、数本の曲線を引くと最適曲線（⻘い点線）も⾒えてくるので、グラフから設計値を⾒出せる様になります。この⽅法は逆計算や求解アルゴリズムがいらない⽅法です。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 18／？

2023.7.8 こうへい 以上から、位相余裕（PM）を30deg、40deg、50deg、60degと決めた際の最適線上の対応 するゲイン交差周波数を求め、それに対応する⽐例ゲインと積分時間を決定し、それぞれの場 合の開ループ伝達関数の周波数特性と過渡応答を⽐較のため重ねてグラフにしてみました。減 衰の速さや、オーバーシュートの⼤きさなどから、PMが40degから50deg程度が妥当なところ だと思われます。 位相余裕が⼤きくなるとオーバっシュートが減少しますが、ゲイン交差周波数は減少し速応 性が失われていうのが過渡応答から判ります。 また、⽐例制御と⽐較すると、外乱を抑制できている事が確認できます。外乱抑制効果は積 分時間が⼩さいほど効果があります。 最適線上から選 んでいるので GCのところに ピークがある 位相余裕 deg ゲイン交差周波数 rad/s 比例ゲイン 積分時間 s ケース１ 30 30 3.3 0.058 ケース２ 40 24.2 2.7 0.090 ケース３ 50 18.9 2.1 0.15 ケース４ 60 13.9 1.5 0.27 異なるゲインの開ループの周波数特性の比較 「マルチコプタの運動と制御」基礎のきそ ver 0.26 19／？

2023.7.8 こうへい 角速度制御：PID制御 PID制御器をブロック線図で表現する と図の様になります。この図から伝達 関数を求めてみると次の式になります。 1 𝑒𝑟𝑟 1 𝑇? 𝑠 𝐾>@ 𝑇C 𝑠 図 𝑢 前述のPI制御において位相余裕を40degにした場合のゲイン設定をそのまま⽤いて、 微分時間を0（微分制御なし）、0.001、0.01、0.1にして計算をしてみた結果を以下に ⽰します。 𝐾DC (𝑇H 𝑠 # + 𝑠 + 1⁄𝑇I ) 𝐶 𝑠 = 𝑠 比例ゲイン： 𝐾>@ PID制御器 各ゲインは正の値となるので、𝑇. > 𝜏の場合はどんな微分時間をとってもマルチコプタ の⾓速度制御系は安定となります。PI制御では𝑇. > 𝜏としなければ安定化できませんでし たが、PID制御では𝑇. < 𝜏でも𝑻𝒅 が安定条件を満たせば、安定化できることになります。 微分制御を付 け加えることで、 位相余裕か⼤き くなります。場 合のよりますが ゲイン交差周波 数が⼤きくなり ます。⽬標値の 突然の増⼤には ⼤きな応答を返 します。外乱抑 制に関しては積 分の効果を減少 させます。 微分時間： 𝑇C 積分時間： 𝑇? 開ループ伝達関数 𝐾2 𝐾23 𝑇4 𝑠 " + 𝑠 + 1⁄𝑇5 𝐿 𝑠 = 𝑠 " (𝜏𝑠 + 1) 閉ループ伝達関数 𝐾D 𝐾DC (𝑇H 𝑠 # + 𝑠 + 1⁄𝑇I ) 𝐿(𝑠) 𝑊 𝑠 = = 1 + 𝐿(𝑠) 𝜏𝑠 $ + (1 + 𝐾D 𝐾DC 𝐾H )𝑠 # + 𝐾DC 𝐾D 𝑠 + 𝐾DC 𝐾D ⁄𝑇I 安定の条件を調べる 特性方程式 𝜏𝑠 $ + 1 + 𝐾D 𝐾DC 𝐾H 𝑠# + 𝐾DC 𝐾D 𝑠 + 𝐾DC 𝐾D ⁄𝑇I = 0 フルビッツ行列 1 + 𝐾D 𝐾DC 𝑇H 𝜏 0 𝐾DC 𝐾D ⁄𝑇I 𝐾D 𝐾DC 1 + 𝐾D 𝐾DC 𝑇H 図 PID制御における一巡伝達関数の周波数特性（微分時間変化時） フルビッツの安定 条件を求めると次 の様になります 0 0 安定の条件: 𝐾DC 𝐾D ⁄𝑇I 𝜏 − 𝑇5 𝑇4 > 𝐾2 𝐾23 𝑇5 図 PID制御における過渡応答（微分時間変化時） 「マルチコプタの運動と制御」基礎のきそ ver 0.26 20／？

2023.7.8 こうへい 位相余裕とゲイン交差周波数を使ったPID制御系設計 位相余裕（Phase margine）とゲイン交差周波数（Gain crossover frequency）に基 づいてマルチコプタのPID制御系の設計を⾒ていきたいと思います。 位相余裕というのはボード線図で⾒るとゲイン曲線が0dBを横切る周波数の時の位相 が-180degまであとどのくらい余裕があるかを表したものです。 位相余裕とゲイン交差周波数のおさらい ゲイン曲線 ゲイン交差周波数𝜔B 0dB ゲイン余裕 位相曲線 -180deg ボード線図で⾒る位相余裕 PID制御の一巡伝達関数（開ループ伝達関数） 𝑠" 𝐾2 𝐾23 𝑇4 + 𝑠 + 1⁄𝑇5 𝐿 𝑠 = 𝑠 " (𝜏𝑠 + 1) 一巡伝達関数の周波数伝達関数 𝐿 𝑗𝜔 = ∆𝐾23 𝑢 + 𝑣𝑗 ※数ページ前のおさらいですが 位相余裕：安定性の⽬安 ゲイン交差周波数：速応性の⽬安 となります。 = ∆𝐾23 𝑢" + 𝑣 " 一巡伝達関数の位相 ∠𝐿 𝑗𝜔 = tan78 𝑣⁄𝑢 ※⽐例ゲイン𝐾12 については後でそ れについて解きたいと思っている のでわざと出しています。 位相余裕はゲイン曲線がボード線図の0dbを横切る周波数の位相を求めるとわかるの で、まずゲインが1（ボード線図はゲインの20logをとったものなので0dBはゲイン１で す。）の時の周波数を𝜔0 とします。 位相余裕はを𝑃𝑀とすると、⼀巡伝達関数の位相は𝑃𝑀 − 𝜋に等しいことになります ∠𝐿 𝑗𝜔/ = tan78 𝑣⁄𝑢 = 𝑃𝑀 − 𝜋 𝑣⁄𝑢 = tan(𝑃𝑀 − 𝜋) ここで 𝛼 = tan(𝑃𝑀 − 𝜋) としておくと 𝑣 = 𝑢𝛼 𝜔/ 𝜏 + 𝛼 ⁄𝑇5 + 𝜔/ " 𝜏𝛼 − 𝜔/ 𝑇4 = 𝜔/ " 𝜔/ 𝜏 + 𝛼 ゲインについては 1⁄𝑇5 − 𝑇4 𝜔" 𝜏 − 1 𝜔 ※伝達関数に𝑗𝜔を代⼊して整理す るのは結構⼤変ですが、するとこ うなります。 𝐿 𝑗𝜔/ = ∆𝐾23 𝑢" + 𝑣 " = 1 この式に𝑢,𝑣を⼊れ直して⽐例ゲイン𝐾>@ について整理すると次の関係が 得られます 𝐾23 = ここで 𝑣= 𝑢 = 𝑇4 − 𝜏 𝜔" − 1⁄𝑇5 𝐾2 ∆= " となります。 𝜔 (1 + 𝜔 " 𝜏 " ) 𝐿 𝑗𝜔 この式に𝑢,𝑣を⼊れ直して整理すると次の関係が得られます 位相余裕𝑃𝑀 図 一巡伝達関数のゲイン 𝜔/ 𝜔/ 𝜏 + 𝛼 𝐾2 1 + 𝛼 " 以上から、設計者が位相余裕とゲイン交差周波数を決めると、そこから⽐例ゲインが ⼀意に決められる事がわかります。またそれとは別に、積分時間を決めると微分時間も 決まります。これは逆でもいいですが、定常特性や外乱応答などに積分時間が⼤きな影 響を与えるので積分時間を決めて応答を確かめる⽅が良いと思われます。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 21／？

2023.7.8 こうへい 設計者が位相余裕とゲイン交差周波数を決めるとPID制御において⽐ 例ゲインの値が決まる事がわかったので、具体的に位相余裕を60deg、 ゲイン交差周波数を30rad/sに決めて⽐例ゲインを求めると、約3.8と なりました。あとは積分ゲインを⾃由に決められるので、いくつかの 値に変えて、周波数応答、過渡応答について調べてみます。 積分ゲインが⼩さいほどオーバシュートが⼩さく、外乱の抑制効果 も⾼いけれども、振動的になる。また位相曲線がS字の場合急激な⼊⼒ にインパルス的な出⼒がでる。位相曲線がZ字の場合はインパルス的な 出⼒は抑えられる。滑らかに右上がりの位相曲線が理想的です。 比例ゲイン 位相余裕 60 deg ゲイン交差周波数 3.8042 30rad/s 「マルチコプタの運動と制御」基礎のきそ ver 0.26 積分時間 微分時間 0.001 1.1111 0.01 0.1111 0.1 0.0111 1.0 0.0011 22／？

2023.7.8 こうへい 前ページの実験で、積分時間が0.1付近が理想的な事が⾒えてきまし たので、0.1の周りの値を⾒てみることにしました。 0.1付近を境にして⼤きくなるにつれ、位相曲線のカーブがS字からZ 字に変化します。S字の場合はインパルス的な応答が出ていますが、Z 字の場合が望ましい応答になる事がここでも、確認できます。 比例ゲイン 位相余裕 60 deg ゲイン交差周波数 3.8042 30rad/s 「マルチコプタの運動と制御」基礎のきそ ver 0.26 積分時間 微分時間 0.05 0.0222 0.1 0.0111 0.15 0.0074 0.2 0.0056 23／？

2023.7.8 こうへい さらに、0.1付近で絞り込んでいきます。偶然ですが積分ゲインが 0.1が今回の場合は⼀番良い結果が得られそうです。 今回は追及しませんが、ゲイン交差周波数で位相曲線は変曲点と なっていますが理想に近い場合は変曲点を持たない曲線になっている 様に⾒えます。（今後、確認したいと思います。） 位相余裕 比例ゲイン 60 deg ゲイン交差周波数 3.8042 30rad/s 「マルチコプタの運動と制御」基礎のきそ ver 0.26 積分時間 微分時間 0.09 0.0124 0.1 0.0111 0.11 0.0101 24／？

2023.7.8 こうへい 最後に、ゲイン交差周波数は30rad/sのままで、位相余裕を20deg⼩ さくして40degの場合についてみてみました。この場合も⽐例ゲイン は⾃動的に決まり3.5752となりました。積分時間は0.05付近が最適の 様です。（周波数応答と時間応答を⾒た試⾏錯誤によります。） 位相余裕を減らすと、振動的になっているのがわかると思います。 ⾓速度制御については、今のところここまでにしたいと思います。PID制御の設計に ついては理想的なモデルを⽤意して、低次の項からマッチングしていく部分的モデル マッチングという設計⼿法があります。設計の⾒通しが良くて優れた⼿法だと思って ます。もう少ししたら、その話題も追加したいと思います。 位相余裕 比例ゲイン 40 deg ゲイン交差周波数 3.５７５２ 30rad/s 「マルチコプタの運動と制御」基礎のきそ ver 0.26 積分時間 微分時間 0.04 0.0157 0.05 0.0101 0.06 0.0064 25／？

2023.7.8 こうへい 位相余裕とゲイン交差周波数を満たすPIDゲイン決定の手順 角度制御の話題に移る前にPID制御での各ゲインの決定について手順をまとめておこ うと思います。制御対象が任意の場合で成り立つはずです。あくまで我流ですのであま り参考にはならないかもしれません。昨今ではMATLABにもpidtuneコマンドがあり もっと優秀ですので、そちらを使うのがお得かもしれません。 制御対象の周波数伝達関数 𝑃 𝑗𝜔 = 𝑢 + 𝑣𝑗 制御対象は具体的に語られていませんが周波 数伝達関数とは複素数のことなので任意の制 御対象をこの様に書き表すことができます。 実部と虚部の値は周波数𝜔の関数です。 一巡伝達関数の周波数伝達関数 1 目標値 𝑟 − 1 𝑇? 𝑠 𝐾' 𝑦 𝑢 𝑃(𝑠) 制御対象 𝑇. 𝑠 𝐾3 (𝑇4 𝑠 " + 𝑠 + 1⁄𝑇5 ) 𝐶 𝑠 = 𝑠 PIDコントローラの記法としては この書き方は良いと思います。こ れで積分時間と微分時間だけで位 相曲線が決定してしまいます。 周波数伝達関数で考える 𝐾3 (−𝑇4 𝜔" + 𝜔𝑗 + 1⁄𝑇5 ) 𝐶 𝑗𝜔 = 𝜔𝑗 = 𝐾3 − 𝐾3 1⁄𝑇5 − 𝑇4 𝜔" 𝑗⁄𝜔 = 𝛼 + 𝛽𝑗 ここで 𝛼 = 𝐾3 ここで 𝜁 = 𝑢𝛼 − 𝑣𝛽 𝜉 = 𝑢𝛽 + 𝑣𝛼 ゲインと位相 PIDコントローラ PIDコントローラの記述 𝑃 𝑗𝜔 𝐶 𝑗𝜔 = 𝑢𝛼 − 𝑣𝛽 + 𝑢𝛽 + 𝑣𝛼 𝑗 = 𝜁 + 𝜉𝑗 伝達関数の𝑠に𝑗𝜔を代入した ものを周波数伝達関数と呼び ます。代入して整理していく と、結果は複素数になります。 制御では虚数単位は𝑗です。 今後の式の展開のために文字を置き換えておきます 一巡伝達関数は制御対象 とコントローラの伝達関 数の積なので整理すると この様になります。ここ でも文字の置き換えをし ます。 𝑃 𝑗𝜔 𝐶 𝑗𝜔 = 𝜁 " + 𝜉 "（ゲイン） ∠𝑃 𝑗𝜔 𝐶 𝑗𝜔 = tan78 𝜉 ⁄𝜁（位相） 一巡伝達関数のゲインと 位相は周波数伝達関数の 実部と虚部を用いてこの 様になります。 位相余裕とゲイン交差周波数 位相余裕：𝑃𝑀 ゲイン交差周波数： 𝜔B 𝜙9 = 𝑃𝑀 − 𝜋 とすると 𝜁" + 𝜉" = 1 (g) tan 𝜙9 = 𝜉 ⁄𝜁 (p) 𝜁 1 + tan 𝜙9 = 1 (gʼ) 𝜉 = 𝜁tan 𝜙9 (pʼ) 𝛽 = − 𝐾3 1⁄𝑇5 − 𝑇4 𝜔" ⁄𝜔 「マルチコプタの運動と制御」基礎のきそ ver 0.26 その時の位相：𝜙5 ゲイン交差周波数𝜔/ ではゲインは１な のでこの式が成り立ちます。 ゲイン余裕 𝑃𝑀 の時の位相 𝜙& はなので、 位相に関してもこの式が成り立ちます。 (p)式を用いて(g)式を書き換える と、この様に書けます。 (p)式を変形しました 26／？

2023.7.8 こうへい (pʼ)式にこれまで置き換えたものを入れ戻していき、𝑇3 について解きます。 積分時間と微分時間の関係 1 𝑣 − 𝑢 tan 𝜙9 (d) 𝑇4 = " − 𝜔/ 𝑇5 𝜔/ 𝑢 + 𝑣 tan 𝜙9 ここで 𝜙9 = 𝑃𝑀 − 𝜋 位相余裕、ゲイン交差周波数 を設計仕様として決定して、 整理していくと積分時間と微 分時間の関係式が導かれます。 この両者に対して比例ゲイン は独立しています。 (gʼ)式にこれまで置き換えたものを入れ戻していき、さらに（d）式を用いると、積分時 間と微分時間が消えます。 比例ゲイン 𝑢 cos 𝜙9 + 𝑣 sin 𝜙9 𝐾3 = 𝑢" + 𝑣 " (f) 全てを代入して整理していく とこの様に比例ゲインを決定 する式が導かれます。 【まとめ】PIDゲイン決定の手順 位相余裕𝑃𝑀とゲイン交差周波数𝜔! を決める 位相余裕𝑃𝑀の時の位相 𝜙" を計算する 制御対象の周波数伝達関数を求める 制御対象の周波数伝達関数からの𝜔! の時の実 部𝑢の値と虚部𝑣の値を求める ⑤ （f）式から比例ゲインを求める ⑥ 積分時間を適当に決めて（d）式から微分時 間を求める ⑦ 求めたゲインで周波数応答、ステップ応答、 インパルス応答、外乱からの応答等を見る。 良くなければ⑥にもどる。良ければ終了 ① ② ③ ④ ※実機での調整がこの後あります こんな 風に周 波数応 答や過 渡応答 を確認 「マルチコプタの運動と制御」基礎のきそ ver 0.26 27／？

2023.7.8 こうへい

PIDゲイン決定のPythonコード（PIDtune）
import numpy as np
import control.matlab as matlab
import matplotlib.pyplot as plt
#制御対象作成(python-control使用)
tau=0.02 #制御対象の時定数
K=10.0 #制御対象のゲイン
Plant=matlab.tf([K],[tau,1,0])

これらのモジュールが必要なので
インストールをしてください。
制御対象のパラメータを設定して
ます。例としてマルチコプタの
ピッチレート系です。
伝達関数表記でシステムオブ
ジェクトを作ります。

#①位相余裕𝑃𝑀とゲイン交差周波数𝜔̲𝑐を決める
PM=30*np.pi/180
設計者が決めた任意の位相余裕、ゲ
omega̲c=30
イン交差周波数を設定します。
#②位相余裕𝑃𝑀の時の位相 𝜙̲𝑚を計算する
phi̲m=PM-np.pi
#③制御対象の周波数伝達関数を求める
#pythonではz=1+2*1jの様に複素数が使えます
Plant̲freq=K/(omega̲c*1j)/(tau*1j*omega̲c+1)

pythonでは虚数単位1jを
使って複素数を直接扱えま
す。制御対象の周波数伝達
関数（複素数）を計算しま
す。

#⑦求めたゲインで周波数応答、ステップ応答、インパルス応答、
#外乱からの応答等を見る。
#良くなければTiの設定を変えて本スクリプトを再実行する
Kps=np.ones(len(Tis))*Kp
print('積分時間はこの範囲で設定してください')
if (v-u*np.tan(phi̲m))/omega̲c/(u+v*np.tan(phi̲m))>0:
print('0 < Ti < {:f}¥n'.format((u+v*np.tan(phi̲m))/omega̲c/(vu*np.tan(phi̲m))))
位相余裕とゲイン交差周波数の値に
else:
より積分時間の設定可能範囲は制限
print('Ti>0¥n')
されます。ここでその範囲を表示し
ます。積分時間が範囲外になってい
fig1=plt.figure(figsize=(10,7))
る場合は手動で訂正してください。
fig2=plt.figure(figsize=(20,3.5))

〜以降省略〜

#④制御対象の周波数伝達関数からの𝜔̲𝑐の時の実部𝑢の値と虚部𝑣の値を求める
u=Plant̲freq.real
虚数オブジェクトPlantのメンバーである実部
v=Plant̲freq.imag
(real)と虚部（imag）にアクセスしてu,vとし
ます。
#⑤（f）式から比例ゲインを求める
Kp=(u*np.cos(phi̲m)+v*np.sin(phi̲m))/(u**2+v**2)
比例ゲインゲット！

このコードで前ページの手順が実行で
きます。赤字の部分が制御対象のパラ
メータ、伝達関数、制御仕様である位相
余裕、ゲイン交差周波数、そして調整パ
ラメータである積分時間です。それぞれ
書き換えてください。
実行すると左の様に周波数応答と過渡
応答が表示されますので、良くなるまで
制御仕様または積分時間を調整してくだ
さい。コードは以下にあります
https://github.com/kouhei1970/fun
damental̲of̲multicopter̲control

積分時間はこの様に複
#⑥積分時間を適当に決めて（d）式から微分時間を求める
数の値を記述可能です。
Tis=np.array([0.03, 0.04, 0.05])
Tds=1/omega̲c**2/Tis - (v-u*np.tan(phi̲m))/omega̲c/(u+v*np.tan(phi̲m))
微分時間ゲット！

「マルチコプタの運動と制御」基礎のきそ ver 0.26

28／？

• https://github.com/kouhei1970/fun

2023.7.8 こうへい マルチコプタの角度制御系の設計 目標値 𝜃= − 𝐶" (𝑠) − 角度PID 制御器 𝐶8 (𝑠) 角速度PID 制御器 𝑢 𝑃(𝑠) 𝑞 1 𝑠 𝜃 機体 角度制御の制御対象 マルチコプタの角度制御系 前の角速度制御系の数値例を元にして、位相余裕とゲイン交差周波数を望ましいもにす る角度制御系PID制御器を設計してみます。設計手順は前のページと同じ手順です。 角速度制御系の設計値 位相余裕 60 deg ゲイン交差周波数 30rad/s 角度制御系一巡伝達関数のボード線図 比例ゲイン 積分時間 微分時間 3.8042 0.1 0.0111 角度制御の制御対象の伝達関数 0.3847𝑠 " + 34.66𝑠 + 346.6 0.0193𝑠 : + 1.385𝑠 6 + 34.66𝑠 " + 346.6𝑠 ここまでくると解析 的に伝達関数を記述 するのは大変になっ てくるので数値解で 失礼です。 角度制御系の設計値 位相余裕 60 deg ゲイン交差周波数 30rad/s 比例ゲイン 積分時間 微分時間 25.9369 0.07 0.0352 角度制御系の過渡応答 少々、試行錯誤を繰り返していますが、最終的に絞り込んだ際の比較結果が、上の図で す。最終的に制御入力が大きくなりすぎないかなど気になりますがPID制御では制御器 がインプロパーなため解析的には制御入力を見ることができないので、ここはひとまず 終了とします。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 29／？

2023.7.8 こうへい マルチコプタのシミュレーション 𝑥̇ = 𝑓(𝑡, 𝑥, 𝑢) Pythonでの実装例 対象の動作を表す微分方程式 常微分方程式の解法：４次ルンゲ・クッタ法 𝑘8 = ℎ𝑓(𝑡, 𝑥, 𝑢) 𝑘" = ℎ𝑓(𝑡 + 0.5ℎ, 𝑥 + 0.5𝑘8 , 𝑢) 𝑘6 = ℎ𝑓(𝑡 + 0.5ℎ, 𝑥 + 0.5𝑘" , 𝑢) 𝑘: = ℎ𝑓(𝑡 + ℎ, 𝑥 + 𝑘6 , 𝑢) 𝑥 = (𝑘8 + 2𝑘" + 2𝑘6 + 𝑘: )⁄6 𝑡 =𝑡+ℎ 𝑥：状態量 𝑡：時刻 ℎ：刻み幅 準備 状態と時刻の更新 𝑢：入力等 上記の計算により刻み幅分の時刻進んだ時刻の状態xを今のx,t,uから求めていくもので、 逐次それらの値が求められます。初期値がなければ計算を始められないので、初期値を 与える必要があります。 状態量は複数あっても構いません。また、高階の微分方程式の場合、変数の置き換え によって一階の連立微分方程式に書き換えられるので、高階の常微分方程式でも解くこ とができます。 スッティフ（硬い、やっかいな）なシステムの場合 例えばマルチコプタのシミュレーションをするためにモータのインダクタンスの影響 を無視しないとしましょう。そうするとモータの回路方程式について電流の挙動も解く 必要がありますが、電流の挙動は機体の運動に対して時定数が極めて小さくなります。 この様にシステムに変化の著しく違う状態が混在していると（この様なシステムをス ティッフといいます。）数値計算的な不安定が発生します。解決には刻み幅を小さくす ることですが、計算時間がかかるのが問題です。 import matplotlib.pyplot as plt def rk4(func, t, h, x, *p): k1=h*func(t, x, *p) k2=h*func(t+0.5*h, x+0.5*k1, *p) k3=h*func(t+0.5*h, x+0.5*k2, *p) k4=h*func(t+h, x+k3, *p) x=x+(k1 + 2*k2 + 2*k3 + k4)/6 return x def xdot(t,x,u): a=-10 b=1 return a*x+b*u ルンゲ・クッタ法を一度だけ計算する関数 *pの様にアスタリスクがつ いた引数は引数の数を可変 して受け取る時に使います 微分方程式から導関数を定義した関数 #ここからメイン h=0.01 t=0 x=0 u=1 fintime=1 T=[] X=[] for _ in range(int(fintime/h)): x=rk4(xdot,t,h,x,u) t=t+h T.append(t) X.append(x) こんな感じの計算結果が出ます plt.plot(T,X) plt.show() 「マルチコプタの運動と制御」基礎のきそ ver 0.26 30／？

2023.7.8 こうへい u,v,w p,q,r p,q,r 角速度PID制御 制御入力 機体の運動方程式 udot,vdot,wdot pdot,qdot,rdot ルンゲ・クッタ法 等の数値積分 u,v,w p,q,r φ,θ,ψ 角速度目標値 角度目標値 角度PID制御 p,q,r 位置とクォータニオン の微分方程式 φ,θ,ψ q1dot,q2dot, q3dot,q4dot xedot, yedot, zedot φ,θ,ψ ルンゲ・クッタ法 等の数値積分 q1,q2,q3,q4 xe,ye,ze クォータニオン →DCM→オイラー角 角度制御シミュレーションブロック線図 「マルチコプタの運動と制御」基礎のきそ ver 0.26 31／？

2023.7.8 こうへい PID制御の基本的な実装 誤差 𝑒; = 𝑦1<$ ; − 𝑦; 数値積分（積分器） 𝑠; = 𝑠;78 + 𝑒; ℎ 数値微分 𝑑𝑒; = (𝑒; − 𝑒;78 )⁄ℎ PID制御は微分項があるのでそれ自体はイン プロパー（分母の次数より分子の次数が高 い。）なので実現は難しいのですが、微分 を左の様に差分で近似することで、実装す ることはできます。積分の部分はこの例で は、区分求積法と同じで誤差の高さで幅がh の細長い長方形を逐次足していくことで実 装します。比例項と積分項及び微分項を足 し合わせたものがPID制御の算出する操作量 （制御入力）となります。 PID制御の操作量 𝑢; = 𝐾3 (𝑒; + 𝑠; ⁄𝑇5 + 𝑇4 𝑑𝑒; ) 積分項が減少し、本来ありうる値付近になるまで不自然な動きが続きます。この様な現 象をワインドアップと言います。巻いて置いてある長いロープの端に取り付けられてい るものを、ロープを手繰り寄せることで手元に引き寄せる際に、ずいぶん余計な量の ロープを巻き取ることになると思いますが、制御のワインドアップはそのことを連想さ せます。 積分器を持たないPID制御の実装 一般的なPID制御の操作量の微分をもとめ、操作量の微分を数値微分と置き換えること で、PID制御式が積分項を持たない形式にすることができます。これを位置の微分が速 度であることのアナロジーで速度型PIDと言いいます。また、これまでに示したPID制 御式は位置型PIDとも呼ばれています。積分器を持たなくて良いことからワインドアッ プアップ対策などする必要がなく良いのですが、制御系が非最小位相系になることも知 られています。実装の際にはさまざまなところで速度型が多用されているとも聞きます が、非最小位相系になってしまうところが気になり、私は採用を躊躇います。 速度型PID制御の操作量 積分器のオーバーフロー対策 積分器に関しては、実際にマイコン等で実装する際には変数のオーバーフローを考慮し ておく必要があります。短い制御周期の中で何度も積算していくと場合によっては積分 器に割り当てられている変数はオーバーフローしてしまう可能性が高いです。オーバー フローが起こった時に正負が逆転してしまい、制御中に異常な動作に陥り危険です。何 があるか分からないので、積分器はある値以上または以下にならない様に値を制限する 工夫でオーバーフローを防止してください。 𝑢; = 𝑢;78 + 𝐾3 (𝑒; − 𝑒;78 + ℎ𝑒; ⁄𝑇5 + 𝑇4 𝑒; − 2𝑒;78 + 𝑒;7" ⁄ℎ) （位置型）PID制御と速度型PID制御の応答の比較 操作量リミッタとワインドアップ 特に微分項が入っている場合に、操作量が大きくなりすぎて、実際の制御対象に加えて 良い制限を超える場合があります。その様に過大な操作量が制御対象に加わることを防 ぐために、操作量を制限します。その制限をリミッタと呼びます。例えば、目標値が大 きく誤差が大きくなると比例項だけでも操作量がリミッタに引っかかることもあります。 その場合、制御対象は操作量が限度目一杯で最大の能力で動作することになります。そ して目標値に近づく間は積分項の大きさが溜まり続けます。この場合積分項は本来では あり得ない大きさまで大きくなります。そして、制御対象が目標値を超えると、積分器 は減少に転じます。 速度型PIDではこ の様に逆応答が生 じる 「マルチコプタの運動と制御」基礎のきそ ver 0.26 32／？

以下にシミュレーションで用いたPID制御の実装例を示します。実際の組み込み系で はCやC++等の高速な言語で置き換える必要があります。 #位置型PID制御の実装例 class pid: def __init__(self, kp, ti, td,limit_upper=100.0,limit_lower=-100.0,limitter=False): self.Sum=0.0 self.olderr=0.0 self.oldtime=0.0 self.limit_upper= limit_upper self.limit_lower= limit_lower self.limitterFlag=True self.Kp=kp self.Ti=ti self.Td=td self.u=0.0 def controller(self, y, ref, t): err=ref-y period=t-self.oldtime self.Sum=self.Sum+err*period #print(period) if period==0.0: errdot=0.0 else: errdot=(err-self.olderr)/period self.u=self.Kp*(err + self.Sum/self.Ti + self.Td*errdot) #リミッター if self.limitterFlag==True: if self.u>self.limit_upper: self.u=self.limit_upper elif self.u<self.limit_lower: self.u=self.limit_lower 2023.7.8 こうへい class vpid: def __init__(self, kp, ti, td,limit_upper=100.0,limit_lower=-100.0,limitter=False): self.Sum=0.0 self.olderr1=0.0 self.olderr2=0.0 self.oldtime=0.0 self.limit_upper= limit_upper self.limit_lower= limit_lower self.limitterFlag=True self.Kp=kp self.Ti=ti self.Td=td self.u=0.0 def controller(self, y, ref, t): err=ref-y period=t-self.oldtime self.Sum=self.Sum+err*period #errdot=(err-self.olderr)/period if period==0.0: self.u=self.u\ + self.Kp*(err-self.olderr1 + period*err/self.Ti) else: self.u=self.u\ + self.Kp*(err-self.olderr1 + period*err/self.Ti\ + self.Td*(err-2*self.olderr1+self.olderr2)/period) #リミッター if self.limitterFlag==True: if self.u>self.limit_upper: self.u=self.limit_upper elif self.u<self.limit_lower: self.u=self.limit_lower self.oldtime=t self.oldtime=t self.olderr=err self.olderr2=self.olderr1 self.olderr1=err return self.u return self.u 「マルチコプタの運動と制御」基礎のきそ ver 0.26 33／？

2023.7.8 こうへい 送信機のチャンネル番号 モード１の場合 ⑤ 日本では これが標準 ② ④ モード２の場合 ③ ⑥ ⑤ ① ④ 一般的な機能 ①エルロン ②エレベータ ③スロットル ④ラダー 日本で売られている飛行機用のラジコンプロポは多くがこの様に チャンネルが割り振られています。これをモード１と言います。 プログラムする際は機能を必ず上記の様に割り振る必要はありませ ん。どのスティックが何チャンネルなのかを確認してプログラムし ながら機能を割り振ってください。 当研究室ではモード１のプロポをモード３の様に使える様にプログ ラムしています。 ② ⑥ ③ ① 一般的な機能 ①エルロン ②エレベータ ③スロットル ④ラダー 日本以外のほとんどの国で上記の様にチャンネルが割り振られてい ます。これをモード２と言います。 エルロンとエレベータが同じスティックに割り振られていて、直感 的に操縦が可能です。 本物の飛行機は操縦桿がエレベータとエルロン、足のペダルがラ ダーなので、本物の飛行機の操縦感覚にも似ていると思います。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 34／？

2023.7.8 こうへい 送信機のチャンネル番号 ◎ モード３の場合 モード４の場合 当研究室 の使い方 ⑤ ② ① ③ ⑥ ⑤ ④ ① 一般的な機能 ①エルロン ②エレベータ ③スロットル ④ラダー モード３はモード２の左右が反対版です。 日本のプロポはスロットルが右に割り当てられているので、モード ３の様に操縦できるとエルロンとエレベータが同じスティックなの で直感的に操縦できます。 ② ⑥ ③ ④ 一般的な機能 ①エルロン ②エレベータ ③スロットル ④ラダー モード４はモード１の左右反対版です。 安価な送信機はモードが固定ですが。高級機になるとモードを設定 することができます。スロットルはバネが効かず手を離してもその 場を保つ様になっているものが多いですが、それも改造して左右を 入れ替えたりバネが効く様にできるものもあります。 「マルチコプタの運動と制御」基礎のきそ ver 0.26 35／？

基本的なマルチコプタ制御プログラムを作り上げるまでの学習過程 学習カリキュラム ① ② ③ ④ ⑤ ⑥ ⑦ ⑧ ⑨ ⑩ ⑪ ⑫ ⑬ マイコンの開発環境のインストール 開発環境を用いたビルド工程の習得 Lチカプログラムのビルドとマイコンへの書き込み PWM信号生成方法の習得 UART通信でのコンソールとの通信方法の習得 インターバルタイマー割り込み方法の習得 I2C及びSPI通信方法の習得 ジャイロ・距離計等の外部ペリフェラルと通信して 所望のデータを受け取る方法の習得 送信（操縦装置）・受信装置から指令信号をマイコ ンが受け取る方法を習得 送受信装置からの信号に基づきPWM信号のDutyを 変更しモータの速度を可変させる方法の習得 スロットル・ロール・ピッチ・ヨー指令値から４つ のモータへの回転指令値を生成（ミキシング）する 方法の習得 角速度・加速度（・地磁気）情報から姿勢の推定計 算方法を習得 フィルター（EKF・相補フィルタ等）について習得 2023.7.8 こうへい ⑭ ⑮ ⑯ ⑰ PID制御の実装方法を習得 PIDによる角速度制御ループの実装を習得 PIDによる角度制御ループの実装を習得 PIDゲインのチューニングについてモデルに基づく 方法について学習 ⑱ 実機のフライトテストにて調整 ⑲ 現代制御理論での制御へ・・・ 番外編 l l l l l l 使いやすいエディタを見つける ソースコードの分割ビルド方法の習得 便利な統合環境の導入を検討 各言語に習熟 プログラミング作法の勉強 勉強は無限につづく・・ ※C⾔語の基礎的な知識を既 に有しているものとします が、基礎でOK 「マルチコプタの運動と制御」基礎のきそ ver 0.26 36／？

2023.7.8 こうへい 今は、これが精一杯 つづく 「マルチコプタの運動と制御」基礎のきそ ver 0.26 37／？

バックヤード 制作途中の資料など 38／？

1 2 39／？

ジャイロ ⾓速度 ジャイロと加速度計を⽤いた 姿勢推定の概要 クォータニオン 微分値 クォータニオン 加速度計 加速度 ⽅向余弦⾏列 Direction cosine matrix ※⽮印をコードに落とし込む オイラー⾓ 40／？

ジャイロ ⾓速度 ジャイロと加速度計を⽤いた 姿勢推定の概要 クォータニオン 微分値 クォータニオン 加速度計 加速度 ⽅向余弦⾏列 Direction cosine matrix ※⽮印をコードに落とし込む オイラー⾓ 41／？

ジャイロ ⾓速度 ジャイロと加速度計を⽤いた 姿勢推定の概要 クォータニオン 微分値 クォータニオン 加速度計 加速度 ⽅向余弦⾏列 Direction cosine matrix ※⽮印をコードに落とし込む オイラー⾓ 42／？

φ1 16 4 28 ハブ部分の重量全体の2分の1 ブレード1枚を4分の1と想定 ハブの慣性モーメント 全体重量 0.27e-3kg ハブ重量 0.135e-3kg ブレード重量 0.0675e-3kg ブレードの慣性モーメント（質点として計算） 𝑏$ − 𝑎$ (2×10&' )$ −(0.5×10&' )$ &' = 4.7813×10&() 𝐼# = 𝑀 = 0.135×10 2(𝑏 − 𝑎)% 2(2×10&' − 0.5×10&' )% 𝐼* = 𝑀𝑟 % = 0.0675×10&' ×0.016% = 1.728×10&+ プロペラの慣性モーメント 𝐼, = 𝐼# + 2𝐼* = 4.7813×10&() + 2×1.728×10&+ = 3.5038×10&+ kgm2

軸⻑ 3 2. 25 24.5 φ1 ロータの慣性モーメント 全体重量 0.71e-3kg 軸の質量 𝑚- = 𝜌𝜋𝑟 % 𝑙 = 7850×𝜋×0.0005% ×0.0245 = 0.15105×10&' 𝐼. = 𝐼- + 𝐼! = 1.1881×10&(( + 2.7511×10&+ = 2.7522×10&+ kgm2 コイルの質量 𝑚! = 0.71 − 0.15105 ×10&' = 0.55895×10&' 軸の慣性モーメント 1 𝐼- = 𝑀𝑟 % = 0.5×0.15105×10&' ×0.0005% = 1.8881×10&(( 2 コイルの慣性モーメント 𝑏$ − 𝑎$ (3×10&' )$ −(2.25×10&' )$ &' = 2.7511×10&+ 𝐼! = 𝑀 = 0.55895×10 2(𝑏 − 𝑎)% 2(3×10&' − 2.25×10&' )%

直接計測できなくて未知な量としては粘性抵 抗係数𝐷; とプロペラのトルク係数𝐶D があり ます。それらについて式を整理しますと タコジェネレータ 慣性モーメント𝐽8 粘性抵抗係数𝐷; 実験装置 のモデル コアレスモータ プロペラ 慣性モーメント𝐽C トルク係数𝐶D 慣性モーメント𝐽8 粘性抵抗係数𝐷; トルク定数𝐾; = 8.73𝑒 − 8 巻線抵抗𝑅; = 1.16 実験装置の数学モデルと定常状態 " 𝑑𝜔 𝐾9 𝐾9 2𝐽1 + 𝐽3 + 2𝐷9 + 𝜔 + 𝐶2 𝜔" = 𝑒 𝑑𝑡 𝑅9 𝑅9 2𝜔8 𝐷9 ＋𝜔8" 𝐶2 + ⾓速度は⻘い線の様に上昇し⼀定値になる ここで ∑ 𝐴"= ∑ 𝐴= 𝐵= " 𝐾9 𝐾9 2𝐷9 + 𝜔8 + 𝐶2 𝜔8" = 𝑒 𝑅9 𝑅9 8 となります 𝐾9 𝐾 𝜔 − 𝑒9 𝑅9 9 = ∑ 𝐴= 𝐵= 𝐷9 ∑ 𝐴= 𝐶= = − 𝐶2 ∑ 𝐵= 𝐶= ∑ 𝐵=" ということで逆⾏列があるなら 𝐷9 ∑ 𝐴"= =− 𝐶 ∑ 𝐴= 𝐵= ある電圧𝑒" を印加して回すとある⾓速度𝜔" で定常になります。 2 その際は上式の微分項は0と考えられます。したがって 最⼩⼆乗法を使って実験データからで左 辺の値の2乗和が最⼩になる𝐷; と𝐶D を求 めてみます。 𝐴= = 2𝜔= 𝐵= = 𝜔=" 𝐶= = とおいておくと 𝐾9 𝐾 𝜔 − 𝑒8 = 0 𝑅9 9 8 ∑ 𝐴= 𝐵= ∑ 𝐵=" 78 ∑ 𝐴= 𝐶= ∑ 𝐵= 𝐶= ※𝐶1 は計測しようと思え ば測れますがここでは計 測していないとします。 実験結果 en(V) 1 1.5 2 2.5 3 3.5 ωn(rad/s) 623 892 1114 1331 1472 1632 以上から、実験のデータを使って𝐷; と𝐶D を求めると 𝐷9 = 4.29×10788 [Kgm2/s] 𝐶2 = 4.53×1078: [kgm2] ※𝐷, に対して𝐶1 がずいぶん⼩さいので 無視してしまうと計算が合いません。 𝐶1 は⾓速度の2乗の係数なのでやはり無 視できない事がわかります。

14 19 9 y 18 8 33 Mass list Motor 3.78e-3 kg Prop 0.27e-3kg M5Atom 5.5e-3kg Body 5.9e-3kg Arm 0.96e-3kg Battery 9.85e-3kg z 29 x 28 8 22 20 y 30 x 1.6 z The moment of inertia Ix =2.14e-5kgm^2 Iy =2.19e-5kgm^2 Iz =4.08e-5kgm^2 Z direction Center of Mass 2.73mm ※全て質点として推定(原点にある物体は分割した) ※ All estimated as mass points

φ1 16 One-half of the total weight of the hub section One blade is assumed to be 1/4 of the total weight Overall weight 0.27e-3kg Hub weight 0.135e-3kg Blade weight 0.0675e-3kg 4 28 Hub moment of inertia 𝑏$ − 𝑎$ (2×10&' )$ −(0.5×10&' )$ &' = 4.7813×10&() 𝐼# = 𝑀 = 0.135×10 2(𝑏 − 𝑎)% 2(2×10&' − 0.5×10&' )% Blade moment of inertia (calculated as a mass point) 𝐼* = 𝑀𝑟 % = 0.0675×10&' ×0.016% = 1.728×10&+ Propeller moment of inertia 𝐼, = 𝐼# + 2𝐼* = 4.7813×10&() + 2×1.728×10&+ = 3.5038×10&+ kgm2

ピッチ（ロール）運動のシミュレーション 14 19 9 y 18 8 33 Mass list Motor 3.78e-3 kg Prop 0.27e-3kg M5Atom 5.5e-3kg Body 5.9e-3kg Arm 0.96e-3kg Battery 9.85e-3kg z 29 x 28 8 22 20 y 30 x 1.6 z The moment of inertia Ix =2.14e-5kgm^2 Iy =2.19e-5kgm^2 Iz =4.08e-5kgm^2 Z direction Center of Mass 2.73mm ※全て質点として推定(原点にある物体は分割した) ※ All estimated as mass points

14 9 8 20 19 40 1.6 29 8 28 AtomFly2の慣性モーメントの計算終わった。 続いて制御 系設計とシミュレーション。 The calculation of the moment of inertia of AtomFly2 is finished. Then control system design and simulation.

不完全微分付きのPID補償器 𝑐 𝑠 = 𝐾3 1 + 1 𝑇4 𝑠 + 𝑇5 𝑠 𝜂𝑇4 𝑠 + 1 双1次変換で離散化 双1次変換とは次の変換です 𝑠= 2(𝑧 − 1) 𝑡> (𝑧 + 1) PID補償器を⼀変に離散化するのは ⾻が折れるので積分と微分の部分 にわけてやっていきます。 積分補償器の離散化 𝑦; = 𝑡> (𝑧 + 1) 𝑒 2𝑇5 (𝑧 − 1) ; 2𝑇5 (𝑧 − 1)𝑦; = 𝑡> (𝑧 + 1)𝑒; 2𝑇5 (𝑦;?8 − 𝑦; ) = 𝑡> (𝑒;?8 + 𝑒; ) 𝑡> 𝑦;?8 = 𝑦; + (𝑒 + 𝑒; ) 2𝑇5 ;?8 ※これは台形積分と同じです 不完全微分補償器の離散化 2(𝑧 − 1) 𝑡> (𝑧 + 1) 𝑦; = 𝑒; 2(𝑧 − 1) 𝜂𝑇4 +1 𝑡> (𝑧 + 1) 2𝑇4 (𝑧 − 1) 𝑦; = 𝑒 2𝜂𝑇4 (𝑧 − 1) + 𝑡> (𝑧 + 1) ; 2𝑇4 (𝑧 − 1) 𝑦; = 𝑒 2𝜂𝑇4 + 𝑡> 𝑧 − (2𝜂𝑇4 − 𝑡> ) ; 𝑇4 2𝜂𝑇4 + 𝑡> 𝑧 − (2𝜂𝑇4 − 𝑡> ) 𝑦; = 2𝑇4 (𝑧 − 1)𝑒; 1 1 𝑇4 𝜂𝑇4 不完全微分の周波数特性 2𝜂𝑇4 + 𝑡> 𝑦;?8 − (2𝜂𝑇4 − 𝑡> )𝑦; = 2𝑇4 (𝑒;?8 − 𝑒; ) 𝑦;?8 2𝜂𝑇4 − 𝑡> 2𝑇4 = 𝑦 + (𝑒 − 𝑒; ) 2𝜂𝑇4 + 𝑡> ; 2𝜂𝑇4 + 𝑡> ;?8 //積分 integral = integral + m_h * (err + m_err)/2/m_ti; //不完全微分 differential = (2*m_eta*m_td – m_h)*differential/(2*m_eta*m_td + m_h) + 2*m_td*(err – m_err)/(2*m_eta*m_td + m_h)

" 𝐾9 𝐾9 𝐷9 + 𝜔. + 𝐶2 𝜔. " = 𝑒 𝑅9 𝑅9 . 𝐽1 + 𝐽3 " 𝑑(∆𝜔 + 𝜔. ) 𝐾9 + 𝐷9 + (∆𝜔 + 𝜔. ) + 𝐶2 ∆𝜔 + 𝜔. 𝑑𝑡 𝑅9 𝐽1 + 𝐽3 " 𝑑∆𝜔 𝐾9 𝐾9 + 𝐷9 + + 2𝐶2 𝜔. ∆𝜔 = ∆𝑒 𝑑𝑡 𝑅9 𝑅9 " 𝐾9 𝐾9 𝐽1 + 𝐽3 𝑠∆𝜔 + 𝐷9 + + 2𝐶2 𝜔. ∆𝜔 = ∆𝑒 𝑅9 𝑅9 " 𝐾9 𝐽1 + 𝐽3 𝑠 + 𝐷9 + + 2𝐶2 𝜔. 𝑅9 ∆𝜔 = 𝐾9 ∆𝑒 𝑅9 𝐾9 ∆𝜔 = 𝑅9 " 𝐾9 𝐽1 + 𝐽3 𝑠 + 𝐷9 + + 2𝐶2 𝜔. 𝑅9 ∆𝑒 " = 𝐾9 (∆𝑒 + 𝑒. ) 𝑅9 𝐾9 " 𝐾9 𝑅9 𝐷9 + + 2𝐶2 𝜔. 𝑅9 ∆𝜔 = ∆𝑒 𝐽1 + 𝐽3 𝑠+1 " 𝐾9 𝐷9 + 𝑅 + 2𝐶2 𝜔. 9