係数図法による姿勢制御パラメータチューニング

# はじめに
この記事は東京大学学術リポジトリの以下の論文を参考に作成しました.
久木「残留磁気モーメント推定を用いた磁気トルクのみによる小型衛星の姿勢制御」(Fully Magnetic Attitude Control for Spacecraft with Estmation of Residual Magnetic Moment)[1]
当該論文のリンクは[こちら](https://repository.dl.itc.u-tokyo.ac.jp/record/1766/files/37056434.pdf)

# 背景・目的
人工衛星の姿勢制御を考えるとき,ゲインチューニングってかなりめんどくさいと思うので何かいい方法はないか模索していたところ当該論文を見つけたので共有します.
理論を理解できておらず,やってみた！以上でも以下でもないので期待しないでください...

# 本題
## 係数図法
- 聞きなじみのない名前だったので調べました.
- 佐藤,近藤,古賀[「係数図法を用いた制御系設計ツールの開発」(JAXA AIREX)](https://repository.exst.jaxa.jp/dspace/handle/a-is/43140#:~:text=%E4%BF%82%E6%95%B0%E5%9B%B3%E6%B3%95%E3%81%AF%E3%80%81%E4%BF%82%E6%95%B0%E5%9B%B3,%E7%B3%BB%E8%A8%AD%E8%A8%88%E6%89%8B%E6%B3%95%E3%81%A7%E3%81%82%E3%82%8B%E3%80%82)[2]
> 	係数図法は、係数図(特性多項式の係数を片対数グラフ上に表現した図)の形状が制御系の特性(安定性/速応性/ロバスト性)を明確に表現していることを利用した制御系設計手法である。

- JAXA論文にも出てきた真鍋さんが書かれたであろうPDF
    - 真鍋,金永[係数図法 制御系設計の理論と応用](http://www.cityfujisawa.ne.jp/manabes/pdfJapanese/CH01mainChapJ.pdf)[3]

## PIDパラメータの決定法
理論をそこまで詳細に追えていないのですが,重要部分だけ抜粋します.

設計する対象は,以下の図に示すような制御系です.
![スクリーンショット 2025-01-07 233606.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3635973/3fa130d2-463c-9df0-a96a-d290fde21273.png)
(参考：[1]図5.1)

> $\tau_lpf$ ローパスフィルタのパラメータ
> $k_p$ PID制御のPゲイン
> $k_i$ PID制御のIゲイン
> $k_d$ PID制御のDゲイン
> $I_{xx}$ 慣性モーメント

端的に言い表せば「1次遅れ系のシステムに対して、前端にローパスを付けたPID制御系で設計する。」というものです.
当該論文では,磁気トルカでの姿勢制御を前提としており,偏差の急激な変化にも緩やかに対応できるようにしたものであるとのこと.
ということで,制御する際には**4つのパラメータ**を調整する必要があります.
しかし,係数図法を用いることで**1つのパラメータ**の調整だけで制御系の設計ができるようになります!

以下の感じでパラメータを設定するといい感じになるらしいです.
(係数図法を追えていないのであれですが。)
```math
K_p = \frac{25I}{2\tau^2}　
K_i = \frac{25I}{2\tau^3}　
K_d = \frac{5I}{\tau}　
\tau_{lpf} = \frac{\tau}{10}　
```
$\tau$さえ決めてしまえばうまくいくということです.これを**等価時定数**と呼ぶそうです.

## やってみる！
これで実装できちゃえば楽そうだなーと思ったので$\tau$を変化させたときに応答がどのように変化するのかを示すソースコードをChatGPTに作らせました.

```python:CDM_for_Attitude.py
import numpy as np
import matplotlib.pyplot as plt

# コントローラーのパラメータ（慣性モーメント）
I = 1.0  # 慣性モーメント

# シミュレーションのパラメータ
dt = 0.1  # タイムステップ
T_end = 100  # シミュレーション時間

# 初期値
theta_ref = 0.0  # 目標角 (0 degrees)
theta_init = -1.0  # 初期の角度
theta_dot_init = 0.0  # 初期の角速度

# 比較したい tau 値のリスト
tau_values = [10.0, 20.0, 30.0, 40.0, 50.0]  # 任意の tau 値を指定

# 角度の正規化関数 (-180度 ～ +180度 に制限)
def normalize_angle(theta):
    return (theta + np.pi) % (2 * np.pi) - np.pi  # ラジアン単位で正規化

# シミュレーション関数
def simulate_system(tau):
    # PIDゲインの計算
    kp = 25 * I / (2 * (tau ** 2))  # 比例ゲイン
    ki = 25 * I / (2 * (tau ** 3))  # 積分ゲイン
    kd = 5 * I / tau  # 微分ゲイン
    tau_lpf = tau / 10  # ローパスフィルタの時定数

    # シミュレーション時間の設定
    time = np.arange(0, T_end, dt)

    # 初期条件
    theta_now = theta_init
    theta_dot = theta_dot_init
    integral = 0.0
    prev_error = 0.0
    prev_output = 0.0

    # ログ
    theta_log = []
    theta_dot_log = []
    control_log = []

    # シミュレーションループ
    for t in time:
        # 誤差
        error = theta_ref - theta_now

        # PID コントローラ
        proportional = kp * error
        integral += ki * error * dt
        derivative = kd * (error - prev_error) / dt
        control_output = proportional + integral + derivative
        #control_output = np.clip(control_output, -1.0, 1.0)  # 出力を-1.0〜1.0に制限

        # ローパスフィルタ
        filtered_output = (tau_lpf / (tau_lpf + dt)) * prev_output + (dt / (tau_lpf + dt)) * control_output
        prev_output = filtered_output

        # プラントモデル (角速度 -> 角速度の更新)
        torque = filtered_output  # 制御出力をトルク入力として利用
        theta_dot += torque / I * dt  # 角加速度を積分して角速度を更新
        theta_now += theta_dot * dt  # 角速度を積分して角度を更新

        # 角度の正規化
        theta_now = normalize_angle(theta_now)  # 角度を-π～+πの範囲に制限

        # ログ
        theta_log.append(np.rad2deg(theta_now))  # 角度を度数で保存
        theta_dot_log.append(theta_dot)
        control_log.append(torque)

        # 次のステップのために更新
        prev_error = error

    return time, theta_log, theta_dot_log, control_log

# プロット
plt.figure(figsize=(10, 8))

for tau in tau_values:
    time, theta_log, theta_dot_log, control_log = simulate_system(tau)

    # 角度応答をプロット
    plt.subplot(3, 1, 1)
    plt.plot(time, theta_log, label=f'θ_now (tau={tau})')
    plt.axhline(y=theta_ref, color='r', linestyle='--', label='θ_ref' if tau == tau_values[0] else "")
    plt.title('Angle Response')
    plt.xlabel('Time [s]')
    plt.ylabel('Angle [deg]')
    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left', borderaxespad=0.)

    # 角速度応答をプロット
    plt.subplot(3, 1, 2)
    plt.plot(time, theta_dot_log, label=f'Angular Velocity (tau={tau})')
    plt.title('Angular Velocity Response')
    plt.xlabel('Time [s]')
    plt.ylabel('Angular Velocity [rad/s]')
    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left', borderaxespad=0.)

    # 制御出力(トルク)をプロット
    plt.subplot(3, 1, 3)
    plt.plot(time, control_log, label=f'Torque (tau={tau})')
    plt.title('Control Output (Torque)')
    plt.xlabel('Time [s]')
    plt.ylabel('Torque [Nm]')
    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left', borderaxespad=0.)

plt.tight_layout(rect=[0, 0, 0.9, 1])  # 右側にスペースを空ける
plt.show()

```
ここで例の等価時定数の計算をしています

    kp = 25 * I / (2 * (tau ** 2))  # 比例ゲイン
    ki = 25 * I / (2 * (tau ** 3))  # 積分ゲイン
    kd = 5 * I / tau  # 微分ゲイン
    tau_lpf = tau / 10  # ローパスフィルタの時定数

等価時定数を変更したいときはここをいじればok

    # 比較したい tau 値のリスト
    tau_values = [10.0, 20.0, 30.0, 40.0, 50.0]  # 任意の tau 値を指定

※大きな等価時定数を扱うときはシミュレーション時間を長くとってください.

実行結果は以下の通りになるはずです.
![Figure_250108.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3635973/7f9e9ff4-f4fb-b56a-3cd6-47e76af3905d.png)

等価時定数$\tau$を変更すると制御の時定数が変化する様子を見ることができます.

# まとめ
## わかったこと
- チューニングするパラメータの数を減らすことができる
    - 各パラメータを0からチューニングするよりかなりマシ
## わからないこと
- どうやって設計するのか
    - 係数図法そのものが分かっていない
    - 安定性や追従性の評価など