【宇宙機制御入門】キネマティクスその3，クォータニオン【第3弾】

学習広場

2024.05.142024.06.04

前回の記事

【宇宙機制御入門】キネマティクスその2，オイラー角【第2弾】

前回の記事はじめにこの講座では，宇宙機の主に姿勢力学・制御について，初学者の方が難しい専門書を読まずにサクッとエッセンスが習得できることを目指しています．また，そういった専門書では述べられない，理解の近道やちょっとし...

Contents

1 はじめに
2 キネマティクス表現
3 クォータニオンによるDCM表現
4 数値計算時の注意点
5 まとめ

はじめに

この講座では，宇宙機の主に姿勢力学・制御について，初学者の方が難しい専門書を読まずにサクッとエッセンスが習得できることを目指しています．また，そういった専門書では述べられない，理解の近道やちょっとした工夫なども盛り込むようにしています．

宇宙工学の分野，特に宇宙機の制御に関する専門書は和書にはほとんどないのが現状です．またそういった専門書は，初学者にはあまりにも難解で途方にくれてしまいます．院試や学科の勉強で苦労される方も多いと思います．実際に筆者もそうでしたしなんなら今もよく困ります笑．そのような方が，まずは手軽に学習のとっかかりを得て・概観と本質を上手くつかんでいただくサポートができれば，と考えています．不明点などがあれば，ぜひ気軽にコメントいただければ，追記させていただきます！

姿勢の表記には，座標系が大切です．ある座標での姿勢と別の座標での姿勢は，座標を取っ払えば同じ姿勢です．しかしながら計算していく上で何かしら量を与えなくてはいけません，そこで，どのように姿勢を表記して，どのように二つの座標間で変換計算を行うかを考えるべきです．

キネマティクス表現

おさらいですが，方式には，

DCM（Direction cosine matrix）方向余弦行列
オイラー角
クォータニオン

がありました．

今回はクォータニオンについてです．

クォータニオンによるDCM表現

クォータニオン（オイラーパラメーター）

2つの座標は，ある軸まわりの1回の回転で一致可能

$$\begin{eqnarray*}q_0, q_1, q_2, q_3 q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1\end{eqnarray*}$$
$$\begin{eqnarray*}q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1\end{eqnarray*}$$

クォータニオンを

$$\begin{eqnarray*}\mathbf{q} &=& (q_0, q_1, q_2, q_3) \\&=& \begin{pmatrix} \cos \frac{\phi}{2} \\ \sin \frac{\phi}{2} \mathbf{m} \end{pmatrix}\end{eqnarray*}$$
と定義する．なお，$\mathbf{m}$ は，回転軸の単位ベクトル．

このとき，回転行列を求めると，

$$\begin{eqnarray*}C^b_i =\begin{pmatrix}q_0^2 + q_1^2 – q_2^2 – q_3^2 & 2(q_1q_2 + q_0q_3) & 2(q_1q_3 – q_0q_2) \\2(q_1q_2 – q_0q_3) & q_0^2 – q_1^2 + q_2^2 – q_3^2 & 2(q_2q_3 + q_0q_1) \\2(q_1q_3 + q_0q_2) & 2(q_2q_3 – q_0q_1) & q_0^2 – q_1^2 – q_2^2 + q_3^2\end{pmatrix}\end{eqnarray*}$$

これを時間微分することで，キネマティック方程式は，

$$\begin{eqnarray*}\dot{\mathbf{q}} = \frac{1}{2}\begin{pmatrix}
-q_1 & -q_2 & -q_3 \\
q_0 & -q_3 & q_2 \\
q_3 & q_0 & -q_1 \\
-q_2 & q_1 & q_0
\end{pmatrix}\mathbf{\omega}^b\end{eqnarray*}$$

となります．クォータニオンは，計算量が少ないため，リアルタイム処理などでは必須級ですね．ドローンなどでもよく使われている印象です．

数値計算時の注意点

宇宙機の姿勢計算において、数値積分時に気をつけるべき重要なポイントの一つが、クォータニオンの正規化です。姿勢の計算では、特に長期間のシミュレーションや高精度な姿勢制御が必要な場合、数値積分の誤差が蓄積してクォータニオンが正規化されなくなることがあります。これが姿勢推定の精度に悪影響を与えるため、以下の点に注意する必要があります。

正規化の重要性

クォータニオンを使用して姿勢を表現する場合、そのクォータニオンは単位クォータニオンである必要があります。単位クォータニオンとは、ノルムが1のクォータニオンです。数値積分を行うと、計算誤差によってクォータニオンのノルムが1からずれてしまうことがあり、このずれを修正するために正規化が必要です。

正規化の手順

クォータニオンのノルムを計算する:
各成分をノルムで割る:

これにより、クォータニオンのノルムが1に保たれ、姿勢の表現が正確に維持されます。

数値積分時の注意点

正規化の頻度:
- 数値積分の各ステップごとにクォータニオンを正規化することが一般的です。これにより、誤差が蓄積する前にクォータニオンが常に単位クォータニオンとして保たれます。
誤差の監視:
- シミュレーション中にクォータニオンのノルムを定期的にチェックし、異常な値が出た場合は適切に修正します。

実装例（Python）

以下に、Pythonでクォータニオンの正規化を含む数値積分の例を示します。

import numpy as np

def normalize_quaternion(q):
    norm = np.linalg.norm(q)
    return q / norm

def quaternion_derivative(q, omega):
    q_dot = 0.5 * np.array([
        -q[1]*omega[0] - q[2]*omega[1] - q[3]*omega[2],
         q[0]*omega[0] + q[2]*omega[2] - q[3]*omega[1],
         q[0]*omega[1] - q[1]*omega[2] + q[3]*omega[0],
         q[0]*omega[2] + q[1]*omega[1] - q[2]*omega[0]
    ])
    return q_dot

def integrate_quaternion(q, omega, dt):
    q_dot = quaternion_derivative(q, omega)
    q = q + q_dot * dt
    return normalize_quaternion(q)

# 初期クォータニオンと角速度
q = np.array([1, 0, 0, 0])  # 単位クォータニオン
omega = np.array([0.1, 0.2, 0.3])  # 角速度ベクトル
dt = 0.01  # タイムステップ

# 数値積分ループ
for _ in range(1000):
    q = integrate_quaternion(q, omega, dt)
    print(q)  # クォータニオンのノルムを監視

この例では、単位クォータニオンを正規化しながら数値積分を行っています。これにより、長時間のシミュレーションでもクォータニオンの正規性が保たれ、正確な姿勢推定が可能になります。

クォータニオンの正規化は数値積分の精度を維持するための重要な手段であり、宇宙機の姿勢制御システムにおいては必須の技術です。