ヘリコプタにおけるオブザーバを用いた速度推定
2010SE216 高田 將人 2010SE011 浅井大地 2010SE116 松田泰知
指導教員：陳 幹

1はじめに
Yaw axis

本研究で使用する２自由度ヘリコプタは, 不安定系であり, 非線形性の強いダイナミクスをもち, 他入力であることから, 線形化誤差, システム自身の特性変動によるモデル
Fy

パラメータの変動など、モデル化誤差が発生する. さらに,ry

多入力多出力システムは互いが干渉するので, 制御するのFpが 1 入力 1 出力系のシステムに比べて難しい. また, ヘリlcm

コプタは積荷の変化を模擬することで, 特性変動を表現す

Fg

る. 本研究では, ヘリコプタの重さを考慮するため, 信頼性

rppitch axisの高い上下界が得られるような, 不確かさをポリトープ表
現し最適レギュレータを拡張したロバスト H2 制御を構成

図 1 :2 自由度のヘリコプタの簡略図する. 速度推定をするため, 同一次元オブザーバを用いて制御設計を行う. 制御装置および種々の警報装置が開発され

表 1 ヘリコプター 物理パラメータている. ヘリコプタのあらゆる箇所にセンサを設置してすべての状態量を検知することは不可能に近く，コスト的にピッチ軸周りの等価粘性減衰係数も極めて不利といえる. そこで，センサは最小限に押さえ，ヨー軸周りの等価粘性減衰係数

センサで検知できない他の状態量を推定する技術が不可欠となる.[1] 同一次元オブザーバはセンサーを新しく付け加ヘリコプターの可動部の全質量ピッチモータの質量ヨーモータの質量える必要がないので, センサーの費用削減することに貢献プロペラ保護板の質量できる. この問題を解決するために近似誤差によって生じピッチ軸から重心までの長さる特性変動に対してロバスト安定性を保証できる制御器をピッチ軸周りの全慣性モーメント設計する. また, 機体に重りを載せることで生じる制御変動

ヨー軸周りの全慣性モーメント

Beq,p = 0.800
Beq,y = 0.318
mheli = 1.3872mm,p = 0.292mm,y = 0.128
mshield = 0.167
lcm = 0.0540
Jeq,p = 0.0384
Jeq,y = 0.0432

[N/V]
[N/V]
[kg]
[kg]
[kg]
[kg]
[m]
[kg ・ m2 ][kg ・ m2 ]に対してもロバスト安定性を保証する. これらの制御系設計を行うことで 安定した制御を実現する.

2

制御対象
本研究で制御対象として扱う２自由度ヘリコプタを図１に示す. これは２つのプロペラをもっており、それぞれDC モーターで駆動する. 前のプロペラはピッチ軸まわりの回転を起こし、ヘリコプター頭部の上下運動を制御する.
後ろのプロペラはヨー軸のまわりの回転を起こし, ヘリコプタ頭部の上下運動を制御する.
上下運動の角度 θ[rad] と, 左右運動の角度 ψ[rad] を測
定 し, 前 の プ ロ ペ ラ の 電 圧 Vm,p [V ],Vm,y [V ] を 操 作 して,θ[rad] と ψ[rad] を目標値に追従させる制御系を設計する. ここで図１のパラメータを示す.
rp :支柱からピッチモータまでの距離 [m]ry :支柱からヨーモータまでの距離 [m]lcm :支柱から重心までの距離 [m]Fp 上下方向に加わる推力 [N ]Fy 左右方向に加わる推力 [N ]Fg 重力 [N ]3モデリングθ[rad]:ピ ッ チ 角,ψ [rad]:ヨ ー 角,Kpp [Nm /V ]:ピ ッ チ
モ ー タ ー か ら ピ ッ チ 軸 に 働 く 推 進 ト ル ク 定 数,
Kyy [Nm /V ]:ヨーモーターからヨー軸に働く推進トルク
定数,Kpy [Nm /V ]:ヨーモータからピッチ軸に働く推進トルク定数, B[N/V ]:等価粘性減衰,K[N m/V ]:推進トルク定数,V [V ]:プロペラにかかる電圧,J[kgm2 ] :慣性モーメン
ト,m[kg]:ヘリコプタの質量,lcm [m]:ピッチ軸から重心までの距離
ピッチ軸から重心までの長さは下式で与えられる.

lcm =(mm,p + mshield )rp − (mm,y + mshield )ry
mm,p + mm,y + 2mshield(1)

二自由度ヘリコプターの質量中心位置 (xcm ,ycm ,zcm ) は以下の式で得られる.

xcm = lcm cosψcosθ(2)

ycm = −lcm sinψcosθ(3)

zcm = lcm sinθ(4)

これらを微分した式は次式になる.

ẋcm = −lcm (ψ̇sinψcosθ + θ̇cosψsinθ)(5)ẏcm = lcm (−ψ̇cosψcosθ + θ̇sinψsinθ)(6)żcm = lcm θ̇cosθ

(7)

2 自由度ヘリコプタでのオイラー・ラグランジュの運動方程式は以下のようになる.
()∂Ld ∂L−(19)Q1 =dt ∂ θ̇∂θ()d ∂L∂L
Q2 =−(20)dt ∂ ψ̇∂ψ

ピッチ側の回転運動エネルギー Tr,p , ヨー側の回転運動エは次式となる.

ネルギー Tr,y , 並進運動エネルギー Tt は次式になる.

ここで,L = T −V , 一般化座標 qi =(8)1Jeq,p ψ 22(9)122mheli (ẋ2cm + ẏcm+ żcm)
2(10)Tr,y =
Tt =[1Tr,p = Jeq,p θ̇22]Tθψθ̇]Tq1q2q3q4

式 (19),(20) の左辺を計算すると以ψ̇下のようになる.

2 自由度ヘリコプターの持つ位置エネルギー T , 運動エネ
ルギー V は次式になる.
V = mheli glcm=[(11)T = Tr,p + Tr,y + Tt
(12)
1= mheli (−lcm ψ̇ sin ψ cos θ − lcm θ̇ cos ψ sin θ)22
運動エネルギー T と位置エネルギー V を合わせると,L =T − V となる.
1mheli (−lcm ψ̇ cos ψ cos θ + lcm θ̇ sin ψ sin θ)22(13)+ (−lcm ψ̇ cos ψ cos θ + lcm θ̇ sin ψ sin θ)2L=

一般化座標は Q1 ,Q2 次式になる.

ddt(∂L∂ q̇1))∂L∂ θ̇
2= (jeq,p + mheli lcm)θ̈d=dt((21))∂L=∂ θ̇2ψ̇ 2 sin θ cos θ − mheli glcm cos θ= −mheli lcm(22))()(d ∂Ld ∂L=dt ∂ q̇2dt ∂ ψ̇∂L∂q1)((2= jeq,y ψ̈ + mheli lcm(ψ̈ cos2 θ − 2ψ̈ θ̈ sin θ cos θ)(23)()) (∂L∂L=0(24)=∂ q̇2∂ ψ̇

以上より, 以下の非線形運動方程式が得られる.
2)θ̈ = Kpp Vm,p + Kpy Vm,y − mheli glcm cosθ
(jeq,p + mheli lcm

Q1 = τp (Vm,p , Vm,y ) − Bp θ̇(14)

Q2 = τy (Vm,p , Vm,y ) − Bp ψ̇

(15)

(25)

モーターからピッチ軸, ヨー角に加わるトルクは次式に2(jeq,y + mheli lcmcosθ2 )ψ̈ = Kyy Vm,y + Kyp Vm,p − By ψ̇

2− Bp θ̇ − mheli lcmsin θ cos θψ̈

2+ 2mheli lcmsin θ cos θψ̇ θ̇なる.

(26)
τp (Vm,p , Vm,y ) = Kpp Vm,p + Kpy Vm,yτy (Vm,p , Vm,y ) = Kyp Vm,p + Kyy Vm,y(16)(17)

運動方程式に含まれている各慣性モーメントは以下のようになる.
２自由度ヘリコプタシステムの運動方程式を求める. 単純 ・ピッチ軸まわりの慣性モーメントな構造の力学系は, 力のつり合いによってその運動方程式を容易に導出することが出来るが, 複雑な構造であるとき,運動方程式の導出は困難である. そのため, 非線形運動方程Jeq,p = Jm,p + Jbody,p + Jp + Jy
・ヨー軸周りの慣性モーメント式をオイラー・ラグランジュ方程式を用いて導く.

Jeq,y = Jm,y + Jbody,y + Jp + Jy + Jshaf t

オイラー・ラグランジュの運動方程式は以下に示す.

Qi =ddt(∂T∂ q̇i)−

∂T
∂V
∂D++∂qi∂qi∂qi(18)

ここで, 運動エネルギー T, 位置エネルギー V, 損失エ
ネルギー D, 一般化座標 qi = [q1 ...qp ]T , 一般化力 Qi =[Q1 ...Qp ]T となる.

・それぞれの慣性モーメント

Jbody,p =mbody,pL2body

Jbody,y =mbody,yL2body

Jshaf t =mshaf t
L2body制御器設計

4

冒頭でも述べたロバスト H2 制御理論より制御器を設計

Jp = (mm,p + mshield )rp2

する. 制御分野でのロバスト性とは「プラントやコントローラなどの制御要素に摂動が生じる場合でも所望の制御

Jy = (mm,y + mshield )ry2効果を保証すること」をさす.[2] 本研究は, 行列ポリトープ
ピッチ軸, ヨー軸に加わるトルクを行列の形に変換すると表現を用いることによって, 機体のホバリングさせる入力次式になる.の変化と機体中心に搭載する重りの質量の変化に対してロτ = D(q)q̈ + C(q, q̇)q̇ + g(q)(27)

バスト性を保証する. また、目標値に定常偏差なく追従するため, 偏差の積分器を導入し, その拡大システム系を考え]
20jeq,p + mheli lcm
る.[3]D(q) =2cos θ20jeq,y + mheli lcm][2Bpmheli lcmsin θ cos θψ̇ 4.1 拡大システムの導出
C(q, q̇) =2By−2mheli glcm sin θ cos θψ̇
制御対象の状態方程式は式 (30) と[[mheli glcm cos θ0g(q) =[τ=Kpp Vm,y + Kpy Vm,yKyp Vm,p + Kyy Vm,y]∫ tu(t) :=]e(τ )dτ(35)0から与えられるとする. 観測出力 y(t) と目標値 r(t) の偏差を e(t) とする.

ここで, 線形化のために θ = 0 で以下のように近似する.

sinθ ≃ θe(t) = r(t) − y(t)(36)

拡大系の状態変数を,

cosθ ≃ 1xe (t) =[x(t)Tw(t)]T(37)

式 (25) と式 (26) は次式に近似できる.とすると, 拡大系の状態方程式は,2(jeq,p + mheli lcm)θ̈ = Kpp Vm,p + Kpy Vm,y(28)− mheli glcm − Bp θ̇2(jeq,y + mheli lcmcosθ2 )ψ̈ = Kyy Vm,y + Kyp Vm,p2− By ψ̇ + 2mheli lcmθψ̇ θ̇(29)状態ベクトルを x, 操作ベクトルを u とすると次式になる.

x=[θψu = [ Vm,pθ̇ψ̇]TVm,y ]

T

状態方程式, 出力方程式は次式になる.

ẋ(t) = Ax(t) + Bu(t)(30)y(t) = Cx(t)(31)

0 0 0 0A= 0 010− Jeq,p +mpheli l20100− Jeq,y +myheli l2Bcm0 0Bcm

00Kpp
B=− Jeq,p +mheli lcm
2K− Jeq,y +mypheli l2

cm[C=00K

cm− Jeq,p +mpyheli l2(32)00] [][ ]B
0=+u(t)+r01(38)と な る. こ の と き,y(t) が 目 標 値 r と な る よ う
な,x(t), u(t), w(t) の定常値 x∞ , u∞ , w∞ は,[ ] [][] [][ ]0A 0x∞B0=+u∞ +r0−C 0w∞01(39)を満たす. ここで,x̃(t) := x(t) − x∞ ,w̃(t) := w(t) −w∞ ,ũ(t) := u(t) − u∞ とすると, 式 (38),(39) より,ẋ(t)Tẇ(t)]4.2(34)A−C00][x(t)Tw(t)H2 制御

入力 u から出力 y の閉ループ系伝達関数を Gyu とする.Gyu の H2 ノルムは次式で定義される.

(||Gyu ||2 =][x̃˙ e (t) = Ae x̃e (t) + Be ũ(t)
(40)
[]
[]
[]
A 0Bx̃(t)Ae =Be =x̃e =−C 00w̃(t)を得る. この拡大系を安定させるために,H2 を利用して,状態フィードバック制御を行う.[3](33)K− Jeq,y +myyheli l2cm

1 0 00 1 0[12π∫ ∞−∞) 12

trace[G∗yu (jω)Gyu (jω)](41)

H2 制御は, システムにインパルスの入力 u が与えられたときの応答の H2 ノルムを最小にし, 外部入力から出力への影響を抑える制御と考えられる.H2 ノルムの上界を γ とするとZe||Gyu ||2 < γ(42)Weとなる.H2 制御では, 式 (41) を満たし, かつ γ を最小にすr(t)る制御器 K を設計する.[4][5]行列ポリトープ表現

4.3

1S+―○

WuZuy(t)

ki○++

P(s)
K

今回は, 機体の前のプロペラに搭載する重りの質量変化を行列ポリトープで表す. また, そのとき同時に変化する,機体の重心の位置の変化も行列ポリトープで表す.

mheli ∈ [mheli,min , mheli,max ] = [0, 30](43)lcm ∈ [lm,min , lm,max ] = [0.0540, 0.0595](44)

図 3 :一般化制御対象 G(s)図 2 に示すこの変動の端点について, 拡大系のシステム
行列 (Ae 0, Be 0),(Ae 1, Be 1),(Ae 2, Be 2),(Ae 3, Be 3) を求める重み, Wu は制御入力を制限するための重みをそれぞれる. このとき同時に,H2 ノルムを最小化する.u = Kx を求示している. G(s) の状態変数をめる. 式 (30), 式 (31) を一般制御対象とした不確かなシス

x = [xp z1 ]テムのロバスト H2 制御は以下の LMI 条件で成り立っている.

T(48)

T(49)とし, 評価出力をZ = [zu ze ]とする. 本研究で用いる一般化制御対象 G(s) は次式となる.

lcm[m]

0.05950.0540Ae1,Be 1・Ae3,Be3・Ae2,Be2・Ae0,Be030

0G(s) ={ẋ = Ae x + B1 r + Be uz = Ce x + De u[]
A 0
Ae =−C 0
 0 0  0 0  0 0 

B1 = 
 0 0 

1 00 1[]B
Be =0mheli[g] W

図 2 :行列ポリトープ表現

(51)

(52)

(53)0 0 0 0 0Weる.[6] 図 2 の不確かさの 2 つの端点に対して,LMI を連立

000000We000 Wa 000 Wb000 0000  00 
De =  00 Ww00Wu

して解くことで, 制御対象すべてに対して,H2 ノルムが γ

ただし, Wxj , Wr : 目標値に追従させるための偏差の積分未満となるようなコントローラを設計する.

に対する重み, We : θの重み, Wa : θ̇の重み, Wb : ψ の重み

X>0[(45)

Ae X + Be Y + Y T BeT + X T ATe
CX + DY

[γ2B

BT−X

XC T + Y T DT−I]
Ce = <0(46)]>0(47)

これらの条件式は ||G||2 < γ となる必要十分条件であxj000000Wr0000(50)(54)(55)

本研究で用いる, 一般制御対象 G(s) を図 3 に示す. 図 3, Wc : ψ̇ の重み, Ww , Wu : 制御入力に対する重みを適応しにおいて,We は目標値に追従させるための偏差積分に対すている。

4.4

H2 制御による制御系設計

Ze

一般化制御対象の評価出力 z を H2 ノルムで評価する.
重み Wxj , Wr , We , Wa , Wb , Wc , Ww , Wu をチューニングWeして,LM I を用いて状態フィードバックゲイン K, 偏差の積分に対するゲイン Ki を求める.

u(t)

1S+―○

Wxj = 1, Wr = 1

Wu

Zuy(t)

ki○++

P(s)
K

We = Wa = Wb = Wc = 1Wu = Ww = 0.5これらの重みを適用すると,LMI を用いて, 状態フィードオブザーバ

バックゲインは次のように求めらる.

[−15.7249
2.4494K=−1.6187−16.7034[ki =4.5

状態推定x(t)^−1.8565
0.0989−0.3474−2.8568

図 4 :オブザーバ(56)]

8.3001 1.2784−0.7881 8.1601]

(57)

4.6

オブザーバゲイン L を求めるために双対システムを考え

同一次元オブザーバーの設計

次に, 同一次元オブザーバを設計する. 速度推定するために Vθ ,Vψ を状態変数に加えると式 (61) のようになる.

x(t) =[θψθ̇ψ̇

双対システムの導出

VθVψ]Tる. 双対システムの状態変数を式 (61) とすると双対システ
ムの状態空間表現は式 (69)(70) のように表される.

(58)

オブザーバのコントローラを最適レギュレータ理論で設計する. 制御対象の状態空間表現は式 (62),(63) のようになる. ただし,L はオブザーバゲインである.[7] 制御対象にオẋv = Av xv + Bv uv(66)

yv = Cv xv(67)

Av = AT0(68)

ただしブザーバを付加させたものを図 4 に示す.

x̂˙ = A0 x̂ + B0 u + L(y − ŷ)(59)

Bv = C0T(69)ŷ˙ = C0 x̂

(60)

Cv = B0T(70)

ここで,双対システムの状態フィードバックゲイン形式のコント
2
a1 = Jeq,p + mheli lcm(61)2
a2 = Jeq,y + mheli lcm(62)

u = Kv xv(71)を設計して双対システムの状態フィードバックゲイン Kv

とおくと,ローラを最適レギュレータで求める. オブザーバゲイン L は式0 0
 0 0

 0 0
A0 =  0 0

0 00 010
B− a1p000
010
B− a2y00− a1py
K− a2yp000000 Kpp −B0 =  Ka1yp
 − a2
00[1 0 0C0 =0 1 000
K00


K− a1py Kyy − a2 

00




K
− a1py 
K− a2yy 
0000

0000

(75) で与えられる.[7][8]L = −KvT

(72)

(63)

5

シミュレーション・実験

ロバスト H2 制御が働いているかを確認するため重りを0[g],30[g] 乗せて実験を行い, 理論の検証を図:5,6 で行った.
次に同一次元オブザーバが設計できているかを確認する(64)

ために速度推定した値を利用せずに, 同一次元オブザーバとセンサーの値が一致しているか図:7 で確認した. そして,本来はセンサーからの情報で制御している値 θ̇,ψ̇ を同一次](65)

元オブザーバを用いて推定した. 同一次元オブザーバで速ˆ ˆ度推定した値 θ̇, ψ̇ を利用してヘリコプタを制御した結果を図:8, 図:9, 図:10 で示す.

図 5 ピッチ角の実験結果, 重り 0[g], 目標値 0[deg]

図 8 :同一次元オブザーバを用いたときのピッチ角の実験
結果

図 6 :ピッチ角の実験結果, 重り 30[g], 目標値 0[deg]˙

図 9 :θ̇,θ̂ の比較したグラフ

図 7 :ピッチ角のセンサーの値と推定値の比較結果, 重り0[g], 目標値 0[deg]6

おわりに
本研究では, 多入力多出力である, 不安定系の制御対象に関して, システムに対する同一次元オブザーバ併合型 H2制御を提案した. 実験を通して, ロバスト性の保証と速度推定の理論検証を行った. しかし, 推定値を利用せずに行った実験 (図:7) でピッチ角の推定値とセンサーの値に大きな差が出てしまっている. これは実験機とオブザーバの状態が一致していないことが原因の一つと考えられる.

7

参考文献

˙

図 10 :ψ̇,ψ̂ の比較したグラフ

スト安定化, 南山大学 情報理工学部,2013.

[3] 井上和夫, 川田昌克, 西岡勝博:MATLAB/Simulink によるわかりやすい制御工学. 森北出版, 東京,2010.
[4] 筧 菜帆:H2 制御理論によるレール上での鋼球の位置決め, 南山大学 数理情報学部,2012.
[5] 蛯 原 義 雄:LMI に よ る シ ス テ ム 制 御, 森 北 出 版, 東京,2012.
[6] 藤森篤:ロバスト制御, コロナ社, 東京,2008.
[7] 佐々木博志:外乱オブザーバを用いた３自由度クレーンの摩擦補償, 南山大学 数理情報学部,2013.

[1] 梅野孝治.「外乱オブザーバに基づく車両状態量推定」[8] 川田昌克:MATLAB/Simulink による現代制御入門
豊田中央研究所,RD レビュー VoL29,No,4,1994.12.
[2] 森川貴光:特性変動が生じる３自由度ヘリコプタのロバ