計測自動制御学会東北支部 第 329 回研究集会 (2020.10.30)
資料番号 329-1

座標系の切り替えを用いた剛体の姿勢制御
Rigid-body Attitude Control with Switched Coordinate Systems○熊谷竜太 ∗ ，村松鋭一 ∗∗○ Ryuta Kumagai∗ , Eiichi Muramatsu∗∗*山形大学*Yamagata University
キーワード : 剛体（Rigid-body），姿勢制御（Attitude control），回転行列 (Rotation matrix )，平衡点（equilibrium），連絡先 : 〒 992-8510 米沢市城南 4-3-16 山形大学大学院 理工学研究科 応用生命システム工学専攻
村松鋭一，Tel.: (0238)26-3327，E-mail: muramatu@yz.yamagata-u.ac.jp

1.

はじめに勢から所望する目標姿勢へ回転させる制御系設

宇宙空間における人工衛星や，回転翼によって空中を浮遊する飛行体，海中で動作するロボットなどにおいては，ボディがどの方向を向くか，すなわち姿勢の高精度な制御が必要とされる．
そのため，これらの姿勢制御の問題は３次元空間における剛体の回転運動の制御の問題として考えることができる．
文献 1) において，剛体の姿勢制御系について解析されている．その制御系の回転運動を行う
剛体の閉ループシステムにおいて，１つの安定計を考える．オイラー方程式と回転行列で表される剛体を制御対象とし，剛体の姿勢と角速度をコントローラにフィードバックする制御系を考える．前述した問題点に対し，座標系を切り替えることによって過渡特性を向上させ，目標姿勢に速く到達させることができるコントロー
ラの設計法を提案する．

2.

剛体の姿勢表現と運動方程式

2.1

回転行列な平衡点と３つの不安定な平衡点が存在する．
本研究では剛体の姿勢を表すために回転軸とこれらの局所的構造を解析すると，不安定な平衡点付近において安定な部分空間，不安定な部回転角を用いて表される回転行列 R を用いる．

分空間が存在し，従来の制御則を用いて制御を

3 次元空間における静止座標系に対し，回転す行った場合，不安定な平衡点の安定な部分空間る剛体に固定した機体座標系を考え，静止座標に近いほど所望する目的姿勢までの収束が遅く系の座標軸を回転軸ベクトル a のまわりに回転なる問題が生じる．

角 θ だけ回転させると機体座標系の座標軸に一本研究では，剛体の姿勢に対して数理モデル致するとき，剛体は回転行列 R で回転していに基づく制御系設計理論の考察を行い，初期姿–1–ると見なす．回転行列を以下のように示す．

R = I + a× sin(θ) + (a× )2 (1 − cos(θ))

メータ，ei (i = 1, 2, 3) は E =(1)[]e1 e2 e3が単位行列となるようなベクトルを表す．制御対象とフィードバックコントローラは R と ω

ただし，(1) 式の θ は回転角，a は回転軸のベに関して非線形であるため，制御において扱う

クトル，I は 3 × 3 の単位行列を表す．a× は交問題は非線形制御系の設計問題となる．

代行列であり，a =[]Ta1 a2 a3に対して

0−a3 a2
a× =  a30−a1 −a2 a10

3.2

閉ループシステムのダイナミクスと平衡点の導出

回転運動を行う剛体の状態量を (R, ω) とした
とき，コントローラ (4) 使用した閉ループシス

で表されるものとする．

テムのダイナミクスは2.2回転を表す運動方程式

J ω̇ = J ω × ω−K v ω − K p Ωa (R)回転行列 R，角速度ベクトル ω を用いて剛体Ṙ = Rω ×の運動方程式を以下に示す式で表現する．

J ω̇ = J ω × ω + u
Ṙ = Rω×(6)(7)(2)

で表される．ここで平衡点を求めるため，(6)，(3)(7) 式の右辺をゼロに等しくする．(7) 式より
Rω × = 0 になるため，R は可逆より ω = 0 を(2) 式の J は慣性テンソル，u は剛体に加える制御トルクを表す．姿勢安定化の目的として，所望する姿勢 Rd および角速度 ω = 0 によって得る．(6) 式に ω = 0 を代入すると Ωa (R) = 0になる．Ωa (Re ) = 0 とすると，(5) 式は下記の(8) 式で表すことができる．

与えられる所望の平衡点を漸近的に安定させる
フィードバックコントローラを設計することでa1 e1 × RTd Re e1 + a2 e2 × RTd Re e2ある．

+a3 e3 × RTd Re e3 = 0.

3.

ここで，RTd Re = [ri,j ]i,j∈1,2,3 である．(8) 式を剛体の制御系とその特性まとめると以下のようにあらわすことができる．

この章では文献 1) で述べられている制御系の a2 r32 = a3 r23∴
a3 r13 = a1 r31
 a r =a r
1 212 12特性についてまとめておく．

3.1

コントローラ制御においてフィードバックコントローラは

u = −K v ω − K p Ωa (R)Ωa (R) ≜3∑

ai ei × (RTd Rei )

(8)

(9)

a1 , a2 , a3 > 0 であることから，(9) 式よりRTd Re は以下のように表すことができる．

(4)
(5)

i=1を使用する．ただし，(4)，(5) 式における K v ，K p はゲイン行列，ai (i = 1, 2, 3) は制御パラ–2–

r11
 a2T
Rd Re =  a1 r12a3a1 r13
r12 r13
r22 r23 a3a2 r23 r33(10)直交行列の性質 (RTd Re )(RTd Re )T = (RTd Re )Tと，r11 , r22 , r33 はそれぞれ+1 または-1 の値を(RTd Re ) = I を用いると，(10) は以下のようにとることになる．以上のことから，下記のようまとめることができる．に定義することができる．

a22 2a23 2(1−)r+(1−)r = 0
a21 12a21 13
a2 2a2 2∴− (1 − 32 )r23=0(1 − 22 )r12

a1a2


22

 (1 − a3 )r2 + (1 − a3 )r2 = 0
13a21a22 23

Proposition1(11)(12)(Re , 0) は以下に示す (14) 式を満たすような，閉ループ姿勢の運動方程式 (6)-(7) における平衡点である．

(13)

RTd Re ∈ S ≜a1 , a2 および a3 は異なる値をとるため，(11)-[[[(13) 式の解は以下の a),b) のいずれかを満たす．

[a) r12 = r13 = r23 = 0
ri,j (i ̸= j) ̸= 0b)]e1 e2 e3,

e1 −e2 −e3−e1 −e2 e3−e1 e2 −e3]]],(14),,つまり，平衡点 (Re , 0) ははじめに a) について，(11)-(13) 式の自明なξ ≜ {(Rd , 0), (Rα , 0), (Rβ , 0), (Rγ , 0), } (15)

解であることに注意しながら考える．r12 ̸= 0 とすると，a1 , a2 および a3 は異なる値をとるため，に対して (Re , 0) ∈ ξ となるものであり，ここでa2a2a2(1 − a22 ) ̸= 0, (1 − a32 ) ̸= 0, (1 − a23 ) ̸= 0 という112

姿勢 Rα , Rβ , Rγ はことになる．また r12 ̸= 0 であることから，(11)Rα ≜ Rd

式は r13 ̸= 0 をもたらし，(12) 式からは r23 ̸= 0が生じる．同様の議論が r13 ̸= 0 および r23 ̸= 0Rβ ≜ Rdの場合にも成り立つ．したがって，(11)-(13) 式

Rγ ≜ Rd[[[e1 −e2 −e3−e1 −e2 e3−e1 e2 −e3]]],,,のすべての解は a)，b) のいずれかを満たす．
次に b) の場合について考える．a1 > a2 とすると，r12 ̸= 0 より (1 −a2a22a21Proposition 1 は，閉ループシステムは 4 つ2 > 0 であるため，)r122 < 0 をもたらす．r 2 は正(11) 式は (1 − a23 )r13131の値より a3 > a1 となるため，a3 > a1 > a2 となる．この不等式は以下のことを表す．

a2 2(1 − 32 )r13< 0,a1である．の異なる平衡点をもつことを意味する．これらの平衡点のうちの一つは，姿勢 Rd によって特定される所望の平衡点であり，一方で残りの平衡点は姿勢 Rα , Rβ および Rγ によって指定さa2 2(1 − 23 )r23<0a2れる．これらの姿勢 Rα , Rβ , Rγ は，剛体のボディに固定された 3 つの座標軸の周りをそれぞしたがって，(12) 式の左辺は負の値となり矛盾が生じる．これと同様の議論は a1 < a2 についれ 180 度回転した点であり，所望の姿勢 Rd とは異なる．て，(11)-(13) 式が b) の場合に対して矛盾を生じさせることを示している．したがって，a1 , a2 , a3は異なる値をとることから，b) の場合は矛盾を3.3

非線形方程式の線形化生じるため，(11)-(13) 式に対する唯一の解は a)

次に，所望の平衡点と残りの 3 つの平衡点のその場合，すなわち r12 = r13 = r23 = 0 である．

れぞれの安定性を解析し，これらの平衡点付近

また，(10) 式に r12 = r13 = r23 = 0 を代入する

における閉ループベクトル場の局所的構造を解–3–析する．各線形化は R6 で展開される．閉ループ化が以下に示す線形化方程式で得られる．

システムにおけるダイナミクスの局所的構造を¨ + K v ∆Θ̇ + K∆Θ = 0J ∆Θ解析するうえで，各平衡点についての閉ループ(21)方程式を線形化する必要がある．(Re , 0) ∈ ξ を

ここで，定数行列 K は各平衡姿勢に応じて次式

閉ループシステム (6)-(7) の平衡解とする．摂動で与えられる．
 K p diag(a2 + a3 , a1 + a3 , a1 + a2 ), −K diag(a + a , a − a , a − a ),p2
3 13 12(22)K=−Kdiag(a−a,a−a,a+a),p323112−K p diag(a2 − a3 , a1 + a3 , a2 − a1 ),パラメータ ε ∈ R に関して，平衡点 (Re , 0) ∈ ξの摂動を考える．
×

初期姿勢の摂動を R(0, ε) = Re eεΘ0 とする．
ここで，RTd Re ∈ S は (14) 式で与えられ，Θ0 ∈R3 である．また，初期角速度は ω(0, ε) = εω 0とし，ω 0∈ R3 である．

ただし，(22) 式における K は上から Re = Rd ，

Re = Rα ，Re = Rβ ，Re = Rγ の条件に対応するものとする．したがって，４つの平衡点付近における局所的なダイナミクスは，正の減衰をJ ω̇(t, ε) = Jω(t, ε) × ω(t, ε)持つ線形二次システムによって与えられる．そ−K v ω(t, ε)−K p Ωa (R(t, ε))Ṙ(t, ε) = R(t, ε)ω(t, ε)×の剛性は各平衡点に依存する．所望の平衡点に(16)近づくと剛性は正定値を示す．すなわち所望の(17)平衡点付近における局所的なダイナミクスは局

次に，線形化された状態量 ∆ω, ∆Θ ∈ R3 を∆ω(t) ≜ ω ε (t, 0), ∆Θ(t)× ≜ RTe Rε (t, 0) として定義する．それによって，以下の式が得られる．

所的指数関数的に安定となる．姿勢 Rα , Rβ および Rγ に対応する３つの平衡点付近においては，それらの剛性行列は少なくとも一つの負の固有値を持ち，その結果不安定となり鞍点を生じる．したがって，３つの各平衡点付近におけ∆Θ̇ = ∆ω(18)に，これらの平衡点の固有値はゼロではないこ˙J ∆ω= −K v ∆ω−K p Ωa (Re ∆Θ× )る閉ループダイナミクスは不安定となる．さらとにより，次のことが言える．

(19)

Proposition2×

ここで，Ωa (Re ∆Θ ) は×

Ωa (Re ∆Θ ) =閉ループシステム (6)-(7) における所望の平衡a1 e1 × (RTd Re ∆Θ× e1 )

点 (Rd , 0) は局所指数関数的収束で漸近的に安定+a2 e2 × (RTd Re ∆Θ× e2 )である．平衡点 (Rα , 0), (Rβ , 0) および (Rγ , 0)+a3 e3 × (RTd Re ∆Θ× e3 )は双曲型関数的に不安定である．

×T= −(a1 e×1 R d R e e1

3.4×
T+a2 e×2 R d R e e2×T+a3 e×
3 Rd Re e3 )∆Θ (20)[であり，RTd Re は Proposition 1 における (14)式で与えられる S に属する．

固有値と固有ベクトル(21) 式より][]] [∆Θ∆Θ̇0I=∆Θ̇∆Θ̈−J −1 K −J −1 K v(23)(19)-(20) 式を組み合わせると，(15) 式の各平と表現できる．例として，姿勢 Rγ に対応する不衡点について閉ループシステム (6)-(7) 式の線形

安定な平衡点のおける線形方程式の固有値を調–4–べると，正の固有値と負の固有値がある．この180 度に近い場合に過渡期間の増加が発生する．

ようにして，不安定平衡点の近くにおいて，安これは，解が長期間にわたって不安定な平衡点定な部分空間と不安定な部分空間が存在するこ(Rγ , 0) に近いままであることを示している．ことを確認した．の動作は，初期状態が所望の平衡点 (Rd , 0) の不安定平衡点の近くに初期状態がある場合を考える．安定な固有値に対応する固有ベクトル吸力領域に近いほど平衡点 (Rd , 0) に収束するのに時間がかかることを示している．の線形結合 va = [∆Θa , ∆ω a ] および不安定な固有値に対応する固有ベクトルの線形結合 vb =[∆Θb , ∆ω b ] を以下に示す．

4.

座標系の切り替えを用いた制御系設計



0.40210.5939∆Θa =  0.3037  , ∆ω a =  −0.9528  ,0.2993−0.95424.1

制御方法の提案

前章では，所望する平衡点 (Rd , 0) の他に３つの平衡点 (Rα , 0), (Rβ , 0), (Rγ , 0) の存在を確




00



∆Θb =  0.8432  , ∆ω b =  0.5375  ,0.9827
0.1849認した．平衡点 (Rα , 0), (Rβ , 0), (Rγ , 0) の局所

これらを用いた初期条件を以下に示す．

安定な平衡点付近からの制御において，安定な的な構造の解析により，これらの平衡点の安定性は双曲型関数的に不安定であったため，安定な部分空間と不安定な部分空間が存在する．不部分空間に近いほど過渡期間が増加し，所望のR(0) = diag(−1, 1, −1)×exp(δ∆Θ×a + ε∆Θb ),ω(0) = δ∆ω a + ε∆ω b ,平衡点 (Rd , 0) までの収束時間が長くなる問題(24)を確認した．

(25)

上記の初期条件において，0 < δ ≪ 1 に設定して制御を行う．制御結果を Fig.1 に示す．

本研究ではこの問題に対し，初期姿勢が不安定平衡点における安定な部分空間から大きく離れるように角度を調整した座標系を作成する．
作成した座標系と従来の座標系をつなぎ合わせて，座標系の切り替えを可能とする新たな座標系を提案する．これにより制御の有効性を検討する．

4.2

座標系切り替えの手順はじめに，座標系について説明する．提案する座標系を Fig.2 に示す．A 座標系は目標姿勢

Fig. 1

姿勢角の偏差が単位行列となるような座標系，B 座標系は A

Fig.1 において，本研究では，初期条件 (22)，(23) に δ = 10−4 ，ε = {10−2 , 10−3 , 10−4 , 10−5 }を設定し制御を行った場合の姿勢誤差をプロットした．Fig.1 で示すように，ε の値が小さく座標系を回転行列 RAB によって回転させた座標系とする．それぞれの座標系における剛体の姿勢を表す回転行列を RA , RB とする．B 座標系では B 座標系における単位行列 I B を目標姿なると収束時間が長くなる. さらに姿勢誤差が–5–勢とする制御を行い，A 座標系では目標姿勢 Rdまで剛体を回転させる制御を行う．

Fig. 2

提案する座標系

Fig. 3姿勢角の偏差Fig.2 に示す座標系を用いる制御の手順を以下に示す．

5.

総括1) はじめに，B 座標系で制御を行う．現時点

従来の制御方法で制御を行った場合，３つのでは B 座標系において回転行列 RB を用

不安定平衡点における安定な部分空間からの制いて目標姿勢 I B へ姿勢を到達させる制御

御は所望する平衡点までの収束時間が長くなるを行う．という問題点が確認できた．これに対し，座標2) 剛体が初期姿勢から 90 度回転した時点で系を切り替えて制御を行うことにより収束時間B 座標系から A 座標系へと切り替えを行が長くなる問題点を改善した．異なる座標系をう．座標系の切り替えは RA = RAB RB用いることによって，制御開始地点が安定な部を用いて行う．この操作により，最終的な分空間付近ではなくなったため，制御の立ち上

目標姿勢も A 座標系における Rd に切りがりが遅くならずに素早く目標姿勢に到達した．

替わる．

すなわち，安定な部分空間付近から離れた地点からの全ての制御においてこのことが成り立つ

3) A 座標系において回転行列 RA を用いてと考えられる．

目標姿勢 Rd へと姿勢を到達させる．

今後の展望として，B 座標系に対して合理的

上記の制御における制御結果を Fig.3 に示す．
ただし，初期条件 (24)，(25) に設定する δ, ε については，δ は前回の制御と同様に δ = 10−4 を設な姿勢角の偏差の設定や用いる座標系の個数を変更することにより，コントローラの有効性を評価する．

定するが，ε は Fig.1 にて最も収束時間が長かった ε = 10−5 を設定して制御を行う．また，従来参考文献の制御法を用いた制御結果を破線で示し，提案する制御法を用いた制御結果を実線で示す．

Fig.3 より，提案する手法によって過渡応答が改善され，収束時間を短縮できることを確認した．

–6–[1] N. A. Chaturvedi, A. K. Sanyal, and N. H.
McClamroch：”Rigid-Body Attitude Control,
IEEE Control System Magazine, June, 2011.
[2] 長谷川律雄：回転の表現，計測と制御，Vol.41,
No.9, 2002.