はじめに
実応用上優れた特性を有する方法として，Virtual reference feedback tuning(VRFT)と呼ばれる方法が提案されています。VRFTは一度実験するだけで所望の制御器を自動獲得可能なオートチューニング手法です。VRFTは様々な利点が存在しますが，国内ではあまり活用されていません。そういった経緯から，過去の記事ではVRFT法を取り上げ、VRFTのどういった部分が優れているのか・制御器調整の原理・運用上での注意点等をいくつか解説しました。これまでは読者の皆様がVRFTを体感いただけるようにPythonを使ったツールやMATLABを使ったツールのサンプルコードを載せて、簡単な数値例を通してPIDゲインを一度の入出力データのみで自動調整できることを解説しました。
MATLABやPYthonが制御系設計の主流ツールであることに代わりませんが，昨今の産業界ではロボティクスや自動運転分野を中心にROSによる開発プラットフォームが急速に普及しつつあります。ROS 2はロボットや自動運転開発に必要な情報がすべてセットになっているソフトウェアプラットフォーム(ミドルウェア)です。具体的には以下の2点により，ROS 2を活用したソフトウェア開発が普及しつつあります。
・オープンソース活用によるアプリケーション開発の効率化
ROSのコミュニティでは，非常に高品質なオープンソースソフトが公開されています。代表例として，Autoware foundationが管理している自動運転オープンソースソフト「Autoware」や移動ロボット用オープンソースソフト「Navigation2」が挙げられます。これらのオープンソースソフトを活用することにより，1からソフトウェアを作成しなくても自動運転やロボットのソフト開発を推進できます。
・ROS 2対応型センサの普及
ROSの大きな利点として，ROSに対応したセンサの販売が普及してきたことも挙げられます。ロボットや自動運転のような複雑なシステムでは，カメラ・LiDAR・IMUのような様々な種類のセンサを使用しなければなりません。これらのセンサにて，ROSの利用者が増えたことでROS対応型のセンサを販売する企業が増えてきています。したがって，ユーザーはROSを活用することで市販のセンサを簡単にロボットに組み込むことができるようになりました。様々なセンサに対応できることもROSの非常に強力な利点といえるでしょう。
ROSの商用利用における様々な欠点を克服した「ROS 2」もリリースされており，ROSによる開発がさらに加速している状況です。まだまだ多くの課題も残されていますが，ROSを活用した開発プラットフォームが今後の主流になる可能性は高いと思われます。ロボットや自動運転等の分野では，低レイヤーの制御則もROSで実装されることも多く，ROS 2に対応したPIDゲイン調整ツールも今後必要となる可能性があります。特に，ROSではrosbagという固有の機能・データ形式で制御対象の入出力データを保存するため，rosbagに対応したゲイン調整ツールであればシームレスなゲイン調整を実現できるでしょう。
以上の背景のもと，著者はROS 2の制御器を対象としたPIDゲイン自動調整ツールを作成しました。本ツールではrosbagにて事前取得したROS 2の入出力データを用いることでPIDゲインを自動調整できます。記事では，ROS 2用PIDゲイン調整ツールの概要と例題によるツール利用手順を解説していきます。なお，本記事で紹介するROS 2ツールはGithub上でコードを公開しているため，こちらも合わせてご確認いただければ幸いです。
VRFTの概要
まず，VRFTの概要を述べておきます。過去の記事を既にご覧の方は飛ばしていただいても問題ありません。下記のフィードバック制御系を考えます。オートチューニングの前提として，規範モデル$T_{d}$をあらかじめ与えます。$T_{d}$は閉ループ系の要求仕様を伝達関数として定量化したものだと考えてください。VRFTでは閉ループ伝達関数を規範モデル$T_{d}$に一致させる制御器$C$を導出することを目指します。
SISOの制御系
まず，開ループ/閉ループ実験のどちらかで制御対象の初期入出力データ(時系列データ)を取得します。次に，初期入出力データを用いて擬似誤差信号$e_{s}$と整形後制御入力$u_{s}$を
u_{s}=L u_{0}
e_{s}=L\left(\frac{1}{T_{d}} y_{0}-y_{0}\right)
と導出します。$u_{0}$と$y_{0}$は初期実験で取得した入出力応答の時系列データです。$L$はノイズ除去やノンプロパーな伝達関数計算を回避するためのプレフィルタです。上記の信号を用いて，VRFTの評価関数を
J_{VRFT}(C)=\left\|u_{s}-C e_{s}\right\|^{2}
と定義します。この評価関数は制御器$C$でパラメタライズされています(厳密な表記ではありませんが，わかりやすさ重視でこのような表記をしています)。何らかの数理最適化手法にて評価関数$J_{VRFT}$を最小化するような制御器$C$のパラメータを探索します。例えば，制御器$C$が評価関数に対して線形関数になれば最小二乗法にて評価関数を最小化できます。評価関数を十分小さくできれば，規範モデルと閉ループ伝達関数を一致させるような制御器$C$を導出できます。VRFTによる制御器更新手順は下記のようにまとめることができます。
1.プレフィルタ$L$と規範モデル$T_{d}$をあらかじめ与える。
2.開ループ/閉ループ実験のどちらかで入出力応答を測定する。
3.$J_{VRFT}$を最小化する制御器$C$のパラメータを数理最適化にて導出する。
VRFTは一組の実験データを測定するだけで未知システムに対する制御器をオートチューニングできます。制御器$C$がPID制御器であれば，最小二乗法にてPIDゲインを導出できます。つまり，最適化における計算コストが非常に低く，パラメータの最適性が保証されます。PIDゲインを最小二乗法で求解できる点は，VRFTの強い利点と言ってもいいでしょう。本記事はROS 2のサンプルコードを中心とした解説なので、VRFT自体の解説は概要レベルに留めることにいたします。詳しい理論や利点を知りたい方がいらっしゃれば，過去の記事で解説しておりますので，そちらをご確認いただければと思います。
参考文献
Campi, M. C., Lecchini, A., & Savaresi, S. M. (2002). Virtual reference feedback tuning: a direct method for the design of feedback controllers. Automatica, 38(8), 1337-1346.
ROS 2によるVRFTを用いたPID自動調整
それでは，本命であるROS 2によるPID制御器調整を解説していきます。冒頭で述べたように，VRFTのROS 2プログラムに関しては，github上で既に公開しています。Readmeの方に最低限の利用手順を記載しており，github上の情報だけ読んでもツールを実行できます。ただ，コードや設計の詳細に関して省略しているため，人によっては少し敷居が高いかもしれません。そういった理由から，本記事ではツールの利用手順だけでなくツールの仕様に関して解説していきます。
開発環境
今回は下記の環境にてツールを作成しました。最新のubuntu・ROS 2のバージョンではありませんが，2024年6月時点では比較的多くのユーザーが利用している環境だと思われます。使用言語として，パッケージの大半をC++で記述しており，launchファイルのみPythonで記述しています。また，なるべく簡単にツールを実行できるようにノード実行用のシェルスクリプトファイルも用意しています。そこまで大規模なソフトでないため，wslの仮想環境でも十分動作すると思われます。
OS : ubuntu 22.04
ROS version : ROS 2 humble
使用言語：C++、Python、shell script
ROS 2のVRFT調整に必要な機能
本ソフトにおける機能の概要について述べます。VRFTによるPIDゲインの自動調整を実施する場合，大まかには下記の2つの機能をツールとして実装する必要があります。
(1)
rosbag形式で保存されている制御対象の入力信号$u_{0}$と出力信号$y_{0}$を受信し，入力信号と出力信号を時刻同期させた上で受信する。
(2)
時刻同期済みの入出力信号の時系列データをも用いてVRFTのコスト関数の解を最小二乗法にて計算し，PIDゲイン調整結果をターミナルに表示する。
(1)に関して，ROS 2では各ノード間で送受信される信号(厳密にはトピック)をrosbagという機能で保存します。ROS 2の標準機能を用いることにより，rosbagで保存された入出力応答を再生してROSのネットワーク上に入出力信号を送信できます。ROS 2では実験データをrosbagのデータとして保存・取り扱うため，rosbagの仕様に対応したVRFTのゲイン調整機能を実装する必要があります。一方，ROS 2では各ノードがイベント駆動で通信・稼働するため，rosbagの入力と出力が別々のトピックのメッセージの場合，入力と出力の時刻がそろっていない可能性があります。したがって，rosbagで再生した入力と出力の信号を時刻同期させた上でVRFTの処理を実施しないと，時刻ズレによる悪影響が発生する可能性があります。
(2)に関して，入出力信号の時系列データを用いてVRFTの解を計算する必要があります。今回はrosbagの入出力応答を再生してVRFTのゲイン調整を実行するため，入出力信号を時系列情報としてバッファにためておく必要があります。バッファにためた信号を時系列情報として最小二乗法を利用することにより，PIDゲイン調整結果をターミナルに表示する必要があります。
ソフト構成の概要
以上の背景をもとに，ROS 2でのPIDゲイン調整ツールを作成しました。こちらのリンク上でgithubのコードとして公開しております。mainブランチをクローンすることで自由にお使いいただけます。
github上で公開済みのツールでは，下記のようなフォルダ構成となっています。mainブランチの上位層にREADMEと実行用シェルスクリプトファイルを置いてあります。
フォルダ構成
src以下の階層に各パッケージのソースコードがおいてあります。
src直下のパッケージ
本ソフトには合計5つのパッケージが含まれています。このうち，vehice_simとvelocity_controlはPIDゲイン調整の機能とは関係ありません。後述の例題向けの制御対象と制御器のサンプルのため，自前のrosbagを使用して動作確認する場合は不要なパッケージです。各パッケージの機能は下記のようになります。
controller_msgs
PIDゲイン調整用入出力信号のためのタイムスタンプ付きカスタムメッセージです。制御入力inputと出力応答outputの変数を内包しています。
time_sync
時刻同期用ノードのパッケージです。rosbagでプレイされた別々のトピックの入力信号と出力信号を時刻同期させた上で受信し，controller_msgs/msg/PlantInfo型の単一メッセージとして入出力信号を後述のpid_tunerへ送信します。VRFTにおけるゲイン調整を実施する前の入出力データの前処理(時刻同期)の役割を果たします。
pid_tuner
PIDゲイン調整用ノードのパッケージです。time_syncからcontroller_msgsの入出力データを受信し，最大データ点数分までバッファをためる。バッファが最大データ点数分保存された場合，PIDゲインの自動調整を実行します。調整後のゲインをターミナル上に表示します。
vehicle_sim(option)
例題用の制御対象ノードのパッケージです。後述のvelocity_controlから制御入力(加速度指令)を受信して，出力(車速)をvelocity_controlへ送信します。PIDゲイン調整機能とは関係ないため，例題を実行しない場合は不要です。
velocity_control(option)
例題用の制御器ノードのパッケージです。後述のvelocity_controlから制御入力(加速度指令)を受信して，出力(車速)をvelocity_controlへ送信します。PIDゲイン調整機能とは関係ないため，例題を実行しない場合は不要です。
このツールをビルドして実行すると，下記のようなノード・メッセージの構成となります。vehicle_sim,velocity_controlのパッケージは例題用のため，下記の図では実行していません。rosbagを再生することにより，入力(画像中のaccel)と出力(画像のvehicle_velocity)がrosbag2_playerノードからROS 2のネットワークへ送信されます。sync_node(time_syncパッケージのノード)は，入力と出力の信号を受信してplant_info(入力と出力を内包したメッセージ)をtuner_nodeへ送信します。tuner_nodeではplant_infoで受信した信号をバッファに保存してPIDゲインを調整します。このようなノードによる処理を実施することにより，rosbagの入出力データのみを用いてPIDゲインをオフライン調整できます。
PIDゲイン調整処理におけるノード・トピック構成
注意事項
ここでは，ROS 2におけるPIDゲインオートチューニングツールの注意事項に関して解説していきます。ソースコード内の細かい処理に関してまで言及できませんが，ユーザーがツールを利用する上での注意事項を可能な限り記載させていただきます。
プレフィルタに関して
今回は実装の都合上，プレフィルタを変更できない仕様となっています。VRFTの先行研究に基づき，本ソフトではVRFTにおけるプレフィルタを
L=T_{d}
として実装しています。本フィルタを実装しているため，本ツールの方法では厳密な意味での最適性は保証されません。一方，先行研究における検証ではVRFTを実システムに応用する場合はほとんどのケースで問題ないことが述べられています。実用上特に問題ないことを確認していますが，プレフィルタの設定を先行研究に基づいて固定化していることにご注意ください。
なお，$L=T_{d}$をプレフィルタとして設定した場合における検証結果は下記の参考文献で述べられています。ご興味のある方はこちらもご確認ください。
参考文献
Formentin, S., Savaresi, S. M., & Del Re, L. (2012). Non-iterative direct data-driven controller tuning for multivariable systems: theory and application. IET control theory & applications, 6(9), 1250-1257.
Yahagi, S., & Kajiwara, I. (2021). Direct tuning of gain-scheduled controller for electro-pneumatic clutch position control. Advances in Mechanical Engineering, 13(8), 16878140211036017.
適用可能な制御器の種類
今回のツールではpid_tunerパッケージのlaunchファイルのパラメータを変更することにより，PID制御器だけでなくPI制御器のゲインも調整できます。シチュエーションに応じてPIゲインのみを調整することができます。ただし，今回のツールでは制御器実装の都合上，積分制御と微分制御を単純な差分方程式として実装しています。不完全微分等の方法はまだ実装できていません。ご自身の環境でツールを利用する場合，制御器の種類によってはpid_tunerのコードを修正する必要があることにご注意ください。
rosbagデータの型に関して
time_syncパッケージでは，入出力をgeometory_msgs/TwistStamped型とgeometory_msgs/AccelStamped型でサブスクライブする仕様となっています。例題以外のrosbagを用意してPIDゲイン調整する場合，time_syncパッケージのサブスクライブ処理と時刻同期処理の型を修正する必要があります。また，topic名に関しても自前のrosbagに合わせてリマップ等で変更する必要があります。逆に言えば、time_syncパッケージのみコード修正すれば，他の型の入出力のrosbagでもPIDゲイン調整ができます。
初期入出力データ(PIDゲイン調整に用いるrosbagデータ)
PIDゲイン調整のアルゴリズム(VRFT)の都合上，制御器調整結果が初期実験データの応答波形に依存します。望ましい制御器調整結果とならない場合，初期入出力応答の再取得やpid_tunerパッケージの最大データ点数(launch上のmax_data_points)変更が必要となります。
また，pid_tunerパッケージでは入出力データバッファの最大データ点数をlaunchファイル上で指定する仕様となっています。最大データ点数分のバッファがたまらない場合，pid_tunerのPIDゲイン調整処理が実行されません。したがって，rosbagに保存されている信号のデータ点数がpid_tunerの最大データ点数(max_data_points)より小さい場合，PIDゲインがターミナルに表示されないままノードが待機状態となってしまいます。この点に関してご注意ください。
規範モデルに関して
本ツールでは実装の都合上，規範モデル$T_{d}$を1次遅れ系
T_{d}=\frac{1}{T_{c}s+1}
で固定化しています。$T_{c}$が規範モデルの時定数となります。ユーザーは規範モデルの時定数のみをlaunchファイルのパラメータとして変更できる仕様となっています。したがって，規範モデルを1次遅れ系以外の伝達関数に設定することができない仕様となっています。ただし，多くのケースでは厳密なモデルマッチング達成を目指さないのであれば，1次遅れ系のような簡単な伝達関数を規範モデルとして設定することが多いとされます。実用上必ずしも問題になるとは限らないため，この点に関してもご注意ください。
例題によるPIDゲイン調整の実施手順
ここでは，例題を通してツールによるPIDゲイン調整の手順を説明していきます。一部の手順はgithubのREADMEと重複しますが，ここでは例題の詳細に関しても説明していきます。
例題の問題設定
例題では，自動車の縦運動ダイナミクスにおける車速制御設計を題材としています。制御入力を加速度指令・出力を車速とした場合，制御対象の伝達関数を
P=\frac{K_{a}}{s(T_{a}s+1)}
と表現できます。$T_{a}$が時定数，$K_{a}$が固定ゲインです。また，制御器を一般的なPID制御器：
C=K_{P}+\frac{K_{I}}{s}+K_{D}s
としました。本例題では目標車速が目標値となっています。目標値は振幅10.0のステップ波とします。PIDゲインを調整することにより，出力(車速)を目標値(目標車速)に収束させるような制御入力(加速度指令)を計算することを目指します。制御対象の伝達関数がvehicle_simパッケージに実装されており，制御器がvelocity_controlパッケージに実装されています。ただし，実際には離散化された伝達関数がvelocity_controlパッケージとvehicle_simパッケージに実装されていますのでご注意してください。
velocity_controlパッケージでは，制御器のPIDゲインをvelocity_controlのlaunchファイルのパラメータ変更にて設定できます。本記事では，初期実験時のPIDゲインを$K_{P}=1.4$,$K_{I}=0.0$,$K_{D}=0.0$と設定しました。この条件にてvehicle_simとvelocity_controlのパッケージのノードを起動することにより，閉ループ系の数値シミュレーションを実施できます。なお，規範モデルの伝達関数を
T_{d}=\frac{1}{1.5s+1}
と与えました。以上の問題設定のもと，ツールを使ってPIDゲインを調整することを目指しましょう。
例題の実行手順
本ソフトではシェルスクリプトにノード起動のコマンドをまとめています。シェルスクリプトを所定の手順で実行することにより，例題におけるPIDゲインの自動調整を実行できます。具体的には，下記の要領で実行していただくことで例題を進めることができます。
1.ビルド
mainブランチをクローンします。ソースファイルのディレクトリに移動後，下記のコマンドを実行してビルドします。
source build_setup.sh
2.初期入出力データの測定(rosbagの準備)
本例題ではlaunchファイル(control_run.py)にて例題の制御器におけるPIDゲインを設定してます。適当な値のPIDゲインをcontrol_run.pyに記載して制御器反映します。PIDゲインが$K_{P}=1.4$,$K_{I}=0.0$,$K_{D}=0.0$となるようにlaunchファイルを修正してみてください。下記のコマンドを実行すると制御器と制御対象のノードが起動し，閉ループ系のシミュレーションが開始されます。シェルスクリプト内にrosbag用のコマンドが含まれているため，入出力データ(rosbag)の記録も開始されます。
source run_velocity_control_sim.sh
3.PIDゲイン調整
シェルスクリプト(source run_PID_tuning.sh)内のrosbag名を該当のファイル名に変更します。launchファイル(pid_tuner.py)にて，規範モデル(time_const)の時定数とデータ数(読み込むデータの最大データ点数max_data_points)を設定します。下記のコマンドを実行するrosbagデータが再生され，PIDゲイン調整用のノードが起動します。
source run_PID_tuning.sh
正常に実行ができた場合，下記のようにターミナル上にてPIDゲインの調整結果が表示されます。
4.ゲイン調整結果の確認
PIDゲイン調整結果を確認するため，再度閉ループ応答をシミュレーションします。launchファイル(control_run.py)のPIDゲインを調整後のゲインに変更し，下記のコマンドを実行して制御器調整後の制御応答を確認します。
source run_velocity_control_sim.sh
以上の手順により，適当な値のPID制御器による閉ループシミュレーションを実施し，そのrosbagデータを用いたゲイン調整を実施できます。
PIDゲイン調整前後の閉ループ応答を確認してみましょう。初期のPIDゲインを使った閉ループシミュレーションの閉ループ応答が下記のようにプロットされます。出力がオーバーシュートしており，時定数1.5の閉ループ応答となっていません。したがって，出力と規範モデルの応答が一致するようにPIDゲインを調整しないといけません。
初期のPIDゲインにおける閉ループ応答(出力波形)
このrosbagでの閉ループ応答データを用いて，VRFTにてPIDゲインを調整してみます。正常なゲイン調整をを実現できた場合，PIDゲインの値を$K_{P}= 0.766$, $K_{I}= 0.0$, $K_{D}= 0.72$と得ることができます。制御器調整後の閉ループシミュレーションの結果が下記になります。閉ループ応答が時定数1.5sの応答となっており，望ましい制御器調整を実現できています。本結果から，rosbagデータとして保存した入出力データのみを用いてPID制御器をオートチューニングできることがわかります。
ゲイン調整後の閉ループ応答(出力波形)
おわりに
本記事では，VRFTと呼ばれる一度実験するだけで所望の制御器を自動獲得可能なオートチューニング手法をROS 2で実装してみました。このROS 2のツールを有効利用すれば，rosbagデータを使ってPIDゲインをオートチューニングできます。ROSユーザーの方はぜひご自身の業務でも活用してみてください。