【関連記事#2】宇宙機の姿勢制御パッケージ
共著 @erina_mori, @t-shioya
はじめに
本記事は、Space ROS Advent Calendar 2024の１つです。
Space ROS Advent Calendar 2024の概要については、下記の記事をご参照ください。
本稿ではSpaceROSに関連して現在取り組んでいる（主に軌道上の）宇宙機の姿勢制御パッケージについて、そのコンセプトと活動内容を記載します。
背景
宇宙機に搭載するソフトウェアを開発する上でNASAのcFS/cFEのフレームワークなどがあります。しかしアプリケーションレベル（例えばGNC：Guidance Navigation and Control）に掘り下げたとき宇宙機特有の一品もののソフトウェアになってしまっていると思います。
SpaceROSの公式サイトにも記述があるようにSpaceROSとして開発期間の短縮や再利用性という観点が挙げられており、我々もこの点にフォーカスを当てて、「宇宙機用のROS2パッケージを作ってみたら面白いんじゃないか」というモチベーションで活動をスタートしました。
This will shorten the time for development of novel space robotics capabilities, enable reuse of capabilities between missions, and lower the life-cycle cost of new space robotics missions.
コンセプト
宇宙機の姿勢制御に必要な各機能を抽象化したモジュールとして定義し、ROSとしての強みを活かしたモジュール（ノード）の付替えにより異なるセンサやアクチュエーションを利用してもシステムとして一貫した動作を行うことができるということをコンセプトとしてます。
また、ROSノードとしてモジュール化することにより、ミッション固有の機能についても拡張性を持たせることができます。
これまでの活動内容
Space Week出展
2024年11月に開催された日本橋 Space weekの展示会に出展してきました。
展示会ではポスターでの展示と衛星の姿勢制御の模擬を行うデモンストレーションを行いました。
Raspberry Pi上でROS2ノードとして実装した各ノードを動作させ以下のようなフィードバック制御のデモンストレーションを行いました。
* センサーの入力データを自己姿勢推定のアルゴリズムにInputする。（1）
* 姿勢推定結果と目標姿勢の差異から回転するべき方向とトルクを算出する。
* 算出された値をもとにモーターに対して回転量の指示を行う。
* (1)を繰り返す。
設計詳細
このセクションでは各ノードのデザインの詳細について記載します。（実際に作った人に書いてもらってます。）
センサインプット(@erina_mori)
センサインプットノードは、各ハードウェアから取得したセンサデータを適切に加工し、センサデータトピックとして配信します。
Raspberry Piに接続されたsenseHAT搭載の9軸IMUセンサから、角速度と姿勢クォータニオンを算出しています。
※Raspberry Piにはサンプルコードが用意されており、今回はそちらをもとに計算を行っています。
角速度の算出
センサから取得したジャイロデータの単位変換を行います。
変数
変換パラメータ
-
k_sf
: スケールファクター (単位：deg/s)。 -
k_dr
: deg->rad変換 = 0.0175 (単位：rad/s)。
計算
$$
g_x = g_x \cdot \frac{k_dr}{k_{sf}}
$$
$$
g_y = g_y \cdot \frac{k_dr}{k_{sf}}
$$
$$
g_z = g_z \cdot \frac{k_dr}{k_{sf}}
$$
姿勢クォータニオンの算出
センサから取得したジャイロ、加速度、地磁気データをもとに、姿勢クォータニオンを算出します。
計算フロー概要
-
センサ値の正規化
- 加速度センサ値 (
ax, ay, az) と地磁気センサ値 (mx, my, mz) を単位ベクトルに正規化する。
- 加速度センサ値 (
-
地磁気フラックスの基準方向計算
- 地磁気センサ値をクォータニオンを用いて回転させ、水平成分 (
bx) と鉛直成分 (bz) を計算する。
- 地磁気センサ値をクォータニオンを用いて回転させ、水平成分 (
-
重力と地磁気方向の推定
- クォータニオンから推定した重力 (
vx, vy, vz) と地磁気方向 (wx, wy, wz) を計算する。
- クォータニオンから推定した重力 (
-
誤差の計算
- 推定した方向とセンサ値の差を計算し、各軸ごとの誤差 (
ex, ey, ez) を求める。
- 推定した方向とセンサ値の差を計算し、各軸ごとの誤差 (
-
ジャイロスコープ補正
- 誤差と比例ゲイン (
Kp)、積分ゲイン (Ki) を基に、ジャイロスコープ値 (gx, gy, gz) を補正する。
- 誤差と比例ゲイン (
-
クォータニオンの更新と正規化
- 補正した角速度を用いてクォータニオンを更新し、正規化する。
変数
センサインプット
-
gx, gy, gz
: ジャイロスコープの角速度 (単位：rad/s)。 -
ax, ay, az
: 加速度センサの加速度 (単位：m/s²)。 -
mx, my, mz
: 地磁気センサの地磁気ベクトル (単位：ガウスまたはテスラ)。
クォータニオン
-
q0, q1, q2, q3
: クォータニオン成分。3次元空間の回転を表現。- ( q = [q_0, q_1, q_2, q_3] )
フラックス計算用
-
hx, hy, hz
: 地磁気の基準方向（地磁気センサ値を回転させた結果）。 -
bx, bz
: 地磁気の水平成分 (bx) と鉛直成分 (bz)。
重力・地磁気方向の推定
-
vx, vy, vz
: クォータニオンから推定した重力方向ベクトル。 -
wx, wy, wz
: クォータニオンから推定した地磁気方向ベクトル。
誤差計算
-
ex, ey, ez
: 推定値とセンサ値の誤差。 -
exInt, eyInt, ezInt
: 誤差の積分値。
補正係数
-
Kp
: 比例ゲイン（即時補正用）。 -
Ki
: 積分ゲイン（誤差の累積補正用）。 -
halfT
: サンプリング周期の半分（時間ステップ）。
計算 1: 加速度センサの正規化
$$
\text{norm} = \frac{1}{\sqrt{a_x^2 + a_y^2 + a_z^2}}
$$
$$
a_x = a_x \cdot \text{norm}, \quad a_y = a_y \cdot \text{norm}, \quad a_z = a_z \cdot \text{norm}
$$
計算 2: 地磁気センサの正規化
$$
\text{norm} = \frac{1}{\sqrt{m_x^2 + m_y^2 + m_z^2}}
$$
$$
m_x = m_x \cdot \text{norm}, \quad m_y = m_y \cdot \text{norm}, \quad m_z = m_z \cdot \text{norm}
$$
計算 3: 地磁気フラックスの基準方向
$$
h_x = 2 \cdot m_x \cdot (0.5 - q_2^2 - q_3^2) + 2 \cdot m_y \cdot (q_1q_2 - q_0q_3) + 2 \cdot m_z \cdot (q_1q_3 + q_0q_2)
$$
$$
h_y = 2 \cdot m_x \cdot (q_1q_2 + q_0q_3) + 2 \cdot m_y \cdot (0.5 - q_1^2 - q_3^2) + 2 \cdot m_z \cdot (q_2q_3 - q_0q_1)
$$
$$
h_z = 2 \cdot m_x \cdot (q_1q_3 - q_0q_2) + 2 \cdot m_y \cdot (q_2q_3 + q_0q_1) + 2 \cdot m_z \cdot (0.5 - q_1^2 - q_2^2)
$$
$$
b_x = \sqrt{h_x^2 + h_y^2}, \quad b_z = h_z
$$
計算 4: 重力および地磁気方向の推定
重力方向
$$
v_x = 2 \cdot (q_1q_3 - q_0q_2)
$$
$$
v_y = 2 \cdot (q_0q_1 + q_2q_3)
$$
$$
v_z = q_0^2 - q_1^2 - q_2^2 + q_3^2
$$
地磁気方向
$$
w_x = 2 \cdot b_x \cdot (0.5 - q_2^2 - q_3^2) + 2 \cdot b_z \cdot (q_1q_3 - q_0q_2)
$$
$$
w_y = 2 \cdot b_x \cdot (q_1q_2 - q_0q_3) + 2 \cdot b_z \cdot (q_0q_1 + q_2q_3)
$$
$$
w_z = 2 \cdot b_x \cdot (q_0q_2 + q_1q_3) + 2 \cdot b_z \cdot (0.5 - q_1^2 - q_2^2)
$$
計算 5: 誤差計算
$$
e_x = (a_y \cdot v_z - a_z \cdot v_y) + (m_y \cdot w_z - m_z \cdot w_y)
$$
$$
e_y = (a_z \cdot v_x - a_x \cdot v_z) + (m_z \cdot w_x - m_x \cdot w_z)
$$
$$
e_z = (a_x \cdot v_y - a_y \cdot v_x) + (m_x \cdot w_y - m_y \cdot w_x)
$$
計算 6: クォータニオンの更新と正規化
クォータニオン更新
$$
q_0 = q_0 + (-q_1 \cdot g_x - q_2 \cdot g_y - q_3 \cdot g_z) \cdot \text{halfT}
$$
$$
q_1 = q_1 + (q_0 \cdot g_x + q_2 \cdot g_z - q_3 \cdot g_y) \cdot \text{halfT}
$$
$$
q_2 = q_2 + (q_0 \cdot g_y - q_1 \cdot g_z + q_3 \cdot g_x) \cdot \text{halfT}
$$
$$
q_3 = q_3 + (q_0 \cdot g_z + q_1 \cdot g_y - q_2 \cdot g_x) \cdot \text{halfT}
$$
クォータニオン正規化
$$
\text{norm} = \frac{1}{\sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}}
$$
$$
q_0 = q_0 \cdot \text{norm}, \quad q_1 = q_1 \cdot \text{norm}, \quad q_2 = q_2 \cdot \text{norm}, \quad q_3 = q_3 \cdot \text{norm}
$$
センサトピックは以下のROS2 msgを使用しています。それぞれ対応するセンサデータが格納、配信されています。
-
sensor_msgs/Imu
- orientation: 姿勢クォータニオン
- angular_velocity: 角速度
- linear_acceleration: 加速度
-
sensor_msgs/MagneticField
- magnetic_field: 地磁気
ナビゲーション(@akira_hirota)
ナビゲーションノードとしてはセンサインプットノードがpublishしたセンサデータ（角速度と姿勢クォータニオン）をもとに基準とする系（衛星ではJ2000/赤道座標系などが採用されたりするそうです。）における自身の姿勢を決定します。
実際に姿勢を決定するためにはExtended Kalman filter (EKF) を用いて姿勢センサから出力されるノイズの乗った姿勢をフィルタリングしながら姿勢決定を行います。
以下にナビゲーションノードにおけるEKFの設計を記します。
- State definition
State = \begin{bmatrix} q_x \\ q_y \\ q_z \\ q_w \end{bmatrix}
- Process Model
q_{k+1} = f(q_k, \omega_k) = (I_{4 \times 4} + \frac{1}{2}\Omega_k \Delta T )q_k + w_k
- State Transition Matrix
A_{4 \times 4} = \begin{bmatrix} 0 & \omega_3 & -\omega_2 & \omega_1 \\
-\omega_3 & 0 & \omega_1 & \omega_2 \\
\omega_2 & -\omega_1 & 0 & \omega_3 \\
-\omega_1 & -\omega_2 & -\omega_3 & 0
\end{bmatrix}
- Sensor data
Sensor data = q_{sensor} + \sigma_{noise} =
\begin{bmatrix} q_{x sensor} \\ q_{y sensor} \\ q_{z sensor} \\ q_{w sensor} \end{bmatrix} +
\begin{bmatrix} \sigma_{noise, x} \\ \sigma_{noise, y} \\ \sigma_{noise, z} \\ \sigma_{noise, w} \end{bmatrix}
- Measurement matrix
H = \begin{bmatrix} 1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
- Diagram of the EKF design
- Algorithm
1.Predicted State Estimate
x_{k+1} = f(x_k, u_k)
2.Predicted Covariance of the State Estimate
P_k = F_k P_{k-1}F^T_k + Q_k
3.Innovation or Measurement Residual
y_k = z_k - H(x_k)
4.Near-optimal Kalman Gain
K_k = P_k H^T (H P_k H^T + R)^{-1}
5.Updated State Estimate
x_{k+1|k} = x_{k+1} + K_k y_k
6.Updated Covariance of the State Estimate
P_{k+1|k} = (I - K_k H)P_k
フィルターを実際に動かすと、以下のような動作になります。（青色がフィルター出力、オレンジ色がセンサー出力、緑色が真値）
ここでは意図的にセンサーのデータにノイズを乗せているため真値に対してずれが生じています。フィルターはセンサーデータをゲイン値によって取り込むためオレンジ色の線に引っ張られるような挙動をします。
フィルターの推定結果を信頼性のあるものにするためにはセンサーのノイズ特性や要求される姿勢推定の精度に基づきフィルターのパラメタチューニングを行う必要があります。
- 以下参考とした論文やサイト
（拡張カルマンフィルタを概念的に理解する上で最も参考になったサイトです。）
コントロール(@akira_hirota)
コントロールノードについては特別に何か設計は現時点ではしていません。
ただし今回の展示会では事前の目標とする姿勢を決めておき、ナビゲーションノードの出力する姿勢から目標姿勢に到達するようなPID制御を実装しました。
アクチュエータアウトプット(@t-shioya)
工事中
デモ
これらのノードを組み合わせたときのシミュレーションです。
ここでは目標とする姿勢を1軸周りに+45度方向の姿勢に設定してフィードバック制御の模擬を行っています。
シミュレーションにはPybulletを使っています。
今後について
- 3軸姿勢制御
現在は1軸周りの制御のみを行っていましたが、実際の衛星と同様に今後は3軸の姿勢制御への拡張を進めたいと思っています。 - 軌道系の導入
今回は主に姿勢制御のみをトピックに挙げましたが、当然地球の周りを飛んでいる衛星には自身の位置を推定するために軌道系が必要です。今後は姿勢系だけでなく位置推定のための軌道系アルゴリズムの開発も行っていく予定です。