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

座標変換を用いた剛体の姿勢制御
Rigid-Body Attitude Control with Coordinate Transformation〇大門誠 ∗ ，村松鋭一 ∗〇 Makoto Okado∗ ，Eichi Muramatsu∗*山形大学*Yamagata University
キーワード : 姿勢制御 (Attitude Control)，回転行列 (Rotation Matrix)，座標変換 (CoordineateTransformation)

連絡先 : 山形大学大学院 理工学研究科 機械システム工学専攻 村松研究室
村松鋭一，Tel.:0238-26-3327 ，E-mail: muramatu@yz.yamagata-u.ac.jp

1.

背景と目的あるベクトル v1 が，回転軸 a，回転角 θ で回

人工衛星の姿勢制御においては，3 次元空間における剛体の回転運動の制御が重要となる．こ転してベクトル v2 になった時，v1 と v2 の関係
は回転行列 R を用いて次のように表される．のような剛体の回転運動の方程式は非線形であRv1 = v2るため，非線形制御の問題となる．近年，このR = I + a× sin θ + (a× )2 (1 − cos θ)

3 次元空間における剛体の回転に特化した制御理論が発展している．

×a

本研究ではオイラー方程式で表される剛体の回転運動を，目標姿勢へと回転させる制御を考0=  a3−a2(1)(2)−a3 a2

0−a1 a10

また，回転軸 a は単位ベクトルとする．

察する．文献 1) では初期姿勢から目標姿勢へ到

剛体の姿勢制御系においては回転行列 R(t) を達させる制御方法が述べられているが，回転中剛体の姿勢を表すために次のように用いる．まの軌道は考慮されていない．本研究では，回転ず 3 次元空間において直交する xyz 軸を持つ静の軌道を考慮することにより，指定の回転方向止座標系を考える．また，剛体の重心を原点との制御方法を提案する．

して直交する x’y’z’ 軸を機体座標系として設定する．機体座標系の x’y’z’ 軸が静止座標系の xyz

2.

剛体の回転を表す運動方程式

2.1

回転行列軸を回転行列 R で回転させて得られるとき，剛体の姿勢を R として表す．
姿勢制御の方法として，回転行列のほかに，オ

本研究では剛体の姿勢表現に回転行列 R を用いる．

イラー角やクォータニオンなどを用いたものがある．それらには特異点や姿勢の表現が一通り–1–出ないなど問題点があるが，回転行列には特異て剛体に与える入力トルク u(t) を以下の式と点がなく姿勢の表現が一つに定まるという利点する．

がある．

2.2

u(t) = −Kv ω(t) − Kp Ωa (R(t))

剛体の運動方程式

ここで，Kv ，Kp はそれぞれ 3 行 3 列の正則な本研究で対象となる運動方程式を以下に示す．

J ω̇(t) = Jω(t) × ω(t) + u(t)ゲイン行列である．また，Ωa (R(t)) は以下の通りである．

(3)Ωa (R(t)) =×Ṙ(t) = R(t)ω (t)ただし，ω(t) = [ω1ω2

ω3 ]T は角速度ベクト0ω (t) =  ω3×−ω23∑

ai ei × (RdT Rei )(6)

i=1(4)ルであり，
(5)ただし ei は [e1 e2 e3 ] が単位行列となるような 3次元ベクトルである．

(6) 式は現在姿勢 R(t) が目標姿勢 Rd に一致

−ω3 ω20−ω1 ω10したときに 0 となる式である．このような制御系を用いてシミュレーションを行うと Fig.1 のようになる．とする．また，R(t) は回転行列，u(t) は剛体を回転させる入力トルク，J は慣性モーメントと慣性乗積からなる行列である．

3.

剛体の姿勢制御

3.1

初期姿勢と目標姿勢

初期姿勢 R0 と目標姿勢 Rd を与え，剛体の現在姿勢 R(t)，角速度 ω(t) をフィードバック
して目標姿勢で静止させる制御について考える．
初期姿勢 R0 ，目標姿勢 Rd はそれぞれ回転軸Fig. 1



a01
ad1




a0 =  a02  , ad =  ad2 a03
ad3

3.3と角度 θ0 ，θd を (2) 式の右辺の a と θ に与えた行列とする．

Example of attitude control

回転軸を考慮した制御

上で述べた制御則では回転の軌道を考慮していないため一つの回転軸で回転していない．本
研究では一つの軸で回転するような挙動を与え

3.2

目標姿勢で静止させる制御るため，座標変換を利用する方法を考案した．

剛体の姿勢 R(t) を目標姿勢 Rd へ収束させる制御として次の方法が知られている 1) ．フィー

3.4

ドバックによって得られる R(t) と ω(t) を用い座標変換を用いた制御(5)，(6) 式の制御則で制御する場合，初期姿
勢 R0 から目標姿勢 Rd までの回転を表す回転–2–行列を Re とする．この回転軸 ae が，機体座標系の X，Y，Z 軸のいずれかと一致している場

静止座標系での姿勢には以下のようにして戻すことができる．

合，ae が一つのか固定した回転軸となって回転R = Rf Rc運動が生じる．
そこで座標を変換してこの条件を満たすような形にする方法を考案した．

(15)

提案する方法によるシミュレーション結果を Fig.2に示す．Fig.1 と比べると，一つの回転軸で回転目標姿勢が静止座標系 (3 × 3 の単位行列) としており，軌道に無駄がないことが確認できる．

一致するものとする．目標姿勢を表す回転行列

Rd は単位行列となり，初期姿勢から目標姿勢まで一つの軸で回転させるような回転軸は a0 である．この a0 と機体座標系の X 軸が一致する
ように回転行列を Rc とする．Rc の回転軸を acと回転角を θc とする．それらは，
a01 + 1√
ac =  a02  ÷ (a01 + 1)2 + a202 + a203a03
(7)
θc = π(8)と表される．

Rc を用いた変換による初期姿勢 R0′ と目標姿
勢 Rd′ を作り，変換後の現在姿勢を Rf とする．

Fig. 2

Attitude control by proposed method

この R0′ ，Rd′ ，Rf と R0 ，Rd ，R の関係は以下の通りである．

4.

まとめ

R0′ = R0 RcT(9)Rd′ = Rd RcT(10)ドバック制御による初期姿勢から目標姿勢まで

Rf = RRcT(11)の回転運動を一つの軸で回転するような方法を

これらはそれぞれ，R0 ，Rd ，R から Rc までの回転を表す回転行列が，R0′ ，Rd′ ，Rf であることを示す．
変換後の R0′ を初期姿勢，Rd′ を目標姿勢とす

本研究では文献 1) の制御則をもとに，フィー考案した．一つの軸で回転運動させることにより，剛体に与えるトルクの方向が一つに定まるという利点がある．残されている課題としては平衡点の問題がある．

ると機体座標系の X 軸が a0 と一致する (証明は付録を参照)．ここで (6) 式を使い，制御を行う．

参考文献

制御則は次のようになる．

Ṙf (t) = Rf (t)ω × (t)u(t) = −Kv ω(t) − Kp Ωa (Rf (t))Ωa (Rf (t)) =3∑

ai ei × (Rd′T Rf ei )1) N. A. Chaturvedi, A. K. Sanyal, and N.
(12)

H. McClamroch: ”Rigid-Body AttitudeControl”,, IEEE CONTROL SYSTEMS

(13)
(14)

i=1–3–MAGAZINE, JUNE 2011

付録 座標変換の証明
R0′ と Rd′ のとき，機体座標系の X 軸が a0 と一致していることを証明する．
× 2
Rc = I + a×c Sθc + (ac ) (1 − Cθc )

(16)


Cθc + a2c1 (1 − Cθc )ac1 ac2 (1 − Cθc ) − ac3 Sθc ac1 ac3 (1 − Cθc ) + ac2 Sθc
Rc =  ac1 ac2 (1 − Cθc ) + ac3 Sθcac2 ac3 (1 − Cθc ) − ac1 Sθc  (17)Cθc + a2c2 (1 − Cθc )ac1 ac3 (1 − Cθc ) − ac2 Sθc ac2 ac3 (1 − Cθc ) + ac1 SθcCθc + a2c3 (1 − Cθc )ここで，Cθ = cos θSθ = sin θ

2Cθc + a2c1 (1 − Cθc ) = Cπ +  √a01 + 1(a01 )2 + a202 + a203

 (1 − Cπ)(18)a0 が単位ベクトルであり，a201 + a202 + a203 = 1 となるので整理すると，Cθc + a2c1 (1 − Cθc ) = a01(19)ac1 ac2 (1 − Cθc ) + ac3 sθc = a02(20)ac1 ac3 (a − Cθc ) − ac2 Sθc = a03(21)同様にとなり，結果 Rc は



a01

Rc =  a02a03

a02

a03

a202 −a01 −1a01 +1a02 a03
a01 +1a02 a03
a01 +12a03 −a01 −1a01 +1



(22)

次に，R0 は
× 2
R0 = I + a×0 Sθ0 + (a0 ) (1 − Cθ0 )Cθ0 + a201 (1 − Cθ0 )
R0 =  a01 a02 (1 − Cθ0 ) + a03 Sθ0a01 a03 (1 − Cθ0 ) − a02 Sθ0(23)a01 a02 (1 − Cθ0 ) − a03 Sθ0 a01 a03 (1 − Cθ0 ) + a02 Sθ0Cθ0 + a202 (1 − Cθ0 )a02 a03 (1 − Cθ0 ) − a01 Sθ0 
a02 a03 (1 − Cθ0 ) + a01 Sθ0Cθ0 + a203 (1 − Cθ0 )(24)となる．

R0′ を以下の通りとすると，



′′′
R011
R012
R013
′′′
R0′ =  R021
R022
R023
′′′
R031 R032 R033

(25)

姿勢が R0′ のとき，機体座標系の X 軸が回転軸 a0 と一致しているには，′R011= a01(26)′R021= a02(27)′R031
= a03

(28)

–4–となればよい．

R0′ = R0 RcT より，{}′
R011= Cθ0 + a201 (1 − Cθ0 ) a01 +{a01 a02 (1 − Cθ0 ) − a03 Sθ0 } a02 +{a01 a03 (1 − Cθ0 ) + a02 Sθ0 } a03′R011= a01 (1 − a201 − a202 − a203 )Cθ0 + a01 (a201 + a202 + a303 ){′R011= a01(29)}′= {a01 a02 (1 − Cθ0 ) + a03 Sθ0 } a01 + Cθ0 + a202 (1 − Cθ0 ) a02 +{a02 a03 (1 − Cθ0 ) − a01 Sθ0 } a03
R021′R021= a02 (1 − a201 − a202 − a203 )Cθ0 + a02 (a201 + a202 + a303 )′R021= a02(30){}′R031= {a01 a03 (1 − Cθ0 ) − a02 Sθ0 } a01 +{a02 a03 (1 − Cθ0 ) + a01 Sθ0 } a02 + Cθ0 + a203 (1 − Cθ0 ) a03′R031= a03 (1 − a201 − a202 − a203 )Cθ0 + a03 (a201 + a202 + a303 )′R031= a03(31)Rd′ に関しては，目標姿勢 Rd が単位行列となるので，Rd′ = Rd RcTa01

Rd′ =  a02a03

a02a03a202 −a01 −1a01 +1a02 a03
a01 +1a02 a03
a01 +12a03 −a01 −1a01 +1となり，機体座標系の X 軸と回転軸 a0 が一致する．

–5–(32)