電気学会論文誌 D（産業応用部門誌） IEEJ Transactions on Industry Applications Vol.135 No.8 pp.827–835 DOI: 10.1541/ieejias.135.827 論 文 限定極配置法による極の影響を考慮した 4 発ロータ型小型無人機の姿勢制御 非会員 菅原 康徳∗ 上級会員 島田 明∗∗ Attitude Control of Quadrotor in Consideration of the Effects of a Pole Based on Limited Pole Placement Yasunori Sugawara∗ , Non-member, Akira Shimada∗∗ , Senior Member （2014 年 6 月 16 日受付，2015 年 4 月 13 日再受付） This paper introduces an attitude control technique for a quadrotor aircraft. Considering that the non-linear characteristics of the aircraft makes it difficult to stabilize, a quadrotor controlled with an adaptive algorithm. Accordingly, we proposed a quadrotor application with backstepping based on the Lyapunov function. Furthermore, we designed a separate actuator control to be mounted on the aircraft for the control of the quadrotor. This approach is often used in industrial equipments. In particular, the limited pole placement (LPP) method is applied to design the controller considering the characteristics of the actuator. The representative simulation results are presented and discussed. キーワード：ヘリコプタ，アクチュエータ，姿勢制御，非線形制御，限定極配置法 Keywords: helicopter, actuator, attitude control, non-linear control, LPP (limited pole placement) 1. 緒 の外乱に対して解決する報告等がある (3) 。他の研究報告で 言 は，機体の姿勢制御に関して唱えられているものが多く，機 体に搭載されている個々のアクチュエータ等のハードウェ 近年，UAV（Unmanned Aerial Vehicle）が世界で注目さ れており，日本では汎用性の高いマルチコプタに注目が集 アに関して精密制御を検討する報告は少ない印象を受ける。 まっている。しかし，航空機が有する特徴的な非線形要素 多くの論文では，アクチュエータは機体のダイナミクスの や，外環境から受ける外乱により安定化は容易ではない。 一部として扱われ，システムの一部としての安定化が施さ 特に機体重心周りのモーメントが，常に連成状態であるこ れている。 そこで，本研究ではマルチコプタ機の非線形ダイナミク と，及び静的安定性を持たないことから精密な制御を行う ことが困難である 。この様なマルチコプタに関する研究 スを考慮しつつ，Lyapunov 関数に基づいた Backstepping は多々報告されており，搭載されるアクチュエータモデル 法による非線形制御を適用することで姿勢制御の安定化を (1) 行う。更に，機体の推力系であるアクチュエータの制御を， (2) の不確かさに対する言及や ，ロータの受ける非線形な空気 力学的問題をロバスト制御理論により解決する手法 ，機 Backstepping 法により導出された擬似制御入力に基づき， 体の特異姿勢回避のために，四元数による非線形フィード アクチュエータの極を考慮し，精密に制御行う。従来，機 バックを利用することで，機体の構造的な不確実性や未知 体の一部として安定化が施されるアクチュエータだが，本 (1) 研究ではマルチコプタ機の安定化とアクチュエータの安定 ∗ 化を別々に考慮する。これは産業機器を制御する手法であ 芝浦工業大学大学院理工学研究科電気電子情報工学専攻 〒 108-8548 東京都港区芝浦 3-9-14 り，機体に搭載されるアクチュエータを個々に制御するこ Division of Electrical Engineering and Computer Science, Graduate School of Engineering, Shibaura Institute of Technology 3-9-14, Shibaura, Minato-ku, Tokyo 108-8548, Japan ∗∗ とで，より精密な制御性を達成できる。アクチュエータの 制御には，通信遅延，及び演算処理遅延を考慮し，むだ時 間補償を行うことが可能な，極配置法の概念を拡張した限 芝浦工業大学デザイン工学部デザイン工学科 〒 108-8548 東京都港区芝浦 3-9-14 定極配置法を利用する (4) 。これまでにアクチュエータの極 の影響が機体の制御系に与える影響を考慮する研究報告は Dept. of Engineering and Design, Shibaura Institute of Technology 3-9-14, Shibaura, Minato-ku, Tokyo 108-8548, Japan c 2015 The Institute of Electrical Engineers of Japan. 数が少なかった。この問題に対して提案手法を用いて数値 シミュレーションを行い，その影響と性能の評価を行う。 827 極影響を考慮した無人機の姿勢制御（菅原康徳，他） 載されるアクチュエータの電気的基本式，及び機械系運動 方程式，ロータに働く誘導抗力，形状抗力，粘性摩擦，を 表す負荷トルクを (7) 式から (10) 式に示す (6) 。 dim + KE ωm · · · · · · · · · · · · · · · · · · · · (7) dt τm = KT im · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (8) dωm + Cm ωm + τl · · · · · · · · · · · · · · · · · · · · · · (9) τm = Jm dt δ dωr b + Cr ωr + ρω2m R4 c + aφt (θt − φt ) τ l = Jr dt 4n 2 · · · · · · · · · · · · · · · · · · ·(10) Vm = Rm im + La Fig. 1. Body frame of Quadrotor. ここで，Vm は電圧，Rm は抵抗，im は電流，La は電機子 インダクタンス，KE は逆起電力定数，ωm は回転角速度， マルチコプタ機の構造と物理モデル 2. 体に搭載される前後ロータが時計回りに，左右ロータが反 ωr はロータの回転角速度，τm はトルク，τl はモータの軸 に働く負荷トルク，KT はトルク定数， Jm は慣性モーメン ト，Cm は粘性抵抗，Cr はロータ粘性摩擦，Jr はロータ慣性 モーメント，n はギア比，b はロータ枚数，ρ は空気密度，R はロータ半径，c は翼弦長，δ は抗力係数，a は 2 次元揚力 傾斜，φt は翼端流入角，θt は翼端ピッチ角を表す。負荷ト ルクを表す (10) 式には非線形項が含まれているが，これは 時計回りに回転することで，ロータ間の反トルクを相殺し マクローリン展開を用いて線形化を施した。アクチュエー ている。 タの回転角速度 ωm と電圧 Vm の関係を伝達関数で表すと， 〈2・1〉 マルチコプタ機の機構 4 発ロータ型無人航空 機では対角線上に位置するロータの推力偏差により生まれ るモーメントを利用することで，水平移動が可能である。 垂直移動では，ロータより発する推力の総和により決定さ れ，ロータが回転する際に発生する反トルクに関しては，機 〈2・2〉 機体の運動方程式 (7) 式から (10) 式より (11) 式が求められる。 Fig. 1 に示す 4 発ロータ ωm (s) A = 2 · · · · · · · · · · · · · · · · · · · · · · · · · (11) Vm (s) s + Bs + C 型無人航空機について考える。ここで，機体重心を機体座 標系 ΣB , 及び地上の任意点に地上座標系 ΣE を設置する。 Newton-Euler 法に基づき，機体座標系各軸における力，及 ここで，A = KT La /(Jm + Jr /n2 ), B = La (La Dm + La KT Ke + び軸周りのモーメントを考慮すると，制御対象の並進運動 Ja Ra )/(Jm + Jr /n2 )，C = Ra La (Dm + KT Ke )/(Jm + Jr /n2 ) と 方程式，及び回転運動方程式は (1) 式から (6) 式で表され する。機体に搭載するアクチュエータは小型であり，低電 る (5) 。 θ̇ψ̇ (Iyy − Izz ) − φ̈ = I xx φ̇ψ̇ θ̈ = (Izz − I xx ) + Iyy 力消費かつ電気的時定数が微小であるため，電圧を利用し Jr θ̇Ωr + I xx Jr φ̇Ωr + Iyy l た回転角速度制御を行う (7) 。 U2 · · · · · · · · · · · · · (1) I xx l U3 · · · · · · · · · · · · · (2) Iyy 〈2・4〉 制御対象の推力系 とアクチュエータの回転角速度 ωm は翼素理論に基づき， (12) 式で表される (8) 。 1 ψ̇θ̇ (I xx − Iyy ) + U4 · · · · · · · · · · · · · · · · · · · · · · (3) Izz Izz 1 ẍ = −g sin θ + (sin φ sin ψ + cos φ sin θ cos ψ) U1 m · · · · · · · · · · · · · · · · · · · · (4) 1 ÿ = g cos φ sin θ + (sin θ sin ψ cos φ − sin φ cos ψ) U1 m · · · · · · · · · · · · · · · · · · · · (5) 1 z̈ = g cos φ cos θ + (cos φ cos θ) U1 · · · · · · · · · · · · · (6) m ψ̈ = T= b ρaωm 2 R3 (θt + φt )c· · · · · · · · · · · · · · · · · · · · · · · (12) 4 ここで，本研究では固定ピッチロータを採用しているため， 翼端ピッチ角 θt ，翼端流入角 φt は定数とする。機体が垂直 移動を行うために必要な推力は，各ロータから発生する推 力総和 U1 により変化し，機体が水平移動を行うために必 要なモーメントは，各ロータから発生する推力偏差によっ て表され，ローリングモーメント U2 ，ピッチングモーメン ト U3 ，ヨーイングモーメント U4 は，各ロータの推力を T ri ここで，φ，θ，ψ は機体座標軸周りの回転角を表し，オイ とすると，(13) 式から (16) 式の様に表される。ここで，K ラー角と同様の回転順序を取る。I xx ，Iyy ，Izz は機体軸に働 は推力係数，D は抗力定数を表している。 く慣性モーメント，Jr はロータの慣性モーメント，Ωr は U1 = K(T r21 + T r22 + T r23 + T r24 ) · · · · · · · · · · · · · · · · · ·(13) ロータの回転角速度偏差，l は重心からアクチュエータまで の距離，m は機体質量，g は重力加速度，U1 は機体座標系 U2 = K(T r24 − T r22 ) · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(14) において垂直方向に働く推力の合力，U2 ，U3 ，U4 はそれ U3 = K(T r23 − T r21 ) · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(15) ぞれ機体各軸に働くモーメントであり制御入力を表す。 〈2・3〉 アクチュエータのモデリング ロータが発生する推力 T U4 = D(T r21 − T r22 + T r23 − T r24 ) · · · · · · · · · · · · · · · · · ·(16) 制御対象に搭 828 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） ここで，X2 は Z1 の安定化するための仮想制御入力であり X2 = Ẋ1 を満たす。(19) 式が負定であれば，Lyapunov 関数 候補 (18) 式が導出される。X2 は (20) 式の様に表される。 X2 = ẋ1d + α1 Z1 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (20) ここで，α1 (α1 > 0) は正定値を安定化するための重みであ る。(20) 式に (19) 式を代入すると，(21) 式の様に書き直さ Fig. 2. Block diagram of the control system. れる。 3. V̇(Z1 ) = −α1 Z1 2 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (21) 制御系設計 〈3・1〉 Backstepping 法 ここで，新たな変数 Z2 を導入する。Z2 は 機体の姿勢制御を Lyapunov 関数に基づく Backstepping 法により行い，これにより得ら Z2 = X2 − ẋ1d − α1 Z1 · · · · · · · · · · · · · · · · · · · · · · · · · · (22) れた擬似制御入力を用いて機体の安定化を行う。機体の制 御系ブロック線図を Fig. 2 に示す。機体運動方程式である の様に表される。以上よりシステムを安定化する Lyapunov (1) 式から (6) 式を安定化に必要なサブシステムと考え，計 関数候補は (23) 式の様に表される。 算過程の簡略化のため，下記の変数を導入する。これより V(Z1 , Z2 ) = (1) 式から (6) 式は (17) 式で表されるサブシステムに書き 直され，擬似制御入力 U fi により安定化を施す。 ⎫ ⎧ ⎪ ⎪ ⎪ ⎬ ⎨X1 = φ X2 = φ̇ X3 = θ X4 = θ̇ X5 = ψ X6 = ψ̇ ⎪ X=⎪ ⎪ ⎪ ⎭ ⎩ X7 = x X8 = ẋ X9 = y X10 = ẏ X11 = z X12 = ż⎪ 1 2 (Z1 + Z2 2 )· · · · · · · · · · · · · · · · · · · · · · (23) 2 (23) 式の導関数を導くと，(24) 式の様に表される。 V̇(Z1 , Z2 ) = Z1 Ż1 + Z2 (Ẋ2 − ẍ1d − α1 Ż1 ) = Z2 (A1 X4 X6 + A2 X4 Ωr + B1 U f2 ) A1 = (Iyy − Izz )/I xx , A2 = Jr /I xx , A3 = (Izz − I xx )/Iyy −Z2 ( ẍ1d − α1 (Z2 + α1 Z1 )) A4 = Jr /Iyy , A5 = (I xx − Iyy )/Izz −Z1 Z2 − α1 Z1 2 · · · · · · · · · · · · · · · · · · · · (24) B1 = l/I xx , B2 = l/Iyy , B3 = 1/Izz u x = sin φ sin ψ + cos φ sin θ cos ψ (24) 式に対して擬似制御入力 U f2 は，Ẍ1d = 0，V̇(Z1 , Z2 ) < 0 uy = sin θ sin ψ cos φ − sin φ cos ψ ⎞ ⎛ ⎟⎟⎟ ⎜⎜⎜ X2 ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ X4 X6 A1 − X4 A2 Ωr + B1 U f2 ⎟⎟⎟ ⎜⎜⎜⎜ ⎟⎟⎟ X4 ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ X2 X6 A3 + X2 A4 Ωr + B2 U f3 ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ X6 ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ X4 X2 A5 + B3 U f4 ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ X8 Ẋ = f (X, U) = ⎜⎜⎜⎜⎜ ⎟⎟⎟ ⎟⎟⎟ ⎜⎜⎜ U f1 ⎟⎟⎟ ⎜⎜⎜ −g sin X3 + u x ⎟⎟⎟ ⎜⎜⎜ m ⎟⎟⎟ ⎜⎜⎜ X 10 ⎟⎟⎟ ⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ U f1 ⎟⎟⎟ ⎜⎜⎜ g cos X sin X + u 1 3 y ⎟⎟⎟ m ⎜⎜⎜⎜ ⎟⎟⎟ ⎜⎜⎜ X ⎟⎟ 12 ⎜⎜⎜ ⎜⎜⎝ U f1 ⎟⎟⎟⎟⎠ g cos X1 cos X3 + cos X1 sin X3 m · · · · · · · · · · · · · · · · · · ·(17) を満たすと仮定すると， U f2 = 1 (Z1 − A1 X4 X6 − A2 X4 Ωr B1 −α1 (Z2 + α1 Z1 ) − α2 Z2 ) · · · · · · · · · · · · · · · · (25) ここで，α2 (α2 > 0) は Z1 を安定化するために導入した重み である。U f1 , U f3 , U f4 に関しては U f2 の計算と同様の手順 により導いた。これより (26) 式から (28) 式にその結果を 示す。 m (Z7 − g cos X1 cos X3 cos X1 cos X3 −α7 (Z8 + α7 Z7 ) − α8 Z8 ) · · · · · · · · · · · · · · · · (26) 1 U f3 = (Z3 − α3 X2 X6 − α4 X2 Ωr B2 −α3 (Z4 + α3 Z3 ) − α4 Z4 ) · · · · · · · · · · · · · · · · (27) 1 U f4 = (Z5 − α5 X2 X4 B3 −α5 (Z6 + α5 Z5 ) − α6 Z6 ) · · · · · · · · · · · · · · · · (28) U f1 = 次に，(17) 式を漸近安定化するために，追従誤差 Z1 = x1d −X1 を定義する。これに対する Lyapunov 関数候補式は (18) 式 ここで，Z3 = X3d − X3 ，Z4 = X4 − Ẋ3d −α3 Z3 ，Z5 = X5d − X5 ， の様に表される。 Z6 = X6 − Ẋ5d −α5 Z5 ，Z7 = X11d −X11 ，Z8 = X12 − Ẋ11d −α7 Z7 とする。並進運動方程式である (4) 式，及び (5) 式より，機 体軸 X，Y，Z 方向の運動は全て U f1 に依存しているが， X ，Y 軸方向の運動では u x , uy の方向ベクトルを考慮する 必要がある。方向ベクトルに関して Backstepping 法により Lyapunov 関数，重みを定義し，システムの安定化を図る。 V(Z1 ) = 1 2 Z1 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (18) 2 (18) 式に対する導関数を導くと (19) 式の様に表される。 V̇(Z1 ) = Z1 ( ẋ1d − X2 )· · · · · · · · · · · · · · · · · · · · · · · · · · (19) 829 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） u x ，uy の Lyapunov 関数，及び重みの導出過程は，前述と 同様であるため簡略化し，(29) 式，及び (30) 式に u x ，uy の 疑似制御入力を示す。 m (Z9 + g sin X3 − α9 (Z10 + α9 Z9 ) − α10 Z10 ) U1 · · · · · · · · · · · · · · · · · · ·(29) m U fy = (Z11 − g cos X1 sin X3 U1 − α11 (Z12 + α11 Z11 ) − α12 Z12 ) · · · · · · · · · · · ·(30) U fx = Fig. 3. PID angular velocity control system (Continuous system). ここで，Z9 = X7d −X7 ，Z10 = X8 − Ẋ7d −α9 Z9 ，Z11 = X9d −X9 ， Z12 = X10 − Ẋ9d − α11 Z11 とする。 〈3・2〉 アクチュエータ動作参照値推定 (25) 式から (28) 式より求められた擬似制御入力により，ロータの回転 角速度指令値 Ωrdi を求めるため，以下の連立方程式を解く。 Fig. 4. PID angular velocity control system (Discrete system). U f1 = K(Ω2rd1 + Ω2rd2 + Ω2rd3 + Ω2rd4 ) · · · · · · · · · · · · ·(31) U f2 = K(Ω2rd4 − Ω2rd2 ) · · · · · · · · · · · · · · · · · · · · · · · · · ·(32) U f3 = K(Ω2rd3 − Ω2rd1 ) · · · · · · · · · · · · · · · · · · · · · · · · · ·(33) Fig. 5. Digital control system for limited pole placement. U f4 = D(Ω2rd1 − Ω2rd2 + Ω2rd3 − Ω2rd4 ) · · · · · · · · · · · · ·(34) (31) 式から (34) 式に関して，Ωrdi について解くと，(35) 式 から (38) 式で表される。各アクチュエータに対する回転角 Fig. 4 に示す。制御対象 P(z) はアクチュエータ (11) 式をゼ ロ次ホールダを用いて離散化したものであり，制御器 K(z) は PID 制御器の微分要素を差分法，積分要素を台形積分法 速度指令を得る。 D(U f1 − 2U f3 ) − KU f4 · · · · · · · · · · · · · · ·(35) √ 2 DK D(U f1 − 2U f2 ) + KU f4 =− · · · · · · · · · · · · · ·(36) √ 2 DK D(U f1 − 2U f3 ) − KU f4 = · · · · · · · · · · · · · · ·(37) √ 2 DK D(U f1 + 2U f2 ) + KU f4 =− · · · · · · · · · · · · · ·(38) √ 2 DK により離散化したものである。ここでフィードフォワード Ωrd1 = Ωrd2 Ωrd3 Ωrd4 制御に，逆起電力定数 KE を利用した。これはアクチュエー タの制御を電圧による回転角速度制御を行っているため， アクチュエータ内に発生する逆起電力が制御系内に影響を 及ぼすことを低減するためである。PID 制御器 K(z) は (39) 式の様に表され，制御パラメータ部，及び係数固定部に分 割を行う。これは制御パラメータである K p ，Ki ，Kd の導 出に関与しない項を抜き出すことに等しい。制御対象であ る (40) 式のパラメータは変動しないため，(39) 式のむだ時 (31) 式から (34) 式の関係より，(35) から (38) の分子平方根 内の値は必ず正となる。得られた回転角速度指令値 Ωrdi は 間 (1/z) を含む係数固定部と組み合わせ，係数固定部と制 機体姿勢を安定化する理想的な状態量とし，この指令値に 表される。 御パラメータ部の有理関数は，(41) 式，及び (42) 式の様に 対して，ロータの回転角速度が追従する様にアクチュエー することができる。連続系制御ブロック線図を Fig. 3 に示 Knp (z) Kn f (z) × Kd p (z) Kd f (z) T Kd (z−1)2 +Ki z (z+1) K p z (z−1)+ 1 T 2 × = 1 z (z−1) · · · · · · · · · · · · · · · · · · ·(39) N1 z + N0 Pn (z) = · · · · · · · · · · · · · · ·(40) P(z) = Pd (z) (z − D1 )(z − D2 ) n(z) Kn f (z) Pn (z) 1 × = × d(z) Kd f (z) znm Pd (z) b 1 z + b0 · · · · · · · · · · · · · · · · ·(41) = 5 z + a 4 z 4 + a 3 z 3 + a2 z 2 Knp (z) β2 z2 + β1 z + β0 β(z) = = · · · · · · · · · · · · · ·(42) α(z) Kd p (z) α0 し，これを離散表現に記述した離散系制御ブロック線図を ここで，n(z)/d(z) は係数固定部の有理関数，β(z)/α(z) は K(z) = タの制御器を設計する。 〈3・3〉 限定極配置法–アクチュエータ制御 擬似制御 入力により求められた回転角速度指令値に基づき，離散時 間系でのアクチュエータ制御を行う。通信遅延，及び演算 処理遅延が発生すると仮定し，制御系のサンプリングタイ ムを 10 msec とする。制御系内に 1 サンプリングタイムの むだ時間が存在するとし限定極配置法による固定構成制御 器による制御系を設計する。これは，機体に搭載される計 算機を用いて，アクチュエータの回転角速度制御を行うた め，計算負荷の小さい制御器を設計するためであり，限定 極配置法を利用することで，制御器パラメータ設計の効率 化が図れ，むだ時間要素による制御系への影響を明らかに 830 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） 制御パラメータ部の有理関数，むだ時間は nm = 1 とした。 以上の結果から限定極配置法による極配置を行う。制御パ この際，2 つの有理関数の関係を Fig. 5 に示し，係数固定 ラメータ部，及び従属極の係数ベクトルを ζ ，配置極の係 部とパラメータ部の閉ループ伝達関数を作成すると，(43) 数ベクトルを μ とすると (47) 式が (49) 式で表され，制御 式の様に表される。 パラメータ部を導くことができる。(49) 式を行列表現に直 β(z) n(z) × δ(z) β(z)n(z) α(z) d(z) = G(z) = = β(z) n(z) α(z)d(z) + β(z)n(z) γ(z) 1+ × α(z) d(z) · · · · · · · · · · · · · · · · · · ·(43) すと (50) 式となり，各行列成分は (51) 式から (53) 式で表 される。 Pn p (z)znq = α(z)d(z) + β(z)n(z) − Qnq (z)Pn p (z) · · ·(49) ζ T = μT E −1 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(50) T ζ T = α0 β0 β1 β2 Q0 Q1 · · · · · · · · ·(51) T · · · · · · · · · · · ·(52) μT = 0 0 P0 P1 P2 1 ⎤ ⎡ ⎢⎢⎢ 0 a3 a4 1 ⎥⎥⎥ 0 a2 ⎥⎥ ⎢⎢⎢⎢ b1 0 0 0 0 ⎥⎥⎥⎥ ⎢⎢⎢ b0 ⎥⎥ ⎢⎢⎢ b0 b1 0 0 0 ⎥⎥⎥⎥⎥ ⎢⎢⎢ 0 E = ⎢⎢⎢ ⎥ · · · · · ·(53) ⎢⎢⎢ 0 0 b0 b1 0 0 ⎥⎥⎥⎥⎥ ⎥⎥ ⎢⎢⎢ ⎢⎢⎢ −P0 −P1 −P2 −1 0 0 ⎥⎥⎥⎥ ⎥⎦ ⎢⎣ 0 −P0 −P1 −P2 −1 0 限定極配置法では (43) 式が所望の配置極を持つ様に α(z)， β(z) を定める。ここで，配置極の数を n p ，従属極の数を nq ， 配置極を p1 , . . . , pn p ，従属極を q1 , . . . , qnq とすると，γ(z) は (44) 式の様に表される。配置極とは指定極を表し，従属 極とは自動的に決定する配置不可能な極を表す。 γ(z) = γnγ znγ + γnγ −1 znγ −1 + · · · + γ1 z + γ0 = (Pn p zn p + Pn p −1 zn p −1 + · · · + P1 z + P0 ) × (znq + Qnq −1 znq −1 + · · · + Q1 z + Q0 ) (51) 式から (53) 式を (50) 式に適用することで，制御パラ メータ部を導出し，K p ，Ki ，Kd を (42) 式より求める。 = Pn p (z)(zn p + Qnq (z)) · · · · · · · · · · · · · · · · · · · · · (44) ここで，Pn p ，及び Qnq はそれぞれ配置極，及び従属極を多 4. 項式に因数分解した際の係数であり (×) は乗算演算子を表 Kalman Filter による姿勢推定 未知の状態量を推定するために同一次元オブザーバを設 す。このとき，nq は (45) 式により定めることが出来る。 計する。オブザーバの設計にはカルマンフィルタを利用す nq = (nγ + 1) − (nα + 1 + nβ + 1) るが，(17) 式より，機体の系 f (X, U) は連成項を持つ事が分 = nγ − nα − nβ − 1 · · · · · · · · · · · · · · · · · · · · · · · · · ·(45) かる。つまり，変数 X に対して関数 f (X) は線形関係にな いことを示している。そこで，制御対象の状態推定を行う ここで，nα , 及び nβ は制御パラメータ部の分母次数，及び 手法として Unscented Kalman Filter（以後：UKF）を用い 分子次数である。(45) 式は特性多項式 γ(z) の係数個数から る (9) 。航空機の状態推定には状態方程式を一次近似により 制御パラメータ部の係数個数を減算したものを表している。 以上より，(43) 式の γ(z) は (46) 式の様に考えられる。 線形化した Extended Kalman Filter（以降：EKF）が用いら γ(z) = α(z)d(z) + β(z)n(z) = Pn p (z)(z + Qnq (z)) のみを抽出し，それに対してカルマンフィルタを適用する れることがあるが (10) ，EKF は一次の微分項にある線形関係 nq · · · · · · · · · · · · · · · · · · ·(46) ことで非線形性に対応しているため，二次以上の高次微分 などを持つシステムや不連続点を含むシステムに対しては (46) 式を変形すると (47) 式で表される。 α(z)d(z) + β(z)n(z) − Qnq (z)Pn p (z) = Pn p (z)z 推定結果が不安定になることがある。そのため，本稿では UKF を制御対象に適用した。UKF は EKF の様な線形近似 nq を行わず，状態変数の分布を数点の代表点と捉えることで， · · · · · · · · · · · · · · · · · · ·(47) 非線形モデルに基いて直接的に状態遷移後の分布を推定す (41) 式，及び (42) 式より係数固定部，及び制御パラメータ 部の有理関数の分母，分子次数は nd = 5，nn = 2，nα = 0， nβ = 2 であり，Fig. 4 に示すむだ時間を持つ離散時間系の 閉ループ極数は，nγ = 5 から極数は 5 個となる。これらを (45) 式に当てはめると，従属極の個数は nq = 2，配置極の 個数は制御器パラメータの個数と同様の n p = 3 である。こ こで，配置極を p1 ，p2 ，p3 ，従属極を q1 ，q2 とする。(44) 式の特性多項式を利用すると，(48) 式の様に表される。 γ(z) = Pn p (z) znq + Qnq (z) = z3 + P2 z2 + P1 z + P0 z2 + Q1 z + Q0 る手法 (11) であり，強い非線形性を持つ本稿の機体にも有効 である。UKF のデメリットとして挙げられる計算時間の長 大化であるが，U 変換を利用する UKF では，EKF と同程 度の計算量で推定を行えることが報告されており (12) ，本稿 でも上記条件から利用可能な推定手法だと考えられる。こ れより UKF を用いた状態推定を行うが，EKF との推定精 度の比較を行うため，EKF に基づく同一次元オブザーバに よる状態推定も同様に設計を行う。 〈4・1〉 Extended Kalman Filter による姿勢推定 観測出力からフィルタリングステップ k の状態 x(k) の姿 勢推定を行う。機体の並進運動方程式，及び回転運動方程 · · · · · · · · · · · · · · · · · · ·(48) 式 (1) 式から (6) 式は (54) 式の非線形状態空間モデルで表 831 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） 事後共分散行列，K はシグマポイントに対する重みを調整 される。 するためのスケーリングパラメータを表す。式中の平方根 xk+1 = fk (xk , uk ) + wk · · · · · · · · · · · · · · · · · · · · · · · · · ·(54) は行列 (n + K)Pk−1|k−1 の行列平方根 L を表し， yk = hk (xk ) + vk · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(55) LT L = (n + K)Pk−1|k−1 · · · · · · · · · · · · · · · · · · · · · · · · · (66) ここで，xk = [φ, φ̇, θ, θ̇, ψ, ψ̇, x, ẋ, y, ẏ, z, ż]T であり，wk はシ ステム雑音，vk は観測雑音，uk は制御入力，yk は観測値を の i 番目の行成分を表し，Cholesky 分解を利用することで 表す。状態推定値の初期値 xˆ0 は正規性確立ベクトルとし， 求め，シグマポイントに対する重みの与え方は， ガウス性を持つとする。(54) 式，及び (55) 式を xk の推定値 λ · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(67) n+λ λ + 1 − α2 + β · · · · · · · · · · · · · · · · · · · · · ·(68) W0c = n+λ 1 , i 0 · · · · · · · · · · · · · · · · · · · · · · · · ·(69) Wim = 2(n + λ) より求まる次ステップの推定値 x̄k+1 = fk ( x̂k , ūk ) 近傍で，テ W0m = イラー展開による線形近似化を行うと，(54) 式，及び (55) 式は線形状態空間モデル (56) 式，及び (57) 式と表される。 ( x̄k+1 − xk+1 ) = Φk + ( x̂k − xk ) + Γk (uk − ūk ) + vk · · · · · · · · · · · · · · · · · · ·(56) ここで，(67) 式から (69) 式に示す添字 m は平均に対する yk = hk+1 ( x̄k+1 ) + Hk (xk+1 − x̄k+1 ) + wk+1 · · · · · · · ·(57) ∂f ∂f ここで，Φk = ∂xk ，Γ = ∂uk ，Hk = ∂h∂xk+1 は非線 x̂ ,ū x̂ ,u¯ x̄ たし，α, β はシグマポイントの統計方法を決定づけるパラ 形状態空間モデルにおけるヤコビアン行列であり，ともに メータ，n は状態変数 xk の次数を表している。 k k k k 重み，c は分散に対する重み，λ は λ = α2 (n + K) − n を満 k+1 〈4・2・2〉 時間更新–状態予測 時変である。これらの式に対してカルマンフィルタの時間 更新式，観測更新式を適用すると (13) ，EKF のアルゴリズム 遷移させ，遷移後のシグマポイントの分布より，k 番目の は次の様に表される。 参照点に対する事前確率密度関数を計算し更新を行う。こ x̄k+1 = fk ( x̂k , ūk ) · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(58) こで，ŷ は観測値の推定値を表している。 P̄k+1 = Φk P̂k ΦTk + Γk Qk Γk + Qk · · · · · · · · · · · · · · · ·(59) Xk|k−1 = f (Xk−1|k−1 , uk−1 , k) · · · · · · · · · · · · · · · · · · · ·(70) 2n " x̂k|k−1 = Wim Xi,k|k−1 · · · · · · · · · · · · · · · · · · · · · · · ·(71) x̂k+1 = x̄k+1 + Kk+1 (yk+1 − hk+1 ( x̄k+1 )) · · · · · · · · · · ·(60) Kk+1 = 先に求めた事後確率密 度関数を表すシグマポイントを (54) 式，及び (55) 式に従い T T P̄k+1 Hk+1 (Hk+1 P̄k+1 Hk+1 + σ2k+1 )−1 · · · · · ·(61) P̂k+1 = (I − Kk+1 Hk+1 )P̄k+1 · · · · · · · · · · · · · · · · · · · · ·(62) i=0 Pk|k−1 = ここで，x̄k+1 は事前状態推定値，P̄k+1 は事前誤差共分散行 列，Qk はプロセスノイズ， x̂k+1 は状態推定値，Kk+1 はカ 2n " Wic (Xi,k|k−1 − x̂k|k−1 ) i=0 は確率変 × (Xi,k|k−1 − x̂k|k−1 )T + Q · · · · · · · · · · · · · · ·(72) 数に対する分散を表す。線形化された状態空間モデル (56) Yk|k−1 = h(Xk|k−1 , uk−1 , k) · · · · · · · · · · · · · · · · · · · · · ·(73) 2n " ŷk|k−1 = Wim Yi,k|k−1 · · · · · · · · · · · · · · · · · · · · · · · ·(74) ルマンゲイン，P̂k+1 は事後誤差共分散行列，σ2k+1 式，及び (57) 式を状態推定対象とし，導出された (58) 式か ら (62) 式を逐次的に繰り返すことで，対象の状態の推定を i=0 行う。 〈4・2〉 Unscented Kalman Filter による姿勢推定 ここで，Q はプロセス雑音を表している。 機体の運動方程式を非線形状態空間モデルとし，(54) 式， 〈4・2・3〉 観測値更新 事前確率密度関数を k 番目の 及び (55) 式の状態推定を行う。(54) 式，及び (55) 式の非 参照点に対する予測値とセンサから得られる k 番目の参照 線形状態空間モデルに対して，k 番目の参照点に対する機 点の観測値との誤差を修正し，k 番目の参照点に関する事 体姿勢推定値 xˆk , 更に共分散 Pk を導出する UKF の逐次状 後確率密度関数を求める。以下，観測更新式を記述する。 態推定は次の様に記述される。 〈4・2・1〉 シグマポイントの計算 機体姿勢推定値の Pyk 事後確率密度関数を 2n + 1 個のシグマポイント Xk−1 と，そ 2n " yk = Wic (Yi,k|k−1 − ŷk|k−1 ) yk × (Yi,k|k−1 − ŷk|k−1 )T + R · · · · · · · · · · · · · · ·(75) 2n " = Wic (Xi,k|k−1 − x̂k|k−1 ) i=0 れぞれの項に対する重み Wk−1 で表す。シグマポイントは， X0,k−1|k−1 = x̂k−1|k−1 · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(63) ! Xi,k−1|k−1 = x̂k−1|k−1 + (n + K)Pk−1|k−1 · · · · · ·(64) i ! Xi,k−1|k−1 = x̂k−1|k−1 − (n + K)Pk−1|k−1 · · · · · ·(65) P xk i=0 × (Yi,k|k−1 − ŷk|k−1 )T · · · · · · · · · · · · · · · · · · ·(76) Kk = P xk yk P−1 yk yk · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(77) i ここで，(64) 式は (i = 1, . . . , n)，(65) 式は (i = n+1, . . . , 2n+ x̂k|k = x̂k|k−1 + Kk (y − ŷk | k−1 ) · · · · · · · · · · · · · · · · · · ·(78) 1)，x̂k|k は時刻 k における状態の事後推定値，Pk|k は状態の Pk | k = Pk | k−1 − Kk Pyk yk KkT · · · · · · · · · · · · · · · · · · ·(79) 832 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） ここで，R は観測雑音を表している。以上より，事後推定 せるごとに目標に対する収束性が向上するが，0.7098（極 値 x̂k|k ，及び事後推定共分散 Pk|k を得られた。(63) 式から (79) 式までの計算過程を 1 回とし，これを逐次的に繰り返 すことで機体姿勢の状態推定を行う。これより，UKF の性 能の確認のため，次章では先に設計を行った EKF との性能 配置法 −1300 相当）の極に配置した際に，制御系が不安定 差についての検証を行う。 時間による位相遅れの影響を微分要素で補っていることが 5. になった。この理由は次節で説明を行う。Table 1 を確認す ると，従来の極配置法よりも限定極配置法が全体的にパラ メータが減少しており，各パラメータを確認すると，むだ 確認できる。 数値シミュレーション 〈5・1〉 アクチュエータの回転角速度制御 〈5・2〉 極軌跡解析 Fig. 7 に配置極と従属極の関係を 示す。ここで，◦ 印は配置極，× 印は従属極を表している。 先に設計を 行った限定極配置法の効果を確認するため，連続時間系に 配置極を速い位置に移動すると，従属極が遅い位置に移動 基づくむだ時間を考慮しない極配置法との性能比較を行う。 する事が分かる。0.7970（極配置法 −900 相当）に配置極 極配置法については Fig. 3 に示すブロック線図を基に設計 を移動すると従属極と重なり，以降，更に速い位置に配置 を行った。目標値には 5-1-5 多項式に基づく基本軌道関数 極を移動させると，0.7098（極配置法 −1300 相当）では， を用いた。目標値は，機体がホバリング状態であると仮定 極位置が入れ替わりシステムが不安定になった。極位置の し，Ωrd = 220.0 [rad/s] とした。Fig. 6 に極配置法と限定極 配置法の結果を示す。解析を簡易化するため連続系極配置， Table 1. Control parameters for discrete model and detrmined poles. 及び限定極配置法に関して配置する極は重複極とする。シ ミュレーションは Table 1 の各制御パラメータを，Fig. 4 に - 0.9888 0.9866 0.7970 示す離散時間系モデルに適用したものであり（表中の E±4 - −20 −30 −900 −1300 の表記は 1.0 × 10±4 を表す） ，サンプリングタイムは 10 ms Kp 19.474 44.318 7.1E+04 8.3E+04 である。Table 1 に示す，K p ，Ki ，Kd は連続系極配置制御 器値，KL p ，KLi ，KLd は離散系限定極配置法制御器値を表 す。配置極を速い位置に移動させるごとに，各配置法の応 0.7099 Ki 3.626 10.090 400.0 433.333 Kd 0.073 0.030 8.3E-04 7.6E-04 KL p −0.354 −0.300 5.378 3.842 KLi −0.039 −0.151 2.920 −2.991 8.045 答が目標値に対して，収束性が向上するが，従来の極配置法 KLd 0.328 0.307 0.060 では，徐々に応答が振動的になり，極が −30 を越えると不 q1 −0.031+0.183i −0.027+0.165i 0.774 1.078 q2 −0.031-0.183i −0.027-0.165i −0.261 −0.239 安定となった。限定極配置法では，極を速い位置に移動さ (a) Poles assigned at 0.9910 (−20) (a) Poles assigned at 0.9910 (−20) (b) Poles assigned at 0.9888 (−30) (b) Poles assigned at 0.9888 (−30) (c) Poles assigned at 0.7970 (−900) (c) Poles assigned at 0.7970 (−900) (d) Poles assigned at 0.7098 (−1300) (d) Poles assigned at 0.7098 (−1300) Fig. 6. Angular velocity response of the digital PID system. Fig. 7. Pole location of the digital PID control system (Parameter of table1). 833 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） (a) Roll angle (a) Reference trajectory tracking (X-Y-Z) (b) Pitch angle (c) Yaw angle Fig. 8. Simulation: Backstepping attitude controller. 入れ替わり前後の Table 1 のパラメータを確認すると，パ ラメータが不安定化していることが分かる。これより制御 (b) Reference trajectory tracking (X-Y) 対象の性能限界が，0.7970（極配置法 −900 相当）付近に Fig. 9. Simulation: movement 存在する事が分かる。しかし，配置極と従属極が入れ替わ Controller tracking an circular ることで，設計を行ったシステムが不安定化する事象が数 学的に証明できていないことが今後の課題である。 〈5・3〉 機体の姿勢制御 な姿勢推定が行えていることが分かる。EKF による姿勢推 機体の数学モデル，制御器 定では，初期姿勢の乱れが顕著に表れるものの，その後，安 の有効性，及び UKF の効果を確認するため，機体の姿勢制 定化状態に至った。しかし，真値近辺で振動的値が検出され 御の検証を行う。機体の初期姿勢は， る結果となった。ヤコビ行列に基づく EKF では制御対象が 持つ非線形特性を推定することが困難であることが分かる。 Ed = [0.5235, 0.5235, 0.5235]· · · · · · · · · · · · · · · · (80) 次に機体の位置制御系の検証を行う。機体の位置指令は半径 2 m の円運動を指示した。アクチュエータの制御には限定 極配置法のサーボ限界を，姿勢値は UKF の推定値を利用し た。Backstepping 法による重みは α7 = 10.22，α8 = 9.39， α9 = 45.55，α10 = 32.84，α11 = 34.42，α12 = 25.73 とし た。シミュレーション結果を Fig.9 に示す。ここで極配置 とする。これは機体座標系から見て X，Y，Z 軸回りに 30 [deg] 傾いた姿勢である。機体が得る推力に関しては， Backstepping 法に基づき擬似制御入力により求められた 回転角速度指令値をもとに，先に設計した限定極配置法に よりアクチュエータを制御することで得ている。各サブシ ステムの重みは，α1 = 20.68，α2 = 6.05，α3 = 15.53， α4 = 9.84，α5 = 13.22，α = 38.17 とした。前章で設計を 行った EKF を同条件下においてシミュレーションを行った 結果，及び UKF を用いた状態推定結果を Fig. 8 に示す。こ こで，EKF のプロセス雑音 Qk = 1.0 × 10−4 × I 12×12 ，観測雑 音 Pk = 2.0 × 102 × I 12×12 ，共分散 σk = 100，UKF の事後推 定共分散の初期値 P0|0 = 1.0 × 10−4 × I 12×12 ，プロセス雑音 Q = 1.0×10−4 ×I 12×12 ，観測雑音 R = 2.0×10−4 ×I 12×12 であ り，I は単位行列を表し，シグマポイントの決定パラメータ α = 1.0 × 10−4 ，及び β = 2.0，λ = 2.0，次数 n = 12 である。 UKF による機体の姿勢角推定値はそれぞれ収束速度に差が 発生しているが，各々 1 秒以内に 0 [rad] に収束し安定状態 にあることが確認できる。更に，UKF の推定値が実際のセ ンサデータよりも真値に近いことから，UKF により高精度 法との性能比較を行う。極配置法のサーボ限界を利用し， 限定極配置法と同条件で検証を行った。参照値に対して両 制御器の収束精度は高いが，限定極配置法はむだ時間を考 慮した設計法であるため，位相遅れ補償を行えていること から，極配置法よりも精度の高い収束性を見せている。 6. 結 言 本稿では，4 発ロータ型無人航空機の数学モデルに対し て Backstepping 法，及び限定極配置法によるアクチュエー タの回転角速度制御を併用し，UKF による状態推定値を用 いた姿勢安定化制御器の設計を行った。従来，機体のダイ ナミクスの一部として扱われ，個々に制御系が施されない アクチュエータの極が持つ影響を考慮することで，過去の 報告と比較し，安定かつ追従性の高い制御系を構成できる 834 IEEJ Trans. IA, Vol.135, No.8, 2015 極影響を考慮した無人機の姿勢制御（菅原康徳，他） ことをシミュレーションにより確認を行った。限定極配置 （11） S. Brunke and M. Campbell: “Estimation architecture for future autonomous vehicles”, In American Control Conference, Vol.2, pp.1108–1114 (2002) （12） 山北昌毅： 「UKF(Unscented Kalman Filter) って何?」 , ISCIE Trans, Vol.50, No.7, pp.261–266 (2006) （13） 足立修一・丸田一郎： 「カルマンフィルタの基礎」 , 東京電機大学出版 局 (2012) 法は近年報告された極配置法，位相遅れ補償の手法であり， 今後，更なる検証や解析が必要である。 文 献 （ 1 ） A.T. Gaitan and Y. Bolea: “Modeling and Robust Attitude Control Quadrotor System”, Proc. CCE Conf. on Electrical Engineering, No.10, pp.7–12, Mexico City, Mexico (2013) （ 2 ） D. Seko and M. Yokoyama: “Stabilization of a mini helicopter with four rotors via backstepping”, JSME Dynamics and Design Conference, No.10, pp.444–449 (2007) （ 3 ） D. Chen, X. Bin, Z. Bo, Z. Xu, and L. Shibo: “An output feedback attitude tracking controller design for quadrotor unmanned aerial vehicles using quaternion”, Proc. IEEE/RSJ Conf. on IROS, pp.3051–3056, Tokyo, Japan (2013) （ 4 ） Y. Urakawa: “Parameter Design for a Digital Control System with Calculation Delay Using a Limited Pole Placement Method”, IEEJ Trans. IP, Vol.133, No.3, pp.272–281 (2013) （ 5 ） S. Bouabdallah and R. Siegwart: “Backstepping and Sliding-mode Techniques Applied to an Indoor Micro Quadrotor”, Proc. IEEE Conf. on Robotics and Automation, Barcelona, Spain (2005) （ 6 ） 島田 明・大石 潔・柴田昌明・市川 修： 「EE テキスト モーショ ンコントロール」 , オーム社 (2004) （ 7 ） 松原 厚： 「精密位置決め・送り系設計のための制御工学」 , 森北出版 (2008) （ 8 ） 加藤寛一郎・今永勇生：「ヘリコプタ入門」 , 東京大学出版会 (1985) （ 9 ） 服部泰治・足立修一： 「UKF による未知非線形アクチュエータによ り駆動される非線形システムの同時推定」 , SICE Trans, Vol.45, No.5, pp.286–288 (2009) （10） N. Abas, A. Legowo, Z. Ibrahim, N. Rahim, and M.K. Anuar: “Modeling and System Identification using Extended Kalman Filter for a Quadrotor System”, Proc. EEA Conf. on Applied Mechanics and Materials, Vol.313314, pp.976–981, Bali, Indonesia (2013) 菅 原 康 徳 （非会員） 2013 年 3 月芝浦工業大学デザイン工 学部デザイン工学科卒業。同大学大学院理工学研 究科電気電子情報工学専攻卒業。現在に至る。 島 田 明 （上級会員） 1983 年 3 月電気通大学電子工学科 卒業。同年 4 月（株）第二精工舎（現セイコーイ ンスツル（株） ）に入社し，産業用ロボットコント ローラ開発等に従事。1994 年 4 月より千葉大学 非常勤講師を兼務，1999 年 4 月より客員教授を 兼務。2001 年 4 月より職業能力開発総合大学校 電気システム工学科助教授。 2007 年 4 月より准 教授。2009 年 4 月より芝浦工業大学デザイン工 学部教授。IEEE, 計測自動制御学会，日本ロボット学会等の会員，博 士（工学）（1996 年，慶応義塾大学）。 835 IEEJ Trans. IA, Vol.135, No.8, 2015
© Copyright 2024 ExpyDoc
