1.はじめに
目次
1.1 結論
並進運動
$\begin{pmatrix} F_x \\ F_y \\ F_z \end{pmatrix}=
m\begin{pmatrix} \dot{u}+qw-rv \\ \dot{v}+ru-pw \\ \dot{w}+pv-qu \end{pmatrix}$
回転運動
$\begin{pmatrix} \bar{L} \\ \bar{M} \\ \bar{N} \end{pmatrix}=
\begin{pmatrix}
I_x \dot{p}-(I_y-I_z)qr-I_{xy}(rp-\dot{q})-I_{yz}(q^2-r^2)-I_{zx}(\dot{r}+pq) \\
I_y \dot{q}-(I_z-I_x)rp-I_{xy}(qr-\dot{p})-I_{yz}(\dot{r}+pq)-I_{zx}(r^2-p^2) \\
I_z \dot{r}-(I_x-I_y)pq-I_{xy}(p^2-q^2)-I_{yz}(\dot{q}+rp)-I_{zx}(\dot{p}-qr)
\end{pmatrix}
$
1.2 使用する記号等
機体固定座標系
x軸：機首方向 y軸：右翼方向 z軸：機体の腹方向 の右手系
重心：C
重心：C
地面固定座標 $x_e$ 軸 , $y_e$ 軸 , $z_e$ 軸 earth
原点：O
オイラー角 $(\phi ,\theta ,\psi)$
$\phi$ ： $x_e$ 軸周りの角度 roll角
$\theta$ ： $y_e$ 軸周りの角度 pitch角
$\psi$ ： $z_e$ 軸周りの角度 yaw角
オイラー角 $(\phi ,\theta ,\psi)$
$\phi$ ： $x_e$ 軸周りの角度 roll角
$\theta$ ： $y_e$ 軸周りの角度 pitch角
$\psi$ ： $z_e$ 軸周りの角度 yaw角
機体の質量 $m$
機体の一部の微小質量 $∆ m$
機体の一部の微小質量 $∆ m$
重心の位置ベクトル $r_c$
微小質量の位置ベクトル $r_m$
微小質量の重心からの位置ベクトル $r$
微小質量の位置ベクトル $r_m$
微小質量の重心からの位置ベクトル $r$
速度 $V= (u ,v ,w)$
角速度（機体の重心C周りの回転ベクトル） $\omega = (p ,q ,r)$
全外力 $F= (X ,Y ,Z)$
モーメント $(L ,M ,N)$
角運動量 $H= (H_x ,H_y ,H_z )$
角速度（機体の重心C周りの回転ベクトル） $\omega = (p ,q ,r)$
全外力 $F= (X ,Y ,Z)$
モーメント $(L ,M ,N)$
角運動量 $H= (H_x ,H_y ,H_z )$
x軸,y軸,z軸に関する慣性能率 $(I_{xx} ,I_{yy} ,I_{zz} )$
x-y面,y-z面,z-x面に関する慣性乗積 $(I_{xy} ,I_{yz} ,I_{zx} )$
x-y面,y-z面,z-x面に関する慣性乗積 $(I_{xy} ,I_{yz} ,I_{zx} )$
迎角 $\alpha$：$\tan \alpha = \dfrac{w}{u}$
横滑り角 $\beta$：$\sin \beta =\dfrac{v}{V}$
飛行経路角 $\gamma$：$\gamma=\theta-\alpha$
上反角 $\Gamma$：$\Gamma=\dfrac{-z_e'}{ \sqrt{ {x_e'}^2 + {y_e'}^2 } }$
舵角 $(\delta_a ,\delta_e ,\delta_r )$ : エルロン角、エレベータ角、ラダー角
横滑り角 $\beta$：$\sin \beta =\dfrac{v}{V}$
飛行経路角 $\gamma$：$\gamma=\theta-\alpha$
上反角 $\Gamma$：$\Gamma=\dfrac{-z_e'}{ \sqrt{ {x_e'}^2 + {y_e'}^2 } }$
舵角 $(\delta_a ,\delta_e ,\delta_r )$ : エルロン角、エレベータ角、ラダー角
$u',v',w'=f( u ,v ,w, p ,q ,r, \theta ,\phi,\psi , m ,T ,\rho ,V ,S , C_x ,C_y ,C_z )$
$p',q',r'=f( p ,q ,r, I_x ,I_y ,I_z ,I_{xz} , ρ ,V ,S ,c ,b , C_l ,C_m ,C_n )$
$p',q',r'=f( p ,q ,r, I_x ,I_y ,I_z ,I_{xz} , ρ ,V ,S ,c ,b , C_l ,C_m ,C_n )$
2. 航空機の非線形運動方程式
目次
- 2.1 ベクトルの微分
- 2.2 基底ベクトルと座標成分ベクトル
- 2.3 機体の回転運動（姿勢運動）の方程式
- 2.4 機体重心の運動方程式
- 2.5 座標系の種類と座標成分の変換
- 2.6 慣性行列（テンソル）の座標変換
- 2.7 機体姿勢のオイラー角表現と速度・角速度の座標
- 2.8 重力と慣性乗積の影響
- 2.9 ジャイロ連成と推力によるモーメント
2.1 ベクトルの微分
1.慣性座標系と動座標系
慣性座標系は、慣性空間上に固定された座標系であり「$O_I-X_I,Y_I,Z_I$」と表記される。
動座標系は、ある剛体の重心に原点を置き、剛体に固定されて剛体とともに回転する座標系であり「$O-X,Y,Z$」と表記される。
以下のような3つの位置ベクトルを考える。
- $\vec{r_c}$: 慣性座標系の原点から見た剛体の重心の位置ベクトル
- $\vec{r}$: 剛体上のある質点$p$の動座標系の原点から見た位置ベクトル
- $\vec{R}$: 慣性座標系の原点から見た質点$p$の位置ベクトル
この時、位置ベクトル$\vec{R}$は、以下の関係にある。
$\vec{R}=\vec{r_c}+\vec{r}$
2.動座標系の単位ベクトル
質点$p$の動座標系におけるX,Y,Z軸の座標成分をx,y,zとし、X,Y,Z軸方向の単位ベクトルを$\vec{i},\vec{j},\vec{k}$とする。
位置ベクトル$\vec{r}$は、以下のように表す事ができる。
$\vec{r}=x\vec{i}+y\vec{j}+z\vec{k}$
ここで、$\vec{i},\vec{j},\vec{k}$は、右手直交系をなし、動座標系に固定されて動座標系の回転と共に回転するものとする。
3.回転座標系の時間微分の仕方
通常、固定座標系においては、
$x_e$を微分すると、$\dfrac{dx_e}{dt}$ となるが、
角速度 $\omega$ で回転する回転座標系を基準に考える場合、
$x_e$を微分すると、$\dfrac{dx_e}{dt}=\dfrac{dx}{dt}+ \omega \times x$ となる。
$x_e$を微分すると、$\dfrac{dx_e}{dt}$ となるが、
角速度 $\omega$ で回転する回転座標系を基準に考える場合、
$x_e$を微分すると、$\dfrac{dx_e}{dt}=\dfrac{dx}{dt}+ \omega \times x$ となる。
動座標系の質点p(位置ベクトル$\vec{r}$)の微分（速度）は、以下のように表される。
$\vec{v}=\frac{d\vec{r}}{dt}=\frac{\partial \vec{r}}{\partial t}+\omega\times\vec{r}$
上記の式は、速度だけでなく動座標系の幾何ベクトルを時間微分する時に利用できる。
2.2 基底ベクトルと座標成分ベクトル
機体重心の速度ベクトル$\vec{v_c}$と機体の角速度ベクトル$\vec{\omega}$
基底$\vec{U}$は、以下のような単位ベクトルの組である。
基底: $\vec{U}=[\vec{i}, \vec{j}, \vec{k}]$
U,V,Zを動座標系のX,Y,Z軸方向の速度成分とすると、機体重心の速度ベクトル$\vec{v_c}$は、
基底$\vec{U}$を使用して以下のように表現できる。
$\vec{v_c}=U\vec{i}+V\vec{j}+W\vec{k}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
U\\
V\\
W
\end{bmatrix}
=\vec{U}v_c$
P,Q,Rを動座標系のX,Y,Z軸方向の角速度成分とすると、機体の角速度ベクトル$\vec{\omega}$は、
基底$\vec{U}$を使用して以下のように表現できる。
$\vec{\omega}=P\vec{i}+Q\vec{j}+R\vec{k}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
P\\
Q\\
R
\end{bmatrix}
=\vec{U}\omega$
3次元の物理空間上のベクトルである$\vec{v_c}$や$\vec{\omega}$は、幾何ベクトルと呼ばれる（ベクトル解析）。
一方、$v_c$や$\omega$は、幾何ベクトル$\vec{v_c}$や$\vec{\omega}$から座標系の成分を取り出したもので、
成分ベクトル（列ベクトル）と呼ばれる（行列理論）。
2.3 機体の回転運動（姿勢運動）の方程式
1.剛体の角速度ベクトル
剛体上の質点$p$の微小質量を$dm$、その速度を$\vec{V}$とすると、剛体の角運動量ベクトル$\vec{h}$は、
以下のように位置ベクトル$\vec{r}$と運動量ベクトル$d\vec{p}$の外積和として、定義されるベクトルである。
$\vec{h}=\int_V \vec{r} \times d\vec{p}=\int_V \vec{r} \times \vec{V}dm=\int_V \vec{r} \times (\vec{v_c}+\vec{v})dm$
$\vec{h}=\int_V \vec{r}dm \times \vec{v_c} + \int_V \vec{r} \times \vec{v} dm$
剛体上の重心位置では、$\int_V \vec{r}dm=\vec{0}$ なので、剛体の角運動量ベクトル$\vec{h}$は、以下のように簡単になる。
$\vec{h}=\int_V \vec{r} \times \vec{v} dm$
速度ベクトル$\vec{r}$は、角速度ベクトル$\vec{\omega}$を使って以下のように表せる。
$\vec{v}=\vec{\omega}\times\vec{r}$
従って、角運動量ベクトル$\vec{h}$は、以下の式のようになる。
$\vec{h}=\int_V \vec{r} \times (\vec{\omega}\times\vec{r}) dm$
ここで、ベクトルの3重積に関する以下の変形の公式を使用する。
$\vec{r}\times(\vec{\omega}\times\vec{r})=
(\vec{r}\cdot\vec{r})\vec{\omega}-(\vec{r}\cdot\vec{\omega})\vec{r}=
|\vec{r}|^2\vec{\omega}-(\vec{r}\cdot\vec{\omega})\vec{r}$
そこで、位置ベクトル$\vec{r}$の成分を$(x, y, z)$と書いて、角運動量ベクトル$\vec{h}$を成分単位に分けて、
以下のように表現し直す。
$h_x=\int_V(P r^2 - P x^2 - Q yx - R zx)dm$
$h_x=\int_V(Q r^2 - Q y^2 - P xy - R zy)dm$
$h_z=\int_V(R r^2 - R z^2 - P xz - Q yz)dm$
$h=
\begin{bmatrix}
h_x\\
h_y\\
h_z
\end{bmatrix}
\\=
\begin{bmatrix}
\begin{pmatrix}
\int_V r^2 dm & 0 & 0\\
0 & \int_V r^2 dm & 0\\
0 & 0 & \int_V r^2 dm
\end{pmatrix}
-
\begin{pmatrix}
\int_V x^2 dm & \int_V yx dm & \int_V zx dm\\
\int_V xy dm & \int_V y^2 dm & \int_V zy dm\\
\int_V xz dm & \int_V yz dm & \int_V z^2 dm
\end{pmatrix}
\end{bmatrix}
\begin{bmatrix}
P \\
Q \\
R
\end{bmatrix}
\\=
\begin{bmatrix}
\int_V (y^2+z^2) dm & - \int_V yx dm & - \int_V zx dm\\
- \int_V xy dm & \int_V (x^2+z^2)dm & - \int_V zy dm\\
- \int_V xz dm & - \int_V yz dm & \int_V (x^2+y^2) dm
\end{bmatrix}
\omega
\\=J\omega$
2.慣性行列（慣性テンソル）
上記の角運動量ベクトル$\vec{h}$の式に表れた以下の3行3列の行列$J$を慣性行列（慣性テンソル）と呼ぶ。
$J=
\begin{bmatrix}
I_{xx} & -I_{xy} & - I_{xz}\\
-I_{yx} & I_{yy} & - I_{yz}\\
-I_{zx} & -I_{zy} & I_{zz}
\end{bmatrix}
=
\begin{bmatrix}
\int_V (y^2+z^2)dm & -\int_V yx dm & - \int_V zx dm\\
-\int_V xy dm & \int_V (x^2+z^2)dm & -\int_V zy dm\\
-\int_V xz dm & -\int_V yz dm & \int_V (x^2+y^2)dm
\end{bmatrix}
$
以下の慣性行列（慣性テンソル）$J$を使用した角運動量の成分ベクトル$h$の式は、外積行列を使っても計算できる。
$\vec{h}=\int_V \vec{r} \times (\vec{\omega}\times\vec{r}) dm$
$h=J\omega$
上式の積分の()内は、以下のように外積行列を使って表現できる。
$\vec{\omega}\times\vec{r}=
\begin{vmatrix}
\vec{i} & \vec{j} & \vec{k}\\
P & Q & R\\
x & y & z
\end{vmatrix}
=
(Qz-Ry)\vec{i} + (Rx-Pz)\vec{j} + (Py-Qx)\vec{k}
$
上式は、以下のように変形できる。
$\vec{\omega}\times\vec{r}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
Qz-Ry \\
Rx-Pz \\
Py-Qx \\
\end{bmatrix}
=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
0 & -R & Q \\
R & 0 & -P \\
-Q & P & 0
\end{bmatrix}
\begin{bmatrix}
x\\
y\\
z
\end{bmatrix}
$
上式に表れる以下の3行3列の行列を外積行列と呼び、$\tilde{\omega}$又は$\Omega$で表記する。
$\tilde{\omega}=\Omega=
\begin{bmatrix}
0 & -R & Q \\
R & 0 & -P \\
-Q & P & 0
\end{bmatrix}
=-\Omega^\mathrm{T}=-\tilde{\omega}^\mathrm{T}$
この記法を用いると、上記の被積分項の()内の式は、以下のように単純化して表す事が出来る。
$\vec{\omega}\times\vec{r}=
(\vec{U}\omega)\times(\vec{U}r)=
\vec{U}\tilde{\omega}r$
同様にして、上記の被積分項の3重積も、以下のように単純化して表す事が出来る。
$\vec{r}\times(\vec{\omega}\times\vec{r})=
-\vec{r}\times(\vec{r}\times\vec{\omega})=
-\vec{U}r\times\vec{U}(\tilde{r}\omega)=
\vec{U}(-\tilde{r}\tilde{r})\omega
$
右辺の$\vec{U}(-\tilde{r}\tilde{r})\omega$は、具体的に以下のように計算できる。
$\vec{U}(-\tilde{r}\tilde{r})\omega=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
0 & z & -y\\
-z & 0 & x\\
y & -x & 0
\end{bmatrix}
\begin{bmatrix}
0 & -z & y\\
z & 0 & -x\\
-y & x & 0
\end{bmatrix}
\begin{bmatrix}
P\\
Q\\
R
\end{bmatrix}
$
$=\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
y^2+z^2 & -xy & -xz\\
-yx & z^2 + x^2 & -yz\\
-zx & -zy & x^2+y^2
\end{bmatrix}
\begin{bmatrix}
P\\
Q\\
R
\end{bmatrix}
$
以上の計算から、慣性行列（慣性テンソル）$J$を使って、以下のように角運動量ベクトル$\vec{h}$を計算できる。
$\vec{h}=\int_V \vec{r} \times (\vec{\omega}\times\vec{r}) dm=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
\int_V (y^2+z^2) dm & - \int_V yx dm & - \int_V zx dm\\
- \int_V xy dm & \int_V (x^2+z^2)dm & - \int_V zy dm\\
- \int_V xz dm & - \int_V yz dm & \int_V (x^2+y^2) dm
\end{bmatrix}
\begin{bmatrix}
P\\
Q\\
R
\end{bmatrix}
$
$\vec{h}=\int_V \vec{r} \times (\vec{\omega}\times\vec{r}) dm=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
J
\begin{bmatrix}
P\\
Q\\
R
\end{bmatrix}
$
上記の角運動量ベクトル$\vec{h}$の計算から、以下のように、角運動量の成分ベクトル$h$は、慣性行列$J$と角速度成分の列ベクトル$\omega$の積として表される。
$h=J\omega$
3.剛体のモーメント方程式
剛体の中心にある動座標系の原点から見た位置ベクトルを$\vec{r}$とし、
慣性座標系の原点から見た剛体の中心の位置ベクトルを$\vec{r_c}$とする以下の式が成り立つ。
$\vec{v}=\frac{d\vec{r}}{dt}=
\frac{\delta \vec{r}}{\delta t} + \vec{\omega}\times\vec{r}=
\dot{\vec{r}} + \vec{\omega}\times\vec{r}$
次に、回転運動を考えるために、剛体の角運動量ベクトル$\vec{h}=J\omega$の時間微分を考える。
そのために上記の式を適用する。
$\frac{d\vec{h}}{dt}=
\dot{\vec{h}} + \vec{\omega}\times\vec{h}=
\vec{U}J\dot{\omega} + \vec{U}\tilde{\omega} J \omega=
\vec{M}=\vec{M_a} + \vec{M_t}$
$\vec{M_a}$は、操舵面により空力モーメントであり、$\vec{M_t}$は、推力によるモーメントである。
剛体の回転運動に関するニュートンの第2法則(運動量の時間微分は力)は、以下のモーメント方程式で示される。
モーメント方程式: $\frac{d\vec{h}}{dt}=\vec{M}$
4.回転運動（姿勢運動）の運動方程式
剛体の中心にある動座標系の原点から見た位置ベクトルを$\vec{r}$とし、
慣性座標系の原点から見た剛体の中心の位置ベクトルを$\vec{r_c}$とする以下の式が成り立つ。
$\vec{v}=\frac{d\vec{r}}{dt}=
\frac{\delta \vec{r}}{\delta t} + \vec{\omega}\times\vec{r}=
\dot{\vec{r}} + \vec{\omega}\times\vec{r}$
2.4 機体重心の運動方程式
質点運動に関するNewtonの第2法則から機体重心に関する運動方程式（並進運動方程式）を導く。
質量mの運動量ベクトル$\vec{P}$は、その運動速度を$\vec{v}$とすると、次式で定義される。
$\vec{P}=m\vec{v_c}$
これを時間微分して得られる量は、質量mに加えられる力$\vec{F_{A}}$に等しい。
これが、質点運動に関するNewtonの第2法則である。
力の方程式: $\frac{d\vec{P}}{dt}=m\frac{d\vec{v_c}}{dt}=\vec{F_{A}}$
上式を、力の方程式（force equation）と呼ぶ。
さて、剛体としての機体を考え、その重心速度を$\vec{v_c}$とし、その重心周りの回転ベクトルを$\vec{\omega}$とすると以下の式が成り立つのであった。
$\frac{d^2 \vec{r_{c}}}{d t^2} =\frac{d \vec{v_c}}{d t}= \dot{\vec{v_c}}+\vec{\omega} \times \vec{v_c}$
上記の式を上記の「力の方程式」に代入すると、以下の式を得る。
$m(\dot{\vec{v_c}}+\vec{\omega} \times \vec{v_c})=\vec{F_{A}}$ ---- ①
上式①の左辺の（）内の第1項は、機体に固定された動座標系で表すと、以下のようになる。
$\dot{\vec{v_c}}=
\dot{U}\vec{i}+\dot{V}\vec{j}+\dot{W}\vec{k}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
\dot{U} \\
\dot{V} \\
\dot{W}
\end{bmatrix}=
\vec{U}\dot{v_c}$ ---- ②
上式①の左辺の（）内の第2項の外積$\vec{\omega}\times\vec{v_c}$は、瞬間回転中心に向かう向心加速度であり、以下のように表される。
$\vec{\omega}\times\vec{v_c}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
0 & -R & Q\\
R & 0 & -P\\
-Q & P & 0
\end{bmatrix}
\begin{bmatrix}
U \\
V \\
W
\end{bmatrix}=
\vec{U}\tilde{\omega}v_c
$ ---- ③
上式①の右辺は、力の成分を$F_x,F_y,F_z$とすると、以下のように表される。
$
\vec{F_A}=F_x\vec{i}+F_y\vec{j}+F_z\vec{k}=
\begin{bmatrix}
\vec{i} & \vec{j} & \vec{k}
\end{bmatrix}
\begin{bmatrix}
F_x \\
F_y \\
F_z
\end{bmatrix}=
\vec{U}F_A
$ ---- ④
上式①に、上式②③④を代入すると、以下の機体重心の運動方程式を得る。
$m\vec{U}(\dot{v_C}+\tilde{\omega}v_c)=\vec{U}F_A$
→
$m(\dot{v_C}+\tilde{\omega})=F_A$ ---- ⑤
上式⑤を、質量mで除して加速度成分のベクトルで表せば、以下の連立微分方程式になる。
$\begin{bmatrix}
\dot{U} \\
\dot{V} \\
\dot{W}
\end{bmatrix}=
\begin{bmatrix}
0 & -R & Q\\
R & 0 & -P\\
-Q & P & 0
\end{bmatrix}
\begin{bmatrix}
U \\
V \\
W
\end{bmatrix}+
\frac{1}{m}
\begin{bmatrix}
F_x \\
F_y \\
F_z
\end{bmatrix}
$
2.5 座標系の種類と座標成分の変換
1.座標軸の種類
2.空気力の座標変換
2.6 慣性行列（テンソル）の座標変換
1.慣性行列（テンソル）の座標変換
2.慣性行列の慣性主軸系と安定軸系間の変換
2.7 機体姿勢のオイラー角表現と速度・角速度の座標変換
2.8 重力と慣性乗積の影響
2.9 ジャイロ連成と推力によるモーメント
3. 微小擾乱運動方程式
目次
2.1 クォータニオンについて思うこと
1.説明
本記事ではクォータニオンについて総整理します。クォータニオン (四元数) は、
- 航空機
- 宇宙機
- ロボティクス
- 3D ゲーム
- 3D コンピュータグラフィックス
などなど、幅広い分野で活用されているトピックです。
航空宇宙業界と 3D ゲーム業界との両方で盛んに用いられているという点でとても興味深いです。
特に最近では Unity の流行により、職業ゲームプログラマだけでなく、趣味で 3D ゲーム制作を楽しむ方にとっても、クォータニオンについて理解したいという方が増えているのを感じます。
本記事では、クォータニオンについて必要な事柄を総整理するとともに、クォータニオンをワカッタ感覚で納得することを目標にします。
2.フライトシュミレータ
Unityを使って鳥人間コンテストの滑空機部門を想定したフライトシミュレーターを作成する場合、
以下のような計算が必要になるらしい。
- 質量・重心位置・慣性モーメント
- 迎角と横滑り角 (α, β)
- 突風成分
- 地面効果
- 縦のつり合い
- 縦の空気力 X，Z
- 縦のモーメント M
- 横・方向のつり合い
- 横・方向の空気力 Y
- 横・方向のモーメント L，N
2.2 オイラーの公式からロドリゲスの式へ
オイラーの定理
オイラーの定理は、3次元において物体の回転を
「ひとつの回転軸と回転角度で表すことができる」
ということを示している。
図1: オイラーの定理
たとえば，任意のベクトル$a$を回転行列$R$によって回転させたベクトル$a'$は,
$Ra=a'$
と書くことができ、図１の点Pは回転 $R$ によって点P'に移動する．
これに対して，唯一回転軸 $n$ に対しては
$Rn=n$
が成り立ち，図１の点Qは同じ位置にとどまる．
これは回転 $R$ によって「不変なベクトル」 $n$ が存在し，それが回転軸であることに相当する．
なお， $n$ を単位ベクトルとし，
$Rn= \lambda n$
で記述した直交行列 $R$ に対する固有値$\lambda$は， 1, $e^{i \psi}$, $e^{-i \psi}$ である。
このうちの１である固有値は，回転を行っても不変を意味することから，唯一の回転軸の存在理由となっている．
ロドリゲスの式
物体の回転は，オイラーの定理から「ひとつの回転軸 $n$ と回転角度 $\psi$ で表せる」ので，回転行列は
$R = E + [n \times ] \sin \psi + [n \times ]^2(1- \cos \psi) $ (1)
のように表せる．これがロドリゲスの式（Rodrigues formula）として知られている．
ここで， $E$ は $3\times3$ の単位行列， $[n \times]$ は外積を表す歪対称行列である（次節参照）．この式の導出については最後に示す．
ロドリゲスの式は幾何学条件を使用し，他にも多様な表現が可能だが，ここではこの式の幾何学的意味を考える．
その前に，外積と歪対称行列について次で説明を行う．
外積
ベクトル $a$ と $b$ の外積（outer product）はベクトルであることから $a \times b$ ベクトル積（vector product）とも呼ばれる。
一般に記号で $a \times b$ のように表現されるためクロス積（cross product）とも呼ばれる．
ベクトル $a$ と $b$ の外積（outer product） $a \times b$ は，図２のように，２つのベクトル $a$, $b$ に対して垂直な方向のベクトルを定める．
図2: 外積の幾何学的意味
具体的な外積 $a \times b$ の幾何学的性質は
1. $a$, $b$ の両方に直交し，始点を一致させて $a$ を $b$ に近づけるように，右ネジを回す際の進行方向を向く.
2. その大きさは， $a$, $b$ の始点を一致させて作る平行四辺形の面積（$|a||b|\sin\psi$ ）に等しい.
また，２つのベクトル
$a=\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}$, $b=\begin{bmatrix}b_x\\b_y\\b_z\end{bmatrix}$
に対して，外積の成分を
$a \times b=\begin{bmatrix}a_y b_z - a_z b_y\\a_z b_x - a_x b_z\\a_x b_y - a_y b_x\end{bmatrix}$
のように書くことができる．
歪対称行列を使用した外積表現
$n=[n_x~n_y~n_z]^T$ に対して，先に述べた $[n \times]$ は以下のように定義される行列
$[n \times] \equiv \begin{bmatrix}0&-n_z&n_y\\n_z&0&-n_x\\-n_y&n_x&0\end{bmatrix}$
である．このような $A^T = -A$ の関係を満たす行列を歪対称行列，または交代行列と呼ぶ．
これによって，ベクトルの外積を
$c=a\times b=[a\times]b$
$\begin{bmatrix}c_x\\c_y\\c_z\end{bmatrix}=\begin{bmatrix}a_y b_z - a_z b_y\\a_z b_x - a_x b_z\\a_x b_y - a_y b_x\end{bmatrix}=\begin{bmatrix}0&-a_z&a_y\\a_z&0&-a_x\\-a_y&a_x&0\end{bmatrix}\begin{bmatrix}b_x\\b_y\\b_z\end{bmatrix}$
のように書くことができる．
このような外積計算の行列による表現は，外積計算を含んだ式も線形表現できるので，今後多用することになる．
ロドリゲスの式の行列表現
ここで，ひとつ注意点を述べると，回転行列はオイラー角でも，単位クォータニオン（オイラーパラメータ）などで表現されうことが多いが，ロドリゲスの式でも回転行列を表現できることである．
すなわち回転軸を意味する単位ベクトル $n$ の3変数と回転角度 $\psi$ でも，回転行列を表現できることを示している．
これはクォータニオンの性質と似ているのだがその意味は，次の章で説明する．
式(1)を展開すると，
$R_{n}(\psi) =\\ \hspace{5mm}\left[\begin{array}{cc}\cos\psi + n_x^2(1-\cos\psi) & n_x n_y(1-\cos\psi)-n_z \sin \psi \\ n_y n_x(1-\cos\psi)+ n_z \sin \psi & \cos\psi + n_y^2(1-\cos\psi) \\ n_z n_x(1-\cos\psi) - n_y \sin \psi & n_z n_y(1-\cos\psi) + n_x \sin \psi \end{array}\right.\\ ~\\ \hspace{6cm} \left.\begin{array} {c}n_x n_z(1-\cos\psi) + n_y \sin \psi\\ n_y n_z(1-\cos\psi) - n_x \sin \psi\\ \cos\psi + n_z^2(1-\cos\psi)\end{array}\right]$
となる．ここで，回転軸の単位ベクトル $n$ の成分を
$n=\begin{bmatrix}n_x\\n_y\\n_z\end{bmatrix}$
とした．
ロドリゲスの式の逆変換
逆に，回転行列が既知のときに回転軸 $n$ と，回転角度 $\psi$ も計算可能である．
解析では回転行列を定めてから回転軸と回転角度を知りたいというこちらのニーズのほうが高いかもしれない．
前述の直交行列の固有値が１の場合を考えると，それに対応する固有ベクトルは回転軸に相当することを述べた．その時の固有ベクトル，
すなわち回転軸を $w=[w_1~w_2~w_3]^T$とすると，
$Rw = w$
が成立する．
また， $R$ は直交行列なので， $R^T = R^{-1}R$ が成立し，両辺に $R^T$ を掛けて
$R^T R w = R^Tw \\ w = R^Tw$
となるので，
$(R - R^T)w = 0$
を得る．また，回転行列 $R$ が以下のように既知のとき，
$R=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33}\end{bmatrix}$
とすると，
$(R - R^T)w = 0 \\ \begin{bmatrix}0 & r_{12}-r_{21} &r_{13} - r_{31}\\ r_{21} - r_{12} & 0 & r_{23} - r_{32} \\ r_{31}-r_{13} & r_{32}-r_{23} & 0\end{bmatrix} \begin{bmatrix}w_1\\w_2\\w_3\end{bmatrix}= \begin{bmatrix}0\\0\\0 \end{bmatrix}$
となるが， $R - R^T$ は歪対称行列となっていることに気がつく．
そこで，これをベクトル $W$ で記述された $[W \times]$ と書くと，これは外積の演算を意味するので，
$W \equiv \begin{bmatrix} r_{32}-r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12}\end{bmatrix} \\ R - R^T = \begin{bmatrix}0 & r_{12}-r_{21} &r_{13} - r_{31}\\ r_{21} - r_{12} & 0 & r_{23} - r_{32} \\ r_{31}-r_{13} & r_{32}-r_{23} & 0\end{bmatrix}=[W \times]$
と書け， $W \times w = 0$ を満たす $w$ は，外積の幾何学的意味からも
$w = W$
しかない．そこで，ベクトル $w$ を
$w\equiv\begin{bmatrix}r_{32} - r_{23}\\ r_{13} - r_{31} \\ r_{21}-n_{12}\end{bmatrix}$
と定義すると，大きさが１の回転軸 $n$ は
$n=\frac{w}{||w||}$
のようにベクトル $w$ の単位ベクトルとなり，回転角度 $\psi$ は
$\psi = \mathrm{atan2} (\sin \psi, \cos \psi)$
となる．ただし，ここで
$\cos \psi = \frac{1}{2}(\mathrm{tr}~ R - 1)\\ = \frac{1}{2}(n_{11} + n_{22} + n_{33}-1)$
であり， $n$, $\cos \psi$ が既知なので， $\sin \psi$ はロドリゲスの式 $R = E + [n \times ] \sin \psi + [n \times ]^2(1- \cos \psi)$
(1−cosψ) から算出する．
また，トレース $\rm{tr}~ R$ は，前述の行列 $R$ の対角要素の和
${\mathrm{tr}}~ R = n_{11} + n_{22} + n_{33}$
である．
角度を計算する $\rm{atan2} (y,x)$ はプログラミング言語などの組み込み関数として広く使用されている２引数の逆正接関数（ $\tan()$ の逆関数）である．
引数の順番が y,xy,x の順番であることに注意する．
これを使用することで，角度が四象限にわたる符号も得られ，かつ計算精度も高い．
なお，PythonのNumpy場合のatan2()関数としては，np.arctan2(y, x)を使用する．
これは引数が二つで，arctan(y / x)をラジアンで返す．
この角度は，極座標平面において原点から座標(x, y)へのベクトルがx軸の正の方向となす角度（偏角）であり，戻り値は -\pi−π から \piπ （-180度から180度）の間になる．
計算言語によって，引数の順番，定義域に差があるので注意されたい．
読者は角度 $\psi$ の計算は， $\cos \psi = \frac{1}{2}(\rm{tr}~ R - 1)$ の逆関数で計算すれば良いと考えるかもしれないが，
上記の符号と計算精度の理由から，上記のように計算が複雑化しても，原則 $\cos ^{-1}()$ を角度計算では使用してはいけないことを留意しておくべきである．
2.3 ロドリゲスの式の導出
ベクトルの計算準備
ここでは，次節でロドリゲスの式の証明を行うための，射影と反射影というベクトルの幾何学的計算準備を行う．
図3: ベクトルの射影と直交成分
ベクトル $a$ はベクトル $n$ に平行な成分 $a_{||}$ と，ベクトル $n$ に対して直交する成分 $a_{\perp}$
とに分解することができるため，ベクトル $a$ は
$a = a_{||} + a_{\perp}$ (2)
と書くことができ， $a_{||}$ を $a$ の $n$ への射影(rejection)， $a_{\perp}$ を $a$ の $n$ からの反射影(rejection)とよぶ.
射影 $a_{||}$ の長さは，ベクトル $a$, $n$ の内積 $a^T n$ で計算できるので，射影 $a_{||}$ は
$a_{||}= (a^T n)n$
と書け，さらに
$a_{||} = (a^T n)n= (n^T a)n=n(n^T a)\\ = (n n^T) a$
と書き換えることができる．ここで，内積は順序を入れ替えても良いというルールなどに基づいて書き換えている．
一方，反射影 $a_{\perp}$ は式(2)を利用して，
$a_{\perp} = a - a_{||}= a- (n^T n) a = (E - n^T n)a$
と書くことができる．
この射影と反射影を利用して，次にロドリゲスの式を幾何学的に導いていく．
ロドリゲスの式の幾何学的意味
この節では，ロドリゲスの式の幾何学的意味を理解するために，ロドリゲスの式の幾何学的な導出を行う．
内積，外積，ベクトル三重積などの性質を使用して証明を試みるが，この節は読み飛ばしても構わない．
図4: ロドリゲスの式の幾何学的意味
回転行列 $R$ にベクトル $a$ をかけると，ベクトル $a$ を角度 $\psi$ だけ回転して $a'$ となる．
これを式で表すと，
$a'=Ra = a + [n \times ] a \sin \psi + [n \times ]^2 a(1- \cos \psi)$ (2)
となる．そこで，図４におけるベクトル $a'$ と，この式の関係について検証する．
ベクトル $a'$ は点P'を指し示している．点P'は，原点Oから，点P，Hを経由してP'に達するので
$a' = \overrightarrow{OP}' = \overrightarrow{OP}+\overrightarrow{PH}+\overrightarrow{HP'}$
と書くことができる．そこで，これを分解して考える．まず，円の半径は同じ長さであるので，
$||\overrightarrow{QR}||=||\overrightarrow{QP'}||=||\overrightarrow{QP}||=||[n\times] a||=||a_{\perp}||=||(E-n^T n)a||$
が成り立つ．そして，まず，
１： $ \overrightarrow{OP}$ はベクトル $a$
$\overrightarrow{OP}=a$
である．
２： $\overrightarrow{PH}$ は， $\overrightarrow{PH}$ の長さに $\overrightarrow{PH}$ 方向の単位ベクトルをかけたものなので，
２-１： まず，長さ $||\overrightarrow{PH}||$ を求める．
$||\overrightarrow{PH}||=||\overrightarrow{HP}||=||\overrightarrow{QP}-\overrightarrow{QH}||$ であるため．
２-１-1： まず， $\overrightarrow{QP}$ を考えると，ベクトル $\overrightarrow{QP}$ は $a$ の $n$ からの反射影 $a_{\perp}a$ であるため，
前節の結果を利用し， $\overrightarrow{QP}=a_{\perp}= (E - n^T n)a$ を得る．
２-１-２： また，ベクトル \overrightarrow{QH}
QH
は， \overrightarrow{QP’}
QP’
のベクトルの \overrightarrow{QP}=\bm{a}_{\perp}
QP
=a
⊥
への射影であるので
$\overrightarrow{QH} = \cos \psi ||\overrightarrow{QP'}|| \frac{\overrightarrow{QP}}{||\overrightarrow{QP}||}= \cos \psi ||\overrightarrow{QP}|| \frac{\overrightarrow{QP}}{||\overrightarrow{QP}||}= \cos \psi ~\overrightarrow{QP} \\=\cos\psi ~a_{\perp}=\cos \psi(E-n^Tn) a$
となる．2-1-1と2-1-2の結果を合わせて，
$||\overrightarrow{PH}|| =||\overrightarrow{QP}-\overrightarrow{QH}||\\=||a_{\perp}-\cos \psi a_{\perp}||\\=||(1-\cos \psi)a_{\perp}||$
となる．以上から， $\overrightarrow{PH}$ は，長さ $||\overrightarrow{PH}||$ に
$\overrightarrow{PQ}$ 方向の単位ベクトルをかけたものなので，
$\overrightarrow{PH} = -||(1-\cos \psi)a_{\perp}|| ~\overrightarrow{QP} = -||(1-\cos \psi)a_{\perp}|| ~\frac{a_{\perp}}{||a_{\perp}||}\\=-(1 - \cos \psi)a_{\perp}= -(1 - \cos \psi)(E-n^Tn) a$
を得る．
３： 最後に，ベクトル $\overrightarrow{HP'}$ は，長さ $||\overrightarrow{HP'}||=||\overrightarrow{QP'}||\sin \psi = ||~[n\times] a~|| \sin \psi$
に対して， $\overrightarrow{HP'}$ の向きのベクトル $[n\times] a$ の単位ベクトル $\frac{[n\times] a}{||~[n\times] a~||} $
をかけたものであるので，
$\overrightarrow{HP'}=||~[n\times] a~|| \sin \psi \frac{[n\times] a}{||~[n\times] a~||} = \sin \psi ~[n\times] a $
を得る．以上をまとめて，
$a' = \overrightarrow{OP}' = \overrightarrow{OP}+\overrightarrow{PH}+\overrightarrow{HP'}\\=a-(1 - \cos \psi)(E-n^Tn) a+\sin \psi ~[n\times] a$ (3)
を得る．
なお，ベクトルの三重積から
$(c\times b) a = ([c \times ][b \times ]) a = (bc^T )a − (c^T b)a$
が成り立つが，ここで $b$, $c$ それぞれに $n$ を代入すると，
$([n \times ][n \times ])a = (nn^T )a − Ea = (nn^T − E)a$
となることを利用し，式(3)に代入すると，
$a' = \overrightarrow{OP}' = \overrightarrow{OP}+\overrightarrow{PH}+\overrightarrow{HP'}\\=a+(1 - \cos \psi)([n \times ][n \times ]) a+\sin \psi ~[n\times] a\\= a + [n \times ] a \sin \psi + [n \times ]^2 a(1- \cos \psi)$
を得て，これは式(1)のロドリゲスの式と一致する．以上，ロドリゲスの式を幾何学的に導出した．
最後に
ロドリゲスの式の幾何学的意味を説明した．
ここで重要なことは，この式は回転行列 $R$ を回転軸を示す単位ベクトル $n$ と回転角度 $\psi$ だけで表現でき，特にこの式(1)の形では，複素数で２次元の回転運動を記述するオイラーの公式を連想させる形式で書かれていることにある．
式(1)のベクトル $\overrightarrow{OP}=a$ で平面に移動した後は，図４右の円で表される平面内の運動を考えればよく，この式は，なにやら $[n\times]$ が複素数における虚数と似たような役割を果たしそうなことと，３次元の回転を表すクォータニオンが２次元の複素数の拡張になっている？ことを予感させてくれる．
4.安定微係数の推算
目次
3.1 クォータニオンを使って何をするか
そもそもクォータニオンを使って何がやりたいのでしょうか？大きく分けて以下の 2 つがあるように思います。
- 3D 空間における「回転」を表す
- 3D 物体の「姿勢」を表す
2 つと言ったのですが、メインは「回転」の方でしょう。
「姿勢」は単純に「どういう回転でその向きになったか」として捉えることができます。
注意点として、「姿勢」は「方向」よりも多くの情報を含んでいます。
クォータニオンを用いた回転を扱うときに、回転させたい対象が「姿勢」なのか「方向」なのかを混乱しないようにすることが肝要です。
方向は「進行方向」や「視線」といったものを表していますが、姿勢はそれに加えて、その方向周りの角度の情報も含みます。
航空機でいえば、機首の向いている方向だけでなく、機首に対して機体がどれだけ傾いているのかも指定してはじめて機体の姿勢が完全に決まります。
数理的には以下のようになっています。
| 自由度 | 備考 | |
|---|---|---|
| 方向 | 2 | 進行方向は三次元ベクトル (3 個のパラメータ) で表せますが、そのベクトルの大きさは意味を持たないので 1 減らして 2 です |
| 姿勢 | 3 | 方向 + 1 です |
つまり、姿勢は最小 3 個のパラメータで表せるということです。
回転も姿勢と本質的に同じものなので、同様に 3 個のパラメータで表せます。
実際にオイラー角は 3 個のパラメータで姿勢や回転を表します (回転を表す方法はクォータニオンだけではなく、オイラー角による方法もメジャーな方法の 1 つです)。
オイラー角は Unity ではインスペクターの Rotation 項目で表示されているやつです。
(しかし transform.rotation は Quaternion 型なので少し紛らわしいですね...この記事でも注意喚起がなされています)
3.2 クォータニオンとは
クォータニオンは任意軸回転がキーワードとして言及される場面も多いです。
いかなる回転 (三次元空間内) も表現できることが強みです。具体的には以下のように定義します。
三次元空間において
- 方向ベクトル $(\lambda_x, \lambda_y, \lambda_z)$ を回転軸として
- 角度 $\theta$ だけ回転する
という「回転」を表すクォータニオンは、四次元ベクトル $(\lambda_x \sin{\frac{\theta}{2}}, \lambda_y \sin{\frac{\theta}{2}}, \lambda_z \sin{\frac{\theta}{2}}, \cos{\frac{\theta}{2}})$ で表します。
これらは Unity では、Quaternion 型変数を qua として、
- qua.x = $\lambda_x \sin{\frac{\theta}{2}}$
- qua.y = $\lambda_y \sin{\frac{\theta}{2}}$
- qua.z = $\lambda_z \sin{\frac{\theta}{2}}$
- qua.w = $\cos{\frac{\theta}{2}}$
という風に対応しています。
(が、Unity の Quaternion を扱うときにこれらの成分を直接触ることはあまりないかもです...スクリプトリファレンスにもクォータニオンに熟知していない限りは x, y, z, w の値を直接変更しないでくださいと書いてあります)
また Unity で上記の定義に則ってクォータニオンを生成するときは Quaternion.AngleAxis を用います (オイラー角など他の方法でクォータニオンを生成する方法は後述します)。
// Y 軸 (上方向) まわりに 30 度回転するのを表すクォータニオン Quaternion.AngleAxis(30, Vector3.up)
ところで上の定義を見ると
- なんでいきなり三角関数が...
- $\frac{\theta}{2}$ ってなんだよ... $\theta$ でいじゃん...
と思ってしまいそうです。
その答えは「こう定義すれば具体的な回転計算が超楽だから」なのですが、本記事を読み進めて行くことで少しずつ実感していただけたらと思います。
3.3 クォータニオンによる回転操作
クォータニオンによる「回転」を定義したからには、実際に「三次元ベクトル」や「姿勢」を回転させる計算方法を導出してあげる必要があります。
三次元ベクトル $\vec{v}$ の回転計算は
- キャラクターの視線をある方向へと徐々に向けて行く (3D ゲーム)
- 宇宙機に搭載した望遠鏡を所望の方向へ向ける (航空宇宙)
といった場面で必要になります。
また、姿勢 $p$ の回転計算は、
- クォータニオンとクォータニオンの補間 (3D ゲーム)
- 航空機や宇宙機の三軸姿勢制御 (航空宇宙)
といった場面で必要になります。
結論としては、三次元ベクトル $\vec{v}$ や姿勢クォータニオン $p$ に対し、回転 $q$ を実施した結果は以下のように大変シンプルなものになります！！！！！！！
| 回転させるもの | 回転結果 | 備考 |
|---|---|---|
| $\vec{v}$ | $q \otimes \vec{v} \otimes \bar{q}$ | $\bar{q}$ は $q$ の逆回転 |
| 姿勢クォータニオン $p$ | $q \otimes p$ |
逆に
回転操作をシンプルな計算で実現できるように、クォータニオンをあのように定義した
とも言えます。
なお、クォータニオン同士のかけ算 $q \otimes p$ や、クォータニオンとベクトルとのかけ算 $q \otimes \vec{v}$ はまだ定義していないですが、1-4 で導入します。
また、こうした事柄はクォータニオンを自分で実装する場合には必要な知見なのですが、Unity ではいずれも単に
Unityでの回転操作
v2 = q * v // 三次元ベクトル v の回転 p2 = q * p // 姿勢 p の回転
と「*」演算子で実現することができます。
$q \otimes \vec{v} \otimes \bar{q}$ な量を q * v と記述するのは少し紛らわしさがありますが、実用上とても簡便です。
演算子の順番は重要で、左右を入れ替えてはダメです。
3.4 なぜクォータニオンなのか
三次元空間上での回転や姿勢を表現する方法には代表的なものが 3 つあります。
- クォータニオン
- オイラー角
- 回転行列 or DCM
| パラメータ数 | メモリ消費 | 計算上の扱いやすさ | 備考 | |
|---|---|---|---|---|
| クォータニオン | 4 | 〇 | ◎ | バランス型で、その他様々なメリットがあります |
| オイラー角 | 3 | ◎ | × | 具体的な姿勢をイメージしやすく、メモリ的にも優しいですが、計算上の難点を抱えています |
| 回転行列 or DCM | 9 | × | 〇 |
どの方法にもメリット・デメリットがあるので、状況に応じて使いこなせばいいのですが、総じてクォータニオンはバランスがとれた優秀な方法であることがわかります。
オイラー角はたった 3 個のパラメータで回転や姿勢を表現できるので、メモリ消費は少なくて済むのですが、三角関数を多用することもあって計算時間的には大きくなってしまいます。
それどころかもっと悪いことに、オイラー角の二番目の角度が ±90±90 度になる場合が特異点となって計算上の大きな問題を引き起こすことが知られています (詳しくはジンバルロックを参照)。
端的に言えば、ヨーとロールの区別ができなくなってしまう状態です。
しかしながら特に航空機の世界では
- そもそもジンバルロックが起こるような状態にならない (ピッチが ±90±90 度になる状態...下図のような墜落時などに発生しそうな相当ヤバイ状態です...戦闘機などでは考慮が必要です)
- ヨー、ピッチ、ロールという概念がとてもなじみ深い
といった理由によって盛んに用いられています。
また、3D ゲームの世界などにおいても、オイラー角の直感的なイメージしやすさから、UI や API 部分では用いられるケースも多いです。
3.5 クォータニオンのかけ算
