MATLABを用いた小型ロケットのロール制御系設計

1. はじめに

1.1 ご挨拶

ごめんなさい。全然書き終わりませんでした...

途中までですが、ぜひご覧ください。

 

本記事は、MATLAB/Simulink Advent Calendar 2023の12日目です。

東工大ロケットサークルCREATEが2022年11月に打ち上げました、小型ロケットC-59Jのロール制御系について紹介します。

MATLABを用いて設計したロール制御系について、主に次の3つの観点から記述しました。

  • 制御対象のモデル化とパラメータ推定
  • 制御ゲインの決定
  • 安定性評価

ロケットの機体構造については、ぜひこちらの記事をご覧ください。

tgkhtknk.hatenablog.com

 

1.2 背景

全国にはいくつもの学生ロケットサークルがあり、東京都大島町、秋田県能代市、和歌山県和歌山市などにて、共同打上実験を年に数回実施しています。

規模としましては、全長1~2m程度のロケットを、高度数百~数千mへ打ち上げています。

これまで、ロール制御 (機軸方向まわりの回転角制御) に挑戦したサークルは複数ありますが、制御系の設計法と制御結果について詳しく紹介した例は見当たりません。

ロケットの製作には半年～1年程度を要し、本番は1発勝負であるため、制御ゲインの試行錯誤的な調整ができない難しい環境にあります。

そこで本記事では、成功裡に終えたC-59Jのロール制御系に焦点を当てその詳細を紹介することで、今後ロール制御に挑戦する方々にとって有益な情報になればと思います。

さらに、MATLAB/Simunik界隈の皆様に向け、MATLABを使用して達成した成果を共有したいと思います。

 

2. ロール制御用ハードウェアの構成

図1に、小型ロケットC-59Jのうち、ロール制御に関連するハードウェアの模式図を示します。

ロールモーメントは、2枚の動翼が発生させる揚力により得ます。

2枚の動翼は、傘歯車を介して1個のMAXON社製エンコーダ付きギアモータにより駆動します。エンコーダには512pulse/countの2相インクリメンタルエンコーダを用い、ギアボックスのギア比は29.16です。

この構成により、四逓倍を用いることで出力軸の角度は約0.006度の分解能で計測されます。

 

MCUにはESP32を用います。

ピトー管により計測した対気速度、MEMS 9軸センサにより計測した機体系角速度、エンコーダにより計測した動翼角度などから、適切なモータ電圧指令値を生成し、モータドライバへPWM信号を送信します。

同時に、各センサ値の計測および計測値と制御値の保存を行います。

さらに、機体内から外の景色および動翼の動きを撮影し、ロール制御の成否を定性的に確認するために、2台の小型カメラを搭載します。

 

図1　ハードウェア構成

 

3. 制御対象および制御系のモデル化

3.1 概要

図2に制御対象および制御系を簡易に表したブロック線図を示します。

ただし、目標ロール角は、固定値が与えられるものと仮定します。

与えられた目標ロール角と、姿勢推定器が推定したロール角推定値、およびピトー管を用いて求めた対気速度推定値をもとに、目標動翼角度決定器が目標動翼角度を決定します。

 

次に、目標動翼角度と、エンコーダを用いて計測した動翼角度計測値から、動翼角度制御器がモータに電圧を印加します。

モータ・ギアボックス・傘歯車・動翼から構成されるモータ-動翼系は、印加された電圧に応じて回転します。

このとき、動翼角度および対気速度に応じて発生するロールモーメントが、機体のロール角を変化させます。

 

図2　ブロック線図

3.2 動翼角度制御のモデル化

3.2.1 モータ・動翼系のモデル化とシステム同定

図2において、ロール角のフィードバックループ内に、動翼角度のフィードバックループが存在します。まずは、この部分をモデル化してみましょう。

 

DCモータにおいて、電圧V (s) から角速度Ω (s)までの伝達関数は、式(1)のように二次系で表せることが知られています。今回は、モータ-動翼系のモデル化にこの式を用いることとします。

\displaystyle \frac{\Omega \rm{(s)}}{V \rm{(s)}}=\frac{c}{s^2+as+b} 　(1)
 
ここで、  a, b, c はモータ-動翼系の機械的・電気的特性から決まる固定値です。
  a, b, c は、こちらの記事を参考にしたシステム同定実験により求めました。

sbasami-tech.hatenablog.com

 

図3に、システム同定実験における入力 (電圧) および出力 (角速度) の時間変化を示します。入力は正弦波とし、3 Hzから徐々に100 Hzまで周波数を上げています。

入力の周波数が上がるにつれ、出力の周波数が徐々に小さくなっていることが分かります。

 

図3　モータ-動翼系システム同定実験における入出力の時間変化

 

下記のMATLABコードを用いて、システム同定をしました。MATLABを使えば簡単だなあ...（中身を知らなすぎると危ないけど）

システム同定実験で得られた、1列目がduty比、2列目が角度計測値、3列目が前サンプリングとの時刻差であるcsvファイルを読み込んでいます。

----------------------------------------------------------------------------------------------

dt = 0.002; %サンプリング周期（規定）
data = readmatrix('data.csv'); %csvファイルを読み取る
length = size(data(:, 1), 1); %データの長さ
 
V = data(:, 1) * 9.97; %電圧指令値 duty比にバッテリー電圧を乗算することで計算
theta = data(:, 2); %角度計測値
dt_measured = data(:, 3)/1000/1000; %サンプリング周期（計測値）[μs]から[s]に変換
 
dtheta = zeros(length, 1); %角速度
for loop = 1:length %角度とサンプリング周期から角速度を計算
if loop == 1
dtheta(loop, 1) = 0;
else
dtheta(loop, 1) = (theta(loop, 1) - theta(loop-1, 1)) / dt_measured(loop, 1);
end
end
 
FR.u = V; %電圧が入力
FR.y = dtheta; %角速度が出力
FRdata = iddata(FR.y, FR.u, dt); %IDDATAオブジェクトの作成
FRdata.InputName = 'Voltage';
FRdata.OutputName = 'AngularVelocity';
figure(1), plot(FRdata);
FRdata_det = detrend(FRdata);%データの平均値を0にする
G2 = tfest(FRdata, 2, 0)%極が2、零が0のモデルを同定
figure(2), compare(FRdata, G2, 'r');%モデルの応答比較の表示
----------------------------------------------------------------------------------------------

図4に、同定実験における角速度の測定値 (図3の下段と同じ) 、およびシステム同定結果を用いたシミュレーション結果の角速度の時間変化を示します。（上記コードで得られるfigure(1)です。）

赤色の線は、システム同定実験における入力電圧(図3の上段)を、システム同定結果に基づくモデルに入力したときの応答をシミュレーションしたものです。

いい感じに同定できてそうですね。

  a=1462, b=53850, c=218100と求まりました。

 

図4　システム同定実験および同定結果を用いたシミュレーションの角速度

3.2.2 動翼角度制御器のモデル化

今回は、PD制御を用いて動翼の角度制御を行います。
比例ゲインと微分ゲインをそれぞれ  K_{Pm}, K_{Dm}とおきます。

簡単のために下記の仮定をおくと、動翼角度のフィードバックループは、図5のように表されます。

・動翼角度は誤差なく計測できる

・モータドライバの電圧制御は正確かつ時定数が小さく、モータ電圧指令値とモータ電圧は等しいとみなせる

 

図5　動翼角度フィードバック系のブロック線図


3.2.3伝達関数の導出とボード線図

図5の動翼角度フィードバック系について、伝達関数とボード線図を確認します。

開ループ伝達関数 G_{om}(s)は、式(2)で表されます。

\displaystyle G_{om}(s)=\frac{csK_{Dm}+cK_{Pm}}{s^3+as^2+bs}　(2)

一方、閉ループ伝達関数 G_{cm}(s)は、式(3)で表されます。

\displaystyle G_{cm}(s)=\frac{csK_{Dm}+cK_{Pm}}{s^3+as^2+(b+cK_{Dm})s+cK_{Pm}}　(3)

 

MATLABを使えば、簡単にボード線図が書けて、位相余裕とゲイン余裕も分かっちゃいます。ボード線図で位相余裕とゲイン余裕を確認しつつ、後述(まだ書けてません)する時系列シミュレーションで応答を確認して、PDゲインを決めました。

----------------------------------------------------------------------------------------------

a = 1462; b = 53850; c = 218100; %モータ-動翼系のシステム同定結果
Kpm = 240; %Pゲイン
Kdm = 1.5; %Dゲイン
 
%開ループ伝達関数のボード線図
[A, B, C, D] = tf2ss([c*Kdm c*Kpm], [1 a b 0]); %伝達関数を状態方程式に変換
sys_o = ss(A,B,C,D); %状態空間モデルの作成
figure(1),margin(sys_o) %ボード線図を表示, 位相余裕とゲイン余裕も表示
grid on %グリッドを入れる
 
%閉ループ伝達関数のボード線図
[A, B, C, D] = tf2ss([c*Kdm c*Kpm], [1 a b+c*Kdm c*Kpm]); %伝達関数を状態方程式に変換
sys_c = ss(A, B, C, D);%状態空間モデルの作成
figure(2),bode(sys_c) %ボード線図を表示
grid on %グリッドを入れる
----------------------------------------------------------------------------------------------
上記のコードを実行すると、図6および図7が得られます。
 
まず図6の開ループ伝達関数を見てみましょう。
margin関数を用いると、ボード線図と同時にゲイン余裕と位相余裕も得られちゃいます。
位相余裕は56.4度、位相が-180度以下にならないためにゲイン余裕は無限大となっています。
位相余裕、ゲイン余裕ともに充分でしょう。

図6　動翼角度フィードバック系の開ループ伝達関数のボード線図
 
次に、図7の閉ループ伝達関数を見てみましょう。
ある程度の周波数まで振幅・位相ともに0付近であり、その後どちらも下がっていく理想的な形ですね。
振幅が-3 dBになる周波数は395 rad/s ≒ 63 Hzです。このくらい高い周波数まで追従可能なら、安いPWMサーボモータを買うのではなく、MAXONモータを使って制御した意味があると言えるのでは（テキトー）

図7　動翼角度フィードバック系の閉ループ伝達関数のボード線図

3.3 全体のモデル化

3.3.1 機体系のモデル化とパラメータ推定
3.3.2 目標動翼角度決定器のモデル化
3.3.3 伝達関数の導出とボード線図

4. シミュレーションと事前調整

4.1 動翼角度制御のシミュレーションおよび実験

4.2 ロール制御のシミュレーション

4.3 計測誤差の考慮

5. ロール制御実験

5.1 実験条件

5.1.1 飛行プロファイル

図8に飛行プロファイルを示します。横軸は離床を基準とした時刻、縦軸は気圧センサのデータから算出した高度です。

C-59Jは、エンジン燃焼開始の直後に離床し、離床後約2.7秒で燃焼終了しました。

その後は約7秒間の弾道飛行の後、最高高度の約1秒後にパラシュートを開放し、減速落下しました。

 

安全性に配慮した自主的な規制により、ロール制御はエンジン燃焼終了後に開始することとしました。また、パラシュート開放後は大きな対気速度が得られず、動翼がロールモーメントを発揮できないため、ロール制御が成り立ちません。

よって、ロール制御は弾道飛行中にのみ、約7秒間実施しました。

 

図8　飛行プロファイル

5.1.2 パラメータ設定

動翼角度制御における制御ゲインは、  K_{Pm}=240, K_{Dm}=1.5 とし、

目標ロールモーメント生成における制御ゲインは  K_{Pa}=0.8, K_{Da}=0.07 としました。

 

また、ESP32への実装にあたっては、ノイズの影響を低減するため、制御に用いる計測値の一部にデジタルローパスフィルタを施しました。

ピトー管から算出した対気速度には、カットオフ周波数20 Hzの1次のデジタルローパスフィルタを施しました。

MEMS角速度センサから算出したロール角速度には、カットオフ周波数50 Hzの1次のデジタルローパスフィルタを施しました。

5.1.3 初期値

5.2 実験結果

5.2.1 動画による確認

実験結果を、まずは動画で定性的に確認しましょう。

右側が機体に搭載した小型カメラを用いて撮影した映像です。映っている黒い板が動翼です。

中央上が、MEMSジャイロセンサを用いて計測した機体姿勢を、CGで再現した映像です。中央下が、ロール角のグラフです。

左側上が、エンコーダで計測した動翼角度を、CGで再現した映像です。左側下が動翼角度のグラフです。

 

離床すると、C-59Jはロールしています。これは、機体後端にある固定翼のミスアラインメント、ノーズ形状の非対称性等により発生した、外乱ロールモーメントの影響と考えられます。

離床の約2.7秒後にロール制御が開始され、動翼が動き始めます。その後すぐに、ロールがほぼ停止していることが分かりますね！

 

5.2.2 計測データによる確認

図9に、ロール角の時間変化を示します。

青色で示したロール制御開始前は、ロール角が変化していることが分かります。

一方、ロール制御開始後は、素早く目標ロール角に収束しています。

図9　ロール角の時間変化。ロール制御開始前を青色、開始後を赤色で示す。

 

図10に、ロール制御開始直後の、ロール角の時間変化を示します。

整定時のロール角を90度とみなすと、整定時間 (95%) はたったの0.27秒でした！

 

図10　ロール制御開始直後におけるロール角の時間変化

 

図11に、ロール制御開始からパラシュート開放付近までの、ロール角の時間変化を示します。

制御開始直後を除き、目標ロール角からの偏差が±4度以内に抑えられていることが分かります。

図11　ロール制御開始からパラシュート開放付近までのロール角の時間変化

 

図12に、動翼角度および目標動翼角度の時間変化を示します。
動翼角度は目標値に精度よく追従していることが分かります。

モータ軸に取り付けたエンコーダで計測しているので、ギアボックスおよび傘歯車のバックラッシが計測できない誤差として存在はしていますが。

図12　動翼角度および目標動翼角度の時間変化。動翼角度は、ロール制御開始前を青　
色、開始後を赤色で示す。目標動翼角度は黒色で示す。

 

6. おわりに

肝心なMATLABをこんな感じに使ったよ～の部分が書きおわっておらず、ロケットのロール制御できたよ～状態で申し訳ないです。

2023年中には絶対書き終えます。