機体座標系と地上座標系の変換

# これは何？

ドローンのシミュレーションをやっています。座標変換がこんがらがる時があるので、線形代数での座標変換の正確な議論をまとめておく。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/51055/677f91c5-89da-a1a6-1ea1-e9ceb14d6572.png)

# 機体座標と地上座標

機体座標系 $v = (u, v, w)^T$ から地上座標系 $v_e = (u_e, v_e, w_e)^T$ への速度変換行列は以下のようになります．加速度，角速度，角加速度，力，トルクも同様で，すべてのベクトルについてはこの行列を掛けることで変換できます（ただしオイラー角はベクトルではありません）．

```math
\left[
  \begin{array}{c}
   u_e \\
  v_e \\
  w_e \end{array}
\right] =
  \begin{bmatrix}
    \cos\theta\cos\psi & \sin\phi\sin\theta\cos\psi - \cos\phi\sin\psi & \cos\phi\sin\theta\cos\psi + \sin\phi\sin\psi \\
    \cos\theta\sin\psi & \sin\phi\sin\theta\sin\psi + \cos\phi\cos\psi & \cos\phi\sin\theta\sin\psi -\sin\phi\cos\psi \\
    -\sin\theta & \sin\phi\cos\theta & \cos\phi\cos\theta
  \end{bmatrix}
\left[
  \begin{array}{c} u \\
  v \\
  w \end{array}
\right]
```

この行列は，方向余弦行列（DCM: Direction Cosine Matrix）と呼ばれ，3つの回転行列のこの順の積 $R_z(\psi)R_y(\theta)R_x(\phi)$ となります． DCM は常に直交行列であり，固有値として '1' を1つ持ちます．'1' に対応する固有ベクトルの方向が回転軸の方向（クォータニオンの虚数部）です．

```math
R_z(\psi) = \begin{bmatrix}
    \cos\psi & -\sin\psi & 0 \\
    \sin\psi & \cos\psi & 0 \\
    0 & 0 & 1
  \end{bmatrix}, \quad
R_y(\theta) = \begin{bmatrix}
    \cos\theta & 0 & \sin\theta \\
    0 & 1 & 0 \\
    -\sin\theta & 0 & \cos\theta
  \end{bmatrix}, \quad
R_x(\phi) = \begin{bmatrix}
    1 & 0 & 0 \\
    0 & \cos\phi & -\sin\phi \\
    0 & \sin\phi & \cos\phi
  \end{bmatrix}
```

機体座標系の原点は機体の重心です．機体座標系は機体に固定されており，機体座標系は機体とともに移動します．また，地上座標系の原点を機体座標系の原点に一致していると仮定します．これを特に "Vehicle Carried NED" （機体に固定されたNED座標系）と呼びます．すなわち，以下の式では，地上座標系の原点 $O$ は機体座標系の原点 $O'$ と一致しているとして立式します．

## 座標変換

オイラー角は，地上座標系の座標軸を機体座標へと， $z$ -軸($\psi$)， $y$ -軸($\theta$)， $x$ -軸($\phi$)の順に回転することで重ねられるように表現するとします．

$R_z(\psi), R_y(\theta), R_x(\phi)$ はそれぞれ基底変換行列である。例えば、地上座標系を最初に回転する $R_z(\psi)$ は基底 $e_x, e_y, e_z$ を $z$ 軸周りに回転する。

```math
\begin{bmatrix} \boldsymbol{e}_x' & \boldsymbol{e}_y' & \boldsymbol{e}_z' \end{bmatrix} = 
  \begin{bmatrix} \boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}
  \begin{bmatrix}
    \cos\psi & -\sin\psi & 0 \\
    \sin\psi & \cos\psi & 0 \\
    0 & 0 & 1
  \end{bmatrix} = \begin{bmatrix} \boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}
R_z(\psi)
```


新しい $\boldsymbol{e}_x'$ に着目するとは古い基底の $\boldsymbol{e}_x, \boldsymbol{e}_y, \boldsymbol{e}_z$ を使って次のように表される（行列の一列目に注目）。

```math
\boldsymbol{e}_x' = (\cos\psi) \boldsymbol{e}_x +(\sin\psi) \boldsymbol{e}_y + (0)\boldsymbol{e}_z
```

## 基底変換（一般論）

さて、線形代数の一般論として、基底変換行列 $R$ を使って、基底変換は、

```math
\begin{bmatrix} \boldsymbol{e}_x' & \boldsymbol{e}_y' & \boldsymbol{e}_z' \end{bmatrix} =  \begin{bmatrix} \boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}R
```


と表される。旧座標 $\boldsymbol{r}=(x, y, z)^T$ と新しい座標 $\boldsymbol{r}'=(x', y', z')^T$ は次のような関係になる（両辺のベクトルはもともと同じものを表現している）。

```math
\begin{array}{l}
\begin{bmatrix} \boldsymbol{e}_x' & \boldsymbol{e}_y' & \boldsymbol{e}_z' \end{bmatrix} 
\begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} &=
\begin{bmatrix} \boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}
\begin{bmatrix} x \\ y \\ z \end{bmatrix}
\end{array}
```


この2式から、座標についての変換式は次のようになる（最初の式の両辺に右から $(x',y',z')^T$を掛けて2式目を使う）。

```math
\begin{array}{l}
\begin{bmatrix} x \\ y \\ z \end{bmatrix} &= R \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} \\ \\
\boldsymbol{r} &= R \boldsymbol{r}'
\end{array}
```

右辺は変換後の「基底とその座標成分」の積になっていて、それは変換の前後で変化しない。

## 機体・地上座標変換への適用

さて、これを今回の3つの回転行列すなわち、地上座標系から機体座標系への変換($\psi, \theta, \phi$)にこの順で適用する。

```math
\begin{array}{l}
\boldsymbol{r} &= R_z(\psi)\boldsymbol{r}' \\
\boldsymbol{r}' &= R_y(\theta)\boldsymbol{r}'' \\
\boldsymbol{r}'' &= R_x(\phi)\boldsymbol{r}'''
\end{array}
```

すなわち、

```math
\begin{array}{l}
\boldsymbol{r} = R_z(\psi) R_y(\theta) R_x(\phi) \boldsymbol{r}'''
\end{array}
```

となって、これが機体座標系から地上座標系への変換行列（DCM）になる。