電気学会論文誌 D（産業応用部門誌）IEEJ Transactions on Industry ApplicationsVol.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 PolePlacement

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 charac-

teristics 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. 緒 言

近年，UAV（Unmanned Aerial Vehicle）が世界で注目さ

れており，日本では汎用性の高いマルチコプタに注目が集まっている。しかし，航空機が有する特徴的な非線形要素や，外環境から受ける外乱により安定化は容易ではない。特に機体重心周りのモーメントが，常に連成状態であるこ

と，及び静的安定性を持たないことから精密な制御を行うことが困難である (1)。この様なマルチコプタに関する研究は多々報告されており，搭載されるアクチュエータモデルの不確かさに対する言及や (2)，ロータの受ける非線形な空気

力学的問題をロバスト制御理論により解決する手法 (1)，機体の特異姿勢回避のために，四元数による非線形フィードバックを利用することで，機体の構造的な不確実性や未知

∗ 芝浦工業大学大学院理工学研究科電気電子情報工学専攻〒108-8548 東京都港区芝浦 3-9-14Division of Electrical Engineering and Computer Science,Graduate School of Engineering, Shibaura Institute of Technol-ogy3-9-14, Shibaura, Minato-ku, Tokyo 108-8548, Japan

∗∗ 芝浦工業大学デザイン工学部デザイン工学科〒108-8548 東京都港区芝浦 3-9-14Dept. of Engineering and Design, Shibaura Institute of Tech-nology3-9-14, Shibaura, Minato-ku, Tokyo 108-8548, Japan

の外乱に対して解決する報告等がある (3)。他の研究報告で

は，機体の姿勢制御に関して唱えられているものが多く，機体に搭載されている個々のアクチュエータ等のハードウェアに関して精密制御を検討する報告は少ない印象を受ける。多くの論文では，アクチュエータは機体のダイナミクスの

一部として扱われ，システムの一部としての安定化が施されている。そこで，本研究ではマルチコプタ機の非線形ダイナミク

スを考慮しつつ，Lyapunov関数に基づいた Backstepping

法による非線形制御を適用することで姿勢制御の安定化を行う。更に，機体の推力系であるアクチュエータの制御を，Backstepping法により導出された擬似制御入力に基づき，

アクチュエータの極を考慮し，精密に制御行う。従来，機体の一部として安定化が施されるアクチュエータだが，本研究ではマルチコプタ機の安定化とアクチュエータの安定化を別々に考慮する。これは産業機器を制御する手法であ

り，機体に搭載されるアクチュエータを個々に制御することで，より精密な制御性を達成できる。アクチュエータの制御には，通信遅延，及び演算処理遅延を考慮し，むだ時間補償を行うことが可能な，極配置法の概念を拡張した限

定極配置法を利用する (4)。これまでにアクチュエータの極の影響が機体の制御系に与える影響を考慮する研究報告は数が少なかった。この問題に対して提案手法を用いて数値シミュレーションを行い，その影響と性能の評価を行う。

c© 2015 The Institute of Electrical Engineers of Japan. 827

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

Fig. 1. Body frame of Quadrotor.

2. マルチコプタ機の構造と物理モデル

〈2・1〉 マルチコプタ機の機構 4発ロータ型無人航空機では対角線上に位置するロータの推力偏差により生まれるモーメントを利用することで，水平移動が可能である。

垂直移動では，ロータより発する推力の総和により決定され，ロータが回転する際に発生する反トルクに関しては，機体に搭載される前後ロータが時計回りに，左右ロータが反時計回りに回転することで，ロータ間の反トルクを相殺し

ている。〈2・2〉 機体の運動方程式 Fig. 1 に示す 4 発ロータ

型無人航空機について考える。ここで，機体重心を機体座標系 ΣB, 及び地上の任意点に地上座標系 ΣE を設置する。Newton-Euler法に基づき，機体座標系各軸における力，及び軸周りのモーメントを考慮すると，制御対象の並進運動

方程式，及び回転運動方程式は (1)式から (6) 式で表される (5)。

φ =θψ

Ixx(Iyy − Izz) − Jr

IxxθΩr +

lIxx

U2 · · · · · · · · · · · · · (1)

θ =φψ

Iyy(Izz − Ixx) +

Jr

IyyφΩr +

lIyy

U3 · · · · · · · · · · · · · (2)

ψ =ψθ

Izz(Ixx − Iyy) +

1Izz

U4 · · · · · · · · · · · · · · · · · · · · · · (3)

x = −g sin θ + (sin φ sinψ + cos φ sin θ cosψ)1m

U1

· · · · · · · · · · · · · · · · · · · · (4)

y = g cos φ sin θ + (sin θ sinψ cos φ − sin φ cosψ)1m

U1

· · · · · · · · · · · · · · · · · · · · (5)

z = g cosφ cos θ + (cos φ cos θ)1m

U1 · · · · · · · · · · · · · (6)

ここで，φ，θ，ψは機体座標軸周りの回転角を表し，オイ

ラー角と同様の回転順序を取る。Ixx，Iyy，Izzは機体軸に働く慣性モーメント，Jr はロータの慣性モーメント，Ωr はロータの回転角速度偏差，lは重心からアクチュエータまでの距離，mは機体質量，gは重力加速度，U1 は機体座標系

において垂直方向に働く推力の合力，U2，U3，U4 はそれぞれ機体各軸に働くモーメントであり制御入力を表す。〈2・3〉 アクチュエータのモデリング 制御対象に搭

載されるアクチュエータの電気的基本式，及び機械系運動方程式，ロータに働く誘導抗力，形状抗力，粘性摩擦，を表す負荷トルクを (7)式から (10)式に示す (6)。

Vm = Rmim + Ladimdt+ KEωm · · · · · · · · · · · · · · · · · · · · (7)

τm = KT im · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (8)

τm = Jmdωm

dt+ Cmωm + τl · · · · · · · · · · · · · · · · · · · · · · (9)

τl = Jrdωr

dt+Crωr +

b4nρω2

mR4c{δ2+ aφt(θt − φt)

}

· · · · · · · · · · · · · · · · · · ·(10)

ここで，Vm は電圧，Rm は抵抗，im は電流，La は電機子インダクタンス，KE は逆起電力定数，ωm は回転角速度，ωr はロータの回転角速度，τm はトルク，τl はモータの軸に働く負荷トルク，KT はトルク定数，Jm は慣性モーメン

ト，Cmは粘性抵抗，Crはロータ粘性摩擦，Jrはロータ慣性モーメント，nはギア比，bはロータ枚数，ρは空気密度，R

はロータ半径，cは翼弦長，δは抗力係数，aは 2次元揚力傾斜，φt は翼端流入角，θt は翼端ピッチ角を表す。負荷ト

ルクを表す (10)式には非線形項が含まれているが，これはマクローリン展開を用いて線形化を施した。アクチュエータの回転角速度 ωm と電圧 Vm の関係を伝達関数で表すと，(7)式から (10)式より (11)式が求められる。

ωm(s)Vm(s)

=A

s2 + Bs + C· · · · · · · · · · · · · · · · · · · · · · · · · (11)

ここで，A = KT La/(Jm + Jr/n2), B = La(LaDm + LaKT Ke +

JaRa)/(Jm + Jr/n2)，C = RaLa(Dm + KT Ke)/(Jm + Jr/n2)とする。機体に搭載するアクチュエータは小型であり，低電力消費かつ電気的時定数が微小であるため，電圧を利用した回転角速度制御を行う (7)。

〈2・4〉 制御対象の推力系 ロータが発生する推力 T

とアクチュエータの回転角速度 ωm は翼素理論に基づき，(12)式で表される (8)。

T =b4ρaωm

2R3(θt + φt)c · · · · · · · · · · · · · · · · · · · · · · · (12)

ここで，本研究では固定ピッチロータを採用しているため，翼端ピッチ角 θt，翼端流入角 φt は定数とする。機体が垂直移動を行うために必要な推力は，各ロータから発生する推力総和 U1 により変化し，機体が水平移動を行うために必

要なモーメントは，各ロータから発生する推力偏差によって表され，ローリングモーメントU2，ピッチングモーメントU3，ヨーイングモーメントU4は，各ロータの推力を Tri

とすると，(13)式から (16)式の様に表される。ここで，K

は推力係数，Dは抗力定数を表している。

U1 = K(T 2r1+ T 2

r2+ T 2

r3+ T 2

r4) · · · · · · · · · · · · · · · · · ·(13)

U2 = K(T 2r4− T 2

r2) · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(14)

U3 = K(T 2r3− T 2

r1) · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(15)

U4 = D(T 2r1− T 2

r2+ T 2

r3− T 2

r4) · · · · · · · · · · · · · · · · · ·(16)

828 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

Fig. 2. Block diagram of the control system.

3. 制御系設計

〈3・1〉 Backstepping法 機体の姿勢制御をLyapunov

関数に基づく Backstepping法により行い，これにより得られた擬似制御入力を用いて機体の安定化を行う。機体の制

御系ブロック線図を Fig. 2に示す。機体運動方程式である(1)式から (6)式を安定化に必要なサブシステムと考え，計算過程の簡略化のため，下記の変数を導入する。これより(1)式から (6)式は (17)式で表されるサブシステムに書き

直され，擬似制御入力 U fi により安定化を施す。

X =

⎧⎪⎪⎨⎪⎪⎩X1 = φ X2 = φ X3 = θ X4 = θ X5 = ψ X6 = ψ

X7 = x X8 = x X9 = y X10 = y X11 = z X12 = z

⎫⎪⎪⎬⎪⎪⎭A1 = (Iyy − Izz)/Ixx, A2 = Jr/Ixx, A3 = (Izz − Ixx)/Iyy

A4 = Jr/Iyy, A5 = (Ixx − Iyy)/Izz

B1 = l/Ixx, B2 = l/Iyy, B3 = 1/Izz

ux = sin φ sinψ + cos φ sin θ cosψ

uy = sin θ sinψ cos φ − sinφ cosψ

X = f (X,U) =

⎛⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎝

X2

X4X6A1 − X4A2Ωr + B1U f2

X4

X2X6A3 + X2A4Ωr + B2U f3

X6

X4X2A5 + B3U f4

X8

−g sin X3 + uxU f1

mX10

g cos X1 sin X3 + uyU f1

mX12

g cos X1 cos X3 + cos X1 sin X3U f1

m

⎞⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎠· · · · · · · · · · · · · · · · · · ·(17)

次に，(17)式を漸近安定化するために，追従誤差Z1 = x1d−X1

を定義する。これに対する Lyapunov関数候補式は (18)式の様に表される。

V(Z1) =12

Z12 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (18)

(18)式に対する導関数を導くと (19)式の様に表される。

V(Z1) = Z1(x1d − X2) · · · · · · · · · · · · · · · · · · · · · · · · · · (19)

ここで，X2 は Z1 の安定化するための仮想制御入力でありX2 = X1を満たす。(19)式が負定であれば，Lyapunov関数候補 (18)式が導出される。X2 は (20)式の様に表される。

X2 = x1d + α1Z1 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (20)

ここで，α1(α1 > 0)は正定値を安定化するための重みである。(20)式に (19)式を代入すると，(21)式の様に書き直される。

V(Z1) = −α1Z12 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (21)

ここで，新たな変数 Z2 を導入する。Z2 は

Z2 = X2 − x1d − α1Z1 · · · · · · · · · · · · · · · · · · · · · · · · · · (22)

の様に表される。以上よりシステムを安定化するLyapunov

関数候補は (23)式の様に表される。

V(Z1, Z2) =12

(Z12 + Z2

2) · · · · · · · · · · · · · · · · · · · · · · (23)

(23)式の導関数を導くと，(24)式の様に表される。

V(Z1, Z2) = Z1Z1 + Z2(X2 − x1d − α1Z1)

= Z2(A1X4X6 + A2X4Ωr + B1U f2 )

−Z2(x1d − α1(Z2 + α1Z1))

−Z1Z2 − α1Z12 · · · · · · · · · · · · · · · · · · · · (24)

(24)式に対して擬似制御入力U f2 は，X1d = 0，V(Z1, Z2) < 0

を満たすと仮定すると，

U f2 =1B1

(Z1 − A1X4X6 − A2X4Ωr

−α1(Z2 + α1Z1) − α2Z2) · · · · · · · · · · · · · · · · (25)

ここで，α2(α2 > 0)は Z1を安定化するために導入した重み

である。U f1 ,U f3 ,U f4 に関しては U f2 の計算と同様の手順により導いた。これより (26)式から (28)式にその結果を示す。

U f1 =m

cos X1 cos X3(Z7 − g cos X1 cos X3

−α7(Z8 + α7Z7) − α8Z8) · · · · · · · · · · · · · · · · (26)

U f3 =1B2

(Z3 − α3X2X6 − α4X2Ωr

−α3(Z4 + α3Z3) − α4Z4) · · · · · · · · · · · · · · · · (27)

U f4 =1B3

(Z5 − α5X2X4

−α5(Z6 + α5Z5) − α6Z6) · · · · · · · · · · · · · · · · (28)

ここで，Z3 = X3d−X3，Z4 = X4− X3d−α3Z3，Z5 = X5d−X5，Z6 = X6−X5d−α5Z5，Z7 = X11d−X11，Z8 = X12−X11d−α7Z7

とする。並進運動方程式である (4)式，及び (5)式より，機

体軸 X，Y，Z 方向の運動は全て U f1 に依存しているが，X，Y 軸方向の運動では ux, uy の方向ベクトルを考慮する必要がある。方向ベクトルに関してBackstepping法により

Lyapunov関数，重みを定義し，システムの安定化を図る。

829 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

ux，uy の Lyapunov関数，及び重みの導出過程は，前述と同様であるため簡略化し，(29)式，及び (30)式に ux，uyの疑似制御入力を示す。

U fx =mU1

(Z9 + g sin X3 − α9(Z10 + α9Z9) − α10Z10)

· · · · · · · · · · · · · · · · · · ·(29)

U fy =mU1

(Z11 − g cos X1 sin X3

− α11(Z12 + α11Z11) − α12Z12) · · · · · · · · · · · ·(30)

ここで，Z9 = X7d−X7，Z10 = X8−X7d−α9Z9，Z11 = X9d−X9，Z12 = X10 − X9d − α11Z11 とする。〈3・2〉 アクチュエータ動作参照値推定 (25)式から

(28)式より求められた擬似制御入力により，ロータの回転角速度指令値Ωrdi を求めるため，以下の連立方程式を解く。

U f1 = K(Ω2rd1+ Ω2

rd2+ Ω2

rd3+ Ω2

rd4) · · · · · · · · · · · · ·(31)

U f2 = K(Ω2rd4−Ω2

rd2) · · · · · · · · · · · · · · · · · · · · · · · · · ·(32)

U f3 = K(Ω2rd3−Ω2

rd1) · · · · · · · · · · · · · · · · · · · · · · · · · ·(33)

U f4 = D(Ω2rd1−Ω2

rd2+ Ω2

rd3− Ω2

rd4) · · · · · · · · · · · · ·(34)

(31)式から (34)式に関して，Ωrdi について解くと，(35)式

から (38)式で表される。各アクチュエータに対する回転角速度指令を得る。

Ωrd1 =

√D(U f1 − 2U f3 ) − KU f4

2√

DK· · · · · · · · · · · · · · ·(35)

Ωrd2 = −√

D(U f1 − 2U f2 ) + KU f4

2√

DK· · · · · · · · · · · · · ·(36)

Ωrd3 =

√D(U f1 − 2U f3 ) − KU f4

2√

DK· · · · · · · · · · · · · · ·(37)

Ωrd4 = −√

D(U f1 + 2U f2 ) + KU f4

2√

DK· · · · · · · · · · · · · ·(38)

(31)式から (34)式の関係より，(35)から (38)の分子平方根内の値は必ず正となる。得られた回転角速度指令値Ωrdi は

機体姿勢を安定化する理想的な状態量とし，この指令値に対して，ロータの回転角速度が追従する様にアクチュエータの制御器を設計する。〈3・3〉 限定極配置法–アクチュエータ制御 擬似制御

入力により求められた回転角速度指令値に基づき，離散時間系でのアクチュエータ制御を行う。通信遅延，及び演算処理遅延が発生すると仮定し，制御系のサンプリングタイムを 10 msecとする。制御系内に 1サンプリングタイムの

むだ時間が存在するとし限定極配置法による固定構成制御器による制御系を設計する。これは，機体に搭載される計算機を用いて，アクチュエータの回転角速度制御を行うため，計算負荷の小さい制御器を設計するためであり，限定

極配置法を利用することで，制御器パラメータ設計の効率化が図れ，むだ時間要素による制御系への影響を明らかにすることができる。連続系制御ブロック線図を Fig. 3に示

し，これを離散表現に記述した離散系制御ブロック線図を

Fig. 3. PID angular velocity control system (Continu-ous system).

Fig. 4. PID angular velocity control system (Discretesystem).

Fig. 5. Digital control system for limited pole place-ment.

Fig. 4に示す。制御対象 P(z)はアクチュエータ (11)式をゼロ次ホールダを用いて離散化したものであり，制御器 K(z)

は PID制御器の微分要素を差分法，積分要素を台形積分法により離散化したものである。ここでフィードフォワード

制御に，逆起電力定数 KEを利用した。これはアクチュエータの制御を電圧による回転角速度制御を行っているため，アクチュエータ内に発生する逆起電力が制御系内に影響を

及ぼすことを低減するためである。PID制御器 K(z)は (39)

式の様に表され，制御パラメータ部，及び係数固定部に分割を行う。これは制御パラメータである Kp，Ki，Kd の導出に関与しない項を抜き出すことに等しい。制御対象であ

る (40)式のパラメータは変動しないため，(39)式のむだ時間 (1/z)を含む係数固定部と組み合わせ，係数固定部と制御パラメータ部の有理関数は，(41)式，及び (42)式の様に表される。

K(z) =Knp(z)

Kdp(z)× Kn f (z)

Kd f (z)

=

Kp

{z (z−1)+

Kd

T(z−1)2+Ki

T2

z (z+1)}

1× 1

z (z−1)· · · · · · · · · · · · · · · · · · ·(39)

P(z) =Pn(z)Pd(z)

=N1z + N0

(z − D1)(z − D2)· · · · · · · · · · · · · · ·(40)

n(z)d(z)=

Kn f (z)

Kd f (z)× 1

znm× Pn(z)

Pd(z)

=b1z + b0

z5 + a4z4 + a3z3 + a2z2· · · · · · · · · · · · · · · · ·(41)

β(z)α(z)

=Knp(z)

Kdp(z)=β2z2 + β1z + β0

α0· · · · · · · · · · · · · ·(42)

ここで，n(z)/d(z)は係数固定部の有理関数，β(z)/α(z)は

830 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

制御パラメータ部の有理関数，むだ時間は nm = 1とした。この際，2つの有理関数の関係を Fig. 5に示し，係数固定部とパラメータ部の閉ループ伝達関数を作成すると，(43)

式の様に表される。

G(z) =

β(z)α(z)× n(z)

d(z)

1 +β(z)α(z)× n(z)

d(z)

=β(z)n(z)

α(z)d(z) + β(z)n(z)=δ(z)γ(z)

· · · · · · · · · · · · · · · · · · ·(43)

限定極配置法では (43)式が所望の配置極を持つ様に α(z)，β(z)を定める。ここで，配置極の数を np，従属極の数を nq，配置極を p1, . . . , pnp，従属極を q1, . . . , qnq とすると，γ(z)

は (44)式の様に表される。配置極とは指定極を表し，従属

極とは自動的に決定する配置不可能な極を表す。

γ(z) = γnγznγ + γnγ−1znγ−1 + · · · + γ1z + γ0

= (Pnp znp + Pnp−1znp−1 + · · · + P1z + P0)

× (znq + Qnq−1znq−1 + · · · + Q1z + Q0)

= Pnp (z)(znp + Qnq (z)) · · · · · · · · · · · · · · · · · · · · · (44)

ここで，Pnp，及び Qnq はそれぞれ配置極，及び従属極を多項式に因数分解した際の係数であり (×)は乗算演算子を表

す。このとき，nq は (45)式により定めることが出来る。

nq = (nγ + 1) − (nα + 1 + nβ + 1)

= nγ − nα − nβ − 1 · · · · · · · · · · · · · · · · · · · · · · · · · ·(45)

ここで，nα,及び nβ は制御パラメータ部の分母次数，及び分子次数である。(45)式は特性多項式 γ(z)の係数個数から

制御パラメータ部の係数個数を減算したものを表している。以上より，(43)式の γ(z)は (46)式の様に考えられる。

γ(z) = α(z)d(z) + β(z)n(z) = Pnp (z)(znq + Qnq (z))

· · · · · · · · · · · · · · · · · · ·(46)

(46)式を変形すると (47)式で表される。

α(z)d(z) + β(z)n(z) − Qnq (z)Pnp (z) = Pnp (z)znq

· · · · · · · · · · · · · · · · · · ·(47)

(41)式，及び (42)式より係数固定部，及び制御パラメータ部の有理関数の分母，分子次数は nd = 5，nn = 2，nα = 0，nβ = 2であり，Fig. 4に示すむだ時間を持つ離散時間系の閉ループ極数は，nγ = 5から極数は 5個となる。これらを

(45)式に当てはめると，従属極の個数は nq = 2，配置極の個数は制御器パラメータの個数と同様の np = 3である。ここで，配置極を p1，p2，p3，従属極を q1，q2とする。(44)

式の特性多項式を利用すると，(48)式の様に表される。

γ(z) = Pnp (z)(znq + Qnq (z)

)

=(z3 + P2z2 + P1z + P0

) (z2 + Q1z + Q0

)

· · · · · · · · · · · · · · · · · · ·(48)

以上の結果から限定極配置法による極配置を行う。制御パラメータ部，及び従属極の係数ベクトルを ζ，配置極の係数ベクトルを μとすると (47)式が (49)式で表され，制御パラメータ部を導くことができる。(49)式を行列表現に直

すと (50)式となり，各行列成分は (51)式から (53)式で表される。

Pnp (z)znq = α(z)d(z) + β(z)n(z) − Qnq (z)Pnp (z) · · ·(49)

ζT = μT E−1 · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(50)

ζT =[α0 β0 β1 β2 Q0 Q1

]T · · · · · · · · ·(51)

μT =[

0 0 P0 P1 P2 1]T · · · · · · · · · · · ·(52)

E =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

0 0 a2 a3 a4 1

b0 b1 0 0 0 0

0 b0 b1 0 0 0

0 0 b0 b1 0 0

−P0 −P1 −P2 −1 0 0

0 −P0 −P1 −P2 −1 0

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

· · · · · ·(53)

(51)式から (53)式を (50)式に適用することで，制御パラ

メータ部を導出し，Kp，Ki，Kd を (42)式より求める。

4. Kalman Filterによる姿勢推定

未知の状態量を推定するために同一次元オブザーバを設計する。オブザーバの設計にはカルマンフィルタを利用す

るが，(17)式より，機体の系 f (X,U)は連成項を持つ事が分かる。つまり，変数 X に対して関数 f (X)は線形関係にないことを示している。そこで，制御対象の状態推定を行う手法としてUnscented Kalman Filter（以後：UKF）を用い

る (9)。航空機の状態推定には状態方程式を一次近似により線形化した Extended Kalman Filter（以降：EKF）が用いられることがあるが (10)，EKFは一次の微分項にある線形関係のみを抽出し，それに対してカルマンフィルタを適用する

ことで非線形性に対応しているため，二次以上の高次微分などを持つシステムや不連続点を含むシステムに対しては推定結果が不安定になることがある。そのため，本稿ではUKFを制御対象に適用した。UKFは EKFの様な線形近似

を行わず，状態変数の分布を数点の代表点と捉えることで，非線形モデルに基いて直接的に状態遷移後の分布を推定する手法 (11)であり，強い非線形性を持つ本稿の機体にも有効

である。UKFのデメリットとして挙げられる計算時間の長大化であるが，U変換を利用する UKFでは，EKFと同程度の計算量で推定を行えることが報告されており (12)，本稿でも上記条件から利用可能な推定手法だと考えられる。こ

れより UKFを用いた状態推定を行うが，EKFとの推定精度の比較を行うため，EKFに基づく同一次元オブザーバによる状態推定も同様に設計を行う。〈4・1〉 Extended Kalman Filter による姿勢推定　　観測出力からフィルタリングステップ kの状態 x(k)の姿勢推定を行う。機体の並進運動方程式，及び回転運動方程

式 (1)式から (6)式は (54)式の非線形状態空間モデルで表

831 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

される。

xk+1 = fk(xk, uk) + wk · · · · · · · · · · · · · · · · · · · · · · · · · ·(54)

yk = hk(xk) + vk · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(55)

ここで，xk = [φ, φ, θ, θ, ψ, ψ, x, x, y, y, z, z]T であり，wkはシステム雑音，vk は観測雑音，uk は制御入力，yk は観測値を表す。状態推定値の初期値 x0 は正規性確立ベクトルとし，ガウス性を持つとする。(54)式，及び (55)式を xkの推定値

より求まる次ステップの推定値 xk+1 = fk(xk, uk)近傍で，テイラー展開による線形近似化を行うと，(54)式，及び (55)

式は線形状態空間モデル (56)式，及び (57)式と表される。

(xk+1 − xk+1) = Φk + (xk − xk) + Γk(uk − uk) + vk

· · · · · · · · · · · · · · · · · · ·(56)

yk = hk+1(xk+1) + Hk(xk+1 − xk+1) + wk+1 · · · · · · · ·(57)

ここで，Φk =∂ fk∂x

∣∣∣∣xk ,uk

，Γ = ∂ fk∂u

∣∣∣∣xk ,uk

，Hk =∂hk+1∂x

∣∣∣∣xk+1

は非線

形状態空間モデルにおけるヤコビアン行列であり，ともに時変である。これらの式に対してカルマンフィルタの時間更新式，観測更新式を適用すると (13)，EKFのアルゴリズムは次の様に表される。

xk+1 = fk(xk, uk) · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(58)

Pk+1 = ΦkPkΦTk + ΓkQ

′kΓk + Qk · · · · · · · · · · · · · · · ·(59)

xk+1 = xk+1 + Kk+1(yk+1 − hk+1(xk+1)) · · · · · · · · · · ·(60)

Kk+1 = Pk+1HTk+1(Hk+1Pk+1HT

k+1 + σ2k+1)−1 · · · · · ·(61)

Pk+1 = (I − Kk+1Hk+1)Pk+1 · · · · · · · · · · · · · · · · · · · · ·(62)

ここで，xk+1 は事前状態推定値，Pk+1 は事前誤差共分散行列，Qk はプロセスノイズ，xk+1 は状態推定値，Kk+1 はカルマンゲイン，Pk+1は事後誤差共分散行列，σ2

k+1は確率変数に対する分散を表す。線形化された状態空間モデル (56)

式，及び (57)式を状態推定対象とし，導出された (58)式から (62)式を逐次的に繰り返すことで，対象の状態の推定を行う。〈4・2〉 Unscented Kalman Filter による姿勢推定　　機体の運動方程式を非線形状態空間モデルとし，(54)式，

及び (55)式の状態推定を行う。(54)式，及び (55)式の非線形状態空間モデルに対して，k 番目の参照点に対する機体姿勢推定値 xk,更に共分散 Pk を導出する UKFの逐次状態推定は次の様に記述される。〈4・2・1〉 シグマポイントの計算 機体姿勢推定値の

事後確率密度関数を 2n+1個のシグマポイントXk−1と，それぞれの項に対する重みWk−1で表す。シグマポイントは，

X0,k−1|k−1 = xk−1|k−1 · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(63)

Xi,k−1|k−1 = xk−1|k−1 +

(√(n +K)Pk−1|k−1

)i· · · · · ·(64)

Xi,k−1|k−1 = xk−1|k−1 −(√

(n +K)Pk−1|k−1

)i· · · · · ·(65)

ここで，(64)式は (i = 1, . . . , n)，(65)式は (i = n+1, . . . , 2n+

1)，xk|k は時刻 kにおける状態の事後推定値，Pk|k は状態の

事後共分散行列，K はシグマポイントに対する重みを調整するためのスケーリングパラメータを表す。式中の平方根は行列 (n +K)Pk−1|k−1 の行列平方根 Lを表し，

LT L = (n +K)Pk−1|k−1 · · · · · · · · · · · · · · · · · · · · · · · · · (66)

の i番目の行成分を表し，Cholesky分解を利用することで求め，シグマポイントに対する重みの与え方は，

Wm0 =

λ

n + λ· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(67)

Wc0 =

λ

n + λ+ 1 − α2 + β · · · · · · · · · · · · · · · · · · · · · ·(68)

Wmi =

12(n + λ)

, i � 0 · · · · · · · · · · · · · · · · · · · · · · · · ·(69)

ここで，(67)式から (69)式に示す添字 mは平均に対する重み，cは分散に対する重み，λは λ = α2(n +K) − nを満たし，α, βはシグマポイントの統計方法を決定づけるパラ

メータ，nは状態変数 xk の次数を表している。

〈4・2・2〉 時間更新–状態予測 先に求めた事後確率密度関数を表すシグマポイントを (54)式，及び (55)式に従い遷移させ，遷移後のシグマポイントの分布より，k 番目の

参照点に対する事前確率密度関数を計算し更新を行う。ここで，yは観測値の推定値を表している。

Xk|k−1 = f (Xk−1|k−1, uk−1, k) · · · · · · · · · · · · · · · · · · · ·(70)

xk|k−1 =

2n∑i=0

Wmi Xi,k|k−1 · · · · · · · · · · · · · · · · · · · · · · · ·(71)

Pk|k−1 =

2n∑i=0

Wci (Xi,k|k−1 − xk|k−1)

× (Xi,k|k−1 − xk|k−1)T + Q · · · · · · · · · · · · · · ·(72)

Yk|k−1 = h(Xk|k−1, uk−1, k) · · · · · · · · · · · · · · · · · · · · · ·(73)

yk|k−1 =

2n∑i=0

Wmi Yi,k|k−1 · · · · · · · · · · · · · · · · · · · · · · · ·(74)

ここで，Qはプロセス雑音を表している。

〈4・2・3〉 観測値更新 事前確率密度関数を k 番目の

参照点に対する予測値とセンサから得られる k番目の参照点の観測値との誤差を修正し，k 番目の参照点に関する事後確率密度関数を求める。以下，観測更新式を記述する。

Pyk yk =

2n∑i=0

Wci (Yi,k|k−1 − yk|k−1)

× (Yi,k|k−1 − yk|k−1)T + R · · · · · · · · · · · · · · ·(75)

Pxk yk =

2n∑i=0

Wci (Xi,k|k−1 − xk|k−1)

× (Yi,k|k−1 − yk|k−1)T · · · · · · · · · · · · · · · · · · ·(76)

Kk = Pxk yk P−1yk yk· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(77)

xk|k = xk|k−1 + Kk(y − yk | k−1) · · · · · · · · · · · · · · · · · · ·(78)

Pk | k = Pk | k−1 − KkPyk yk KTk · · · · · · · · · · · · · · · · · · ·(79)

832 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

ここで，Rは観測雑音を表している。以上より，事後推定値 xk|k，及び事後推定共分散 Pk|k を得られた。(63)式から(79)式までの計算過程を 1回とし，これを逐次的に繰り返すことで機体姿勢の状態推定を行う。これより，UKFの性

能の確認のため，次章では先に設計を行った EKFとの性能差についての検証を行う。

5. 数値シミュレーション

〈5・1〉 アクチュエータの回転角速度制御 先に設計を

行った限定極配置法の効果を確認するため，連続時間系に基づくむだ時間を考慮しない極配置法との性能比較を行う。極配置法については Fig. 3に示すブロック線図を基に設計を行った。目標値には 5-1-5多項式に基づく基本軌道関数

を用いた。目標値は，機体がホバリング状態であると仮定し，Ωrd = 220.0 [rad/s]とした。Fig. 6に極配置法と限定極配置法の結果を示す。解析を簡易化するため連続系極配置，及び限定極配置法に関して配置する極は重複極とする。シ

ミュレーションは Table 1の各制御パラメータを，Fig. 4に示す離散時間系モデルに適用したものであり（表中の E±4

の表記は 1.0× 10±4 を表す），サンプリングタイムは 10 ms

である。Table 1に示す，Kp，Ki，Kd は連続系極配置制御

器値，KLp，KLi，KLd は離散系限定極配置法制御器値を表す。配置極を速い位置に移動させるごとに，各配置法の応答が目標値に対して，収束性が向上するが，従来の極配置法では，徐々に応答が振動的になり，極が −30を越えると不

安定となった。限定極配置法では，極を速い位置に移動さ

(a) Poles assigned at 0.9910 (−20)

(b) Poles assigned at 0.9888 (−30)

(c) Poles assigned at 0.7970 (−900)

(d) Poles assigned at 0.7098 (−1300)

Fig. 6. Angular velocity response of the digital PID sys-tem.

せるごとに目標に対する収束性が向上するが，0.7098（極配置法 −1300相当）の極に配置した際に，制御系が不安定になった。この理由は次節で説明を行う。Table 1を確認すると，従来の極配置法よりも限定極配置法が全体的にパラ

メータが減少しており，各パラメータを確認すると，むだ時間による位相遅れの影響を微分要素で補っていることが確認できる。

〈5・2〉 極軌跡解析 Fig. 7に配置極と従属極の関係を示す。ここで，◦印は配置極，×印は従属極を表している。配置極を速い位置に移動すると，従属極が遅い位置に移動する事が分かる。0.7970（極配置法 −900相当）に配置極

を移動すると従属極と重なり，以降，更に速い位置に配置極を移動させると，0.7098（極配置法 −1300相当）では，極位置が入れ替わりシステムが不安定になった。極位置の

Table 1. Control parameters for discrete model and de-trmined poles.

- 0.9888 0.9866 0.7970 0.7099

- −20 −30 −900 −1300

Kp 19.474 44.318 7.1E+04 8.3E+04

Ki 3.626 10.090 400.0 433.333

Kd 0.073 0.030 8.3E-04 7.6E-04

KLp −0.354 −0.300 5.378 3.842

KLi −0.039 −0.151 2.920 −2.991

KLd 0.328 0.307 0.060 8.045

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)

(b) Poles assigned at 0.9888 (−30)

(c) Poles assigned at 0.7970 (−900)

(d) Poles assigned at 0.7098 (−1300)

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

(b) Pitch angle

(c) Yaw angle

Fig. 8. Simulation: Backstepping attitude controller.

入れ替わり前後の Table 1のパラメータを確認すると，パラメータが不安定化していることが分かる。これより制御対象の性能限界が，0.7970（極配置法 −900相当）付近に存在する事が分かる。しかし，配置極と従属極が入れ替わ

ることで，設計を行ったシステムが不安定化する事象が数学的に証明できていないことが今後の課題である。

〈5・3〉 機体の姿勢制御 機体の数学モデル，制御器

の有効性，及びUKFの効果を確認するため，機体の姿勢制御の検証を行う。機体の初期姿勢は，

Ed = [0.5235, 0.5235, 0.5235] · · · · · · · · · · · · · · · · (80)

とする。これは機体座標系から見て 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× I12×12，観測雑音 Pk = 2.0×102× I12×12，共分散σk = 100，UKFの事後推

定共分散の初期値 P0|0 = 1.0× 10−4 × I12×12，プロセス雑音Q = 1.0×10−4×I12×12，観測雑音R = 2.0×10−4×I12×12であり，Iは単位行列を表し，シグマポイントの決定パラメータα = 1.0×10−4，及び β = 2.0，λ = 2.0，次数 n = 12である。

UKFによる機体の姿勢角推定値はそれぞれ収束速度に差が発生しているが，各々 1秒以内に 0 [rad]に収束し安定状態にあることが確認できる。更に，UKFの推定値が実際のセ

ンサデータよりも真値に近いことから，UKFにより高精度

(a) Reference trajectory tracking (X-Y-Z)

(b) Reference trajectory tracking (X-Y)

Fig. 9. Simulation: Controller tracking an circularmovement

な姿勢推定が行えていることが分かる。EKFによる姿勢推

定では，初期姿勢の乱れが顕著に表れるものの，その後，安定化状態に至った。しかし，真値近辺で振動的値が検出される結果となった。ヤコビ行列に基づくEKFでは制御対象が持つ非線形特性を推定することが困難であることが分かる。

次に機体の位置制御系の検証を行う。機体の位置指令は半径2 mの円運動を指示した。アクチュエータの制御には限定極配置法のサーボ限界を，姿勢値はUKFの推定値を利用した。Backstepping法による重みは α7 = 10.22，α8 = 9.39，

α9 = 45.55，α10 = 32.84，α11 = 34.42，α12 = 25.73とした。シミュレーション結果を Fig.9に示す。ここで極配置法との性能比較を行う。極配置法のサーボ限界を利用し，限定極配置法と同条件で検証を行った。参照値に対して両

制御器の収束精度は高いが，限定極配置法はむだ時間を考慮した設計法であるため，位相遅れ補償を行えていることから，極配置法よりも精度の高い収束性を見せている。

6. 結 言

本稿では，4発ロータ型無人航空機の数学モデルに対して Backstepping法，及び限定極配置法によるアクチュエータの回転角速度制御を併用し，UKFによる状態推定値を用

いた姿勢安定化制御器の設計を行った。従来，機体のダイナミクスの一部として扱われ，個々に制御系が施されないアクチュエータの極が持つ影響を考慮することで，過去の

報告と比較し，安定かつ追従性の高い制御系を構成できる

834 IEEJ Trans. IA, Vol.135, No.8, 2015

極影響を考慮した無人機の姿勢制御（菅原康徳，他）

ことをシミュレーションにより確認を行った。限定極配置法は近年報告された極配置法，位相遅れ補償の手法であり，今後，更なる検証や解析が必要である。

文 献

（ 1） A.T. Gaitan and Y. Bolea: “Modeling and Robust Attitude Control Quadro-tor 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 fourrotors 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 atti-tude tracking controller design for quadrotor unmanned aerial vehicles usingquaternion”, Proc. IEEE/RSJ Conf. on IROS, pp.3051–3056, Tokyo, Japan(2013)

（ 4） Y. Urakawa: “Parameter Design for a Digital Control System with Calcu-lation 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 Tech-niques Applied to an Indoor Micro Quadrotor”, Proc. IEEE Conf. onRobotics 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: “Modelingand System Identification using Extended Kalman Filter for a QuadrotorSystem”, Proc. EEA Conf. on Applied Mechanics and Materials, Vol.313-314, pp.976–981, Bali, Indonesia (2013)

（11） S. Brunke and M. Campbell: “Estimation architecture for future autonomousvehicles”, 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)

菅 原 康 徳 （非会員） 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