mbedとrosでドローンを飛ばす(8)~matlabによるシステム同定~

こんにちは。たけぴよです。今回はmatlabを使ってドローンのシステム同定を試してみました。
システム同定について深く理解するよりも早く飛ばせるようになることを優先したかったため、理論的なことについては理解しておらず紹介はしていません。
結果的にうまくできなかったのですが、記録として残します。

~余談~
この記事は自分が一年前くらいに下書きとして保存していたものです。
せっかくドローンの開発をqittaにまとめていたのに、最後まで書ききらずに終わってしまうのはもったいないと思い投稿させていただきます。

システム同定について

システム同定とは実験データに基づくモデリング方法です。対象をブラックボックスとみなし、その入出力データから統計的な手法を用いてモデリングを行います。複雑なシステムに対しても実験データさえ作ることができればモデルを得ることができます。
統計的な処理はMATLABのSystem Identification ToolBoxで行います。
正確な対象のモデルを得ることができれば、ドローンの動きをシュミュレーション上で再現することができるので制御器の設計を行う時に便利になります。(PID制御器のゲインの調整をpidTuner等でできるようになる)

MIMOモデルからSISOモデルへの変換

ドローン(クワッドコプター)は以下のように多入力多出力(MIMO)なシステムです。様々な出力の中から必要な情報をセンサーを用いて入手します。

多入力多出力なシステムを同定するのは一般的に難しいので、ドローンには連成がないことを仮定して単入力単出力なシステムに変換することを考えます。(ドローンが小さくて軽い場合は連成無しを仮定しても問題ないみたいです)

こちらの解説のように、ドローンの姿勢は各モーターの出力のバランスで決まるため、以下のように表された入力を用いれば、システムを単入力単出力に分離することができます。

\begin{bmatrix}
z_{control}　\\
roll_{control} \\
pitch_{control} \\
yaw_{control} \\
\end{bmatrix}
= M
\begin{bmatrix}
duty_{fr} \\ 
duty_{br} \\
duty_{bl} \\ 
duty_{bl}
\end{bmatrix} \\
M = 
\begin{bmatrix}
1 & 1 & 1 & 1 \\
1 & 1 & -1 & -1 \\
1 & -1 & -1 & 1 \\
1 & -1 & 1 & -1
\end{bmatrix}

今回フィードバック制御を行うロール角とピッチ角についての入出力データ$$roll_{control}: roll$$$$pitch_{control}: pitch$$　を時間と共に記録すればシステム同定が行えそうです。

入力系列について

システム同定を行う際は対象の動特性を詳しく調べために、できるだけ変動する入力を与える必要があります。どれくらい信号が多数の周波数成分を含んでいるかを表す指標をPE性と言います。PE性の観点からは白色雑音が望ましいのですが、それは作ることができないため、M系列と呼ばれる入力を使用します。
M系列はmatlabのツールボックスを使って以下のように生成できます。生成したM系列をそのままpublishしています。

M_series.m

duty_low = 0.20;
duty_high = 0.43;
length = 25;

pnSequence = comm.PNSequence('SamplesPerFrame',length);

duty0 = (duty_high - duty_low) * pnSequence() + duty_low;
duty1 = (duty_high - duty_low) * pnSequence() + duty_low;
duty2 = (duty_high - duty_low) * pnSequence() + duty_low;
duty3 = (duty_high - duty_low) * pnSequence() + duty_low;

rosinit('http://localhost:hogehoge');

% geometry_msgs だと何故かpublishできなかったためstd_msgs を使用しています。
duty_0_pub = rospublisher('/input_duty_0', 'std_msgs/Float32');
duty_1_pub = rospublisher('/input_duty_1', 'std_msgs/Float32');
duty_2_pub = rospublisher('/input_duty_2', 'std_msgs/Float32');
duty_3_pub = rospublisher('/input_duty_3', 'std_msgs/Float32');

msg_0 = rosmessage(duty_0_pub);
msg_1 = rosmessage(duty_1_pub);
msg_2 = rosmessage(duty_2_pub);
msg_3 = rosmessage(duty_3_pub);

for i = 1:length
    msg_0.Data = 0.0;
    msg_1.Data = 0.0;
    msg_2.Data = 0.0;
    msg_3.Data = 0.0;
    send(duty_0_pub, msg_0);
    send(duty_1_pub, msg_1);
    send(duty_2_pub, msg_2);
    send(duty_3_pub, msg_3);
    pause(0.1);
end

for i = 1:length
    msg_0.Data = 0.3;
    msg_1.Data = 0.3;
    msg_2.Data = 0.3;
    msg_3.Data = 0.3;
    send(duty_0_pub, msg_0);
    send(duty_1_pub, msg_1);
    send(duty_2_pub, msg_2);
    send(duty_3_pub, msg_3);
    pause(0.1);
end

for i = 1:length
    msg_0.Data = duty0(i,1);
    msg_1.Data = duty1(i,1);
    msg_2.Data = duty2(i,1);
    msg_3.Data = duty3(i,1);
    send(duty_0_pub, msg_0);
    send(duty_1_pub, msg_1);
    send(duty_2_pub, msg_2);
    send(duty_3_pub, msg_3);
    pause(0.1);
end
for i = 1:length
    msg_0.Data = 0.0;
    msg_1.Data = 0.0;
    msg_2.Data = 0.0;
    msg_3.Data = 0.0;
    send(duty_0_pub, msg_0);
    send(duty_1_pub, msg_1);
    send(duty_2_pub, msg_2);
    send(duty_3_pub, msg_3);
    pause(0.1);
end

rosshutdown;

duty_hoge についてM系列を適用しましたが、hoge_controlそのものにM系列を適用したほうが良いかもしれないです。

System Identification Tool Box によるシステム同定

mbedからpublishしてrosbagで記録した、入力と出力データを使ってシステム同定をします。
入力と出力の時刻を補完によって合わせます。

reference_freq = 13;

delta_t = 1 / 13;

[resampled_output_roll, resampled_output_time] = resample(output_x, output_time, reference_freq);
[resampled_output_pitch, resampled_output_time] = resample(output_y, output_time, reference_freq);

[resampled_input_roll, resampled_input_time] = resample(input_roll, input_time, reference_freq);
[resampled_input_pitch, resampled_input_time] = resample(input_pitch, input_time, reference_freq);

plot(resampled_output_time,resampled_output_roll,resampled_input_time, resampled_input_roll)
figure()
plot(resampled_output_time,resampled_output_pitch,resampled_input_time, resampled_input_pitch)

この補完がシステム同定の精度にどう影響を与えてしまうのかを調べたほうが良さそう。

微妙そうですね。。

とりあえずこのデータを使って
https://jp.mathworks.com/help/ident/gs/identify-linear-models-using-the-gui.html
に従いモデルを作成しました。

PID調整器

これはピッチについて調整した結果ですが、なんだかダメそうですね。

一応PIDコントローラーを実装し、ロールピッチについて以下のようにフィードバック制御をしてみました。

Iゲインが大きいため、センサーのノイズの誤差がたまり制御量が極端に小さくなったり大きくなったりしてしまいます。システム同定が上手く行っていないことが原因で妥当なPIDゲインを選べていないことが原因だと考えられます。PD制御にしてもだめでした。

考えられる原因や改善案など

• そもそもちゃんと制御しないと安定しないシステムに、M系列のような入力を行うと動作がはちゃめちゃになります。(部屋で飛ばしたときはめちゃくちゃ怖かったです。。笑)そのせいで機体の端が床に触れるなどの誤差が無視できないほど大きくなった(なるべく綺麗そうなデータを採用しましたが)と考えられます。
• ドローンの機体の完成度がイマイチ(重心が厳密に中央にはなかった)
• 実際に慣性モーメントなどのパラメータを測定したり、グレーボックスモデルの同定を行ったりする方法も試すべきだった。
• とりあえずホバリングを目指していたので、ドローンの非線形モデルを線形化する方法も試すべきだった。

手動でPDゲイン調整

システム同定で正しいモデルを得られないので、モデルが必要ない手動でのPD制御を目指しました。

とりあえず宙には浮きましたが、完全な静止はできませんでした。↓動画

去年宙に浮いたやつを供養しとく pic.twitter.com/LHNeeH572i

— たけし (@NibiJim) January 27, 2022

上記の原因の他に、制御周期が13Hzしか出せていないことも原因だと考えられます。
ゲイン調整を詰めることでどうしてもホバリングさせたかったのですが、墜落を繰り返すうちにモーターが一つ壊れ、そのモーターは販売していなかったのであきらめました。

まとめ

何もロボットを作ったことがなく、授業で制御工学や宇宙工学をかじった程度だった自分にとって、この開発は非常に勉強になりました。特にカルマンフィルタやROSを実際に使うことができてよかったです。ホバリングまでさせることは叶いませんでしたがまた時間をみつけて再びチャレンジしてみたいと思っています。
またホバリングだけでなく深層強化学習などの手法を姿勢制御や経路生成、経路追従に使うなどして、自律移動ドローンを作りたいです。
Jetson nanoを載せられる機体↓はこれも一年前くらいに作ったので試したいところではあります。こちらは研究と折り合いをつけるか、研究と関連させるかなどして取り組みたいです。

部品少し組み立ててみたけどそれ感出てる☺︎
動かせるのはまだ先だけど楽しみや pic.twitter.com/xJCWcgjvCh

— たけし (@NibiJim) March 13, 2021

この一連の投稿が誰かのお役立てたら自分はとっても嬉しいです。ここまで読んでいただき、ありがとうございます。

参考

• System Identification Toolbox
• ドローン工学入門-モデリングから制御まで
• マイクロマウスのためのシステム同定