acadosでモデル予測制御の設計とコード生成

はじめに

昨今の制御界隈ではMPC（モデル予測制御）を実製品に適用することが珍しくなくなってきました。
実際、自動運転やロケット、ロボティクスなどあらゆるシーンで使用されています。

↑TRI-AD（現ウーブン・バイ・トヨタ）では、スープラを改造した試験車を使い、極限走行時（ドリフト）における車両の運動制御を非線形MPCを使ってリアルタイムに行う自律システムを開発し、実験を行った。

↑Tesla AI Day2022からの抜粋。Teslaの運転支援システムFSDでは車両の経路計画においてMPCが使われている。

↑SpaceXの再使用可能なロケット"Falcon9"は、ブースターの帰還着陸においてMPCを使い、機体の誘導を行っている。

↑SpaceXにて着陸制御技術のマネージャを務めるLars Blackmore氏が執筆したThe Bridgeというジャーナルにてリアルタイムな凸最適化アルゴリズムを利用している旨が言及されている。

過去にも関連した記事を書いているので良ければご覧ください。

MPCを設計できるツール

そんなMPCですが、現在はフリー、有償含め様々な設計ツールが登場しており選択肢が豊富にあります。
その中で代表的なものを以下に記載します。

MATLAB ~Model Predictive Control Toolbox~

有償で代表的なものとしては、MATLABの1ツールボックスとしてModel Predictive Control Toolboxがあります。

このToolboxではMATLABを使ってMPCの設計を行うことができ、Simulinkを使った制御シミュレーションやSimulink Coderを使った自動C/C++コード生成に対応しており、MATLABのツールチェーン内で取り組めるメリットがあります。また線形なMPC、適応的なMPC、非線形なMPC（NMPC）とさまざまなタイプのMPCを提供しています。

また外部の有償ソルバーEmbotech社のFORCESPROと連携することで、組み込み用に最適化されたソルバーコードを生成することができ、特にNMPCのソリューションにおいては演算能力を強化することができます。

CasADi

フリーの開発環境として、CasADiは有名です。CasADiはシンボリック変数を使った数理モデリングならびにMPCをはじめとする数理最適化の設計を支援するプラットフォームとして活用可能なオープンソースであり、PythonやMATLABとのインターフェースを備えています。

また解説書として以下の本もあるため、導入しやすい環境が整っています。

acados

acados は CasADi をベースとしたオープンソースでリアルタイム性を重視したNMPCを実装するためのソルバフレームワークを提供します。SQP-RTI（Sequential Quadratic Programming – Real-Time Iteration）手法を採用し，各制御周期において最適解を高速かつ決定的に更新することを可能にしています。

またCasADiと同様にPythonやMATLAB/Octaveで使用可能なインターフェイスAPIを使用でき、Simulinkへの実装ならびにモデルからの自動コード生成にも対応可能なソリューションを提供しています。

ちなみに、似た名称のACADO Toolkitというのも存在します。University of FreiburgのMoritz Diehl教授をはじめとするコミュニティで開発されたもので、acadosの前身となるものです。

acadosを使ってNMPCを設計する

今回はacadosを使ってNMPCを設計してみます。
設計及び検証環境にはMATLAB R2024bおよびSimulinkを活用し、APIを活用しながらNMPCの設計及びモデルによる閉ループでのシミュレーション検証、そしてモデルからのコード生成とビルドを通して行います。

本記事に記載するものは記載日時点で動作を確認できたものであり、将来においてはその限りではありません。

まず、acadosを使い始めるために環境のセットを行います。
詳細はacadosのドキュメンテーションに沿って実施していきます。
今回はWin11かつMATLABの環境を使用するため、以下のドキュメントを参照しました。

ドキュメントにも記載ありますが、acadosを利用するためには

・CMake
・minGW

も併せて必要になるため、インストールの上、環境変数にセットしておく必要があります。

導入については先駆者の方がいたので以下の記事も参考になりました！

導入ができたらいよいよMPCを設計していきます。

MATLABを使った方法については以下のページに情報がまとまっています。

特に定式化の考え方やそれにリンクする各種設定については以下のPDFに情報の詳細が載っています。

PDFより抜粋

acadosの設計手順はおおまかに以下の流れになります。

① CasADi を用いてシステムの予測モデル（状態方程式）を記述する
② CasADi で定義したモデルを AcadosModel に登録する
③ AcadosOcp を用いて，ホライズン長，コスト関数，制約条件などを含む NMPC 問題を定式化する
④ get_acados_simulink_optsでSimulinkブロックを生成するためのオプションを指定する
⑤ AcadosOcpSolver により，定義した NMPC 問題に対応するソルバコードを生成し，ソルバーインスタンスを作成する

以上を踏まえた上で、具体例と共に設計を行ってみます。

着陸船の高度制御

NMPCを用いた設計の例題として、下図に示す着陸船の高度制御を考えてみたいと思います。

着陸船の運動モデルはご覧の通り、推力$F$の発生に伴って燃料が消費されるために質量$m$が時々刻々と変動する非線形なシステムとなっています。
今回の問題では、限られた燃料のうちで所定高度から地面に向けて減速しながら降下するためにどのように推力を操作するかをNMPCによってリアルタイムに解かせます。

NMPCの設計

% acadosのWindos環境をセットアップするAPIを呼び出す
acados_env_variables_windows;

% CasADi+acadosで予測モデルを設計
import casadi.*

% ====予測モデル====
g = 9.81;              % 重力
dry_mass = 300;        % 乾燥時機体質量
m_fuel = 200;          % 燃料質量
m0 = dry_mass+m_fuel;  % 全備時質量
thrustMax = 3*m0*g;    % 最大推力
Isp = 350;             % 比推力

h    = SX.sym('h');    % 高度
v    = SX.sym('v');    % 速度
m    = SX.sym('m');    % 質量
Tth  = SX.sym('Tth'); % 推力のスロットリング率 0-1で操作

x = [h; v; m];         % 状態量のベクトル

m_safe = fmax(m, dry_mass);
xdot = [ v;
         thrustMax*Tth/m_safe - g;
        -thrustMax*Tth/(Isp*g) ];

% AcadosModelに設定
model = AcadosModel();
model.name        = 'rocket1d';
model.x           = x;                        % 状態量
model.u           = Tth;                      % 操作量
model.xdot        = SX.sym('xdot', 3, 1);     % 状態量の微分
model.f_expl_expr = xdot;                     % 状態方程式のExplicit表現の指定 xdot = f(x,u)

% ====NMPCの設計（Optimal Control Problem : OCP）====
ocp = AcadosOcp();
ocp.model = model;

% ==== 参照軌道の仮置き ====
% y = [x; u] → 次元 3 + 1 = 4
ocp.cost.yref   = [0; 0; 0; 0];   % ステージ中の参照値 [x_ref; u_ref]
ocp.cost.yref_e = [0; 0; 0];      % 終端ステージでの参照値 x_ref_e

% ==== cost type ====
% マルチステージ形式(0...N)で各ステージでの評価関数の形式を指定 今回は非線形な2次形式を適用
ocp.cost.cost_type_0 = 'NONLINEAR_LS'; % 最初のステージ
ocp.cost.cost_type   = 'NONLINEAR_LS'; % ステージ中
ocp.cost.cost_type_e = 'NONLINEAR_LS'; % 終端のステージ

% ==== 変数のスケーリング ====
scale_x = [100; 10; 1000];
S = diag(1 ./ scale_x);

% ==== 重み ====
W  = diag([10, 5, -10]);   % state
We = diag([100, 50, -10]); % terminal state
R  = 1;                    % input

% ==== スケーリング吸収後の重み ====
W_scaled  = S' * W  * S;
We_scaled = S' * We * S;

% ==== stage cost (0..N-1) ====
% y = [x; u]
ocp.model.cost_y_expr_0 = [model.x; model.u];
ocp.model.cost_y_expr   = [model.x; model.u];
ocp.cost.W_0 = blkdiag(W_scaled, R);
ocp.cost.W   = blkdiag(W_scaled, R);

% ==== terminal cost (N) ====
ocp.model.cost_y_expr_e = model.x;
ocp.cost.W_e            = We_scaled;

% ====制約条件====
% 状態量に対する制約条件（hとm）
ocp.constraints.idxbx = [0 2];       % 状態量のインデックス（0ベース）
ocp.constraints.lbx = [0; dry_mass]; % h>=0, m>=dry_mass
ocp.constraints.ubx = [inf; inf];    % h<=inf, m<=inf

% 操作量に対する制約条件
ocp.constraints.idxbu = 0;           % 操作量のインデックス（0ベース）
ocp.constraints.lbu  = 0;            % Tth>=0
ocp.constraints.ubu  = 1;            % Tth<=1

% 初期値の条件（仮置き）
ocp.constraints.x0   = [100; 0; m0]; % h0, v0, m0

% ====ソルバー設定====
% 予測時間と分割数
dt = 0.05;                            % 制御周期
T = 10;                               % 予測時間[s]
N = T/dt;                             % 分割数

ocp.solver_options.N_horizon       = N;
ocp.solver_options.tf              = T;
ocp.solver_options.qp_solver       = 'PARTIAL_CONDENSING_HPIPM'; % QPソルバーにHPIPMを指定
ocp.solver_options.nlp_solver_type = 'SQP_RTI';                  % SQP_RTIでNLPを解く
ocp.solver_options.integrator_type = 'ERK';                      % 積分は陽的ルンゲクッタ
ocp.solver_options.regularize_method = 'CONVEXIFY';

ocp.code_export_directory = 'c_generated_rocket1d';              % コード生成ディレクトリの名称を指定

% ====Simulink ブロック生成====
simulink_opts = get_acados_simulink_opts();
simulink_opts.output_dir = 'c_generated_rocket1d';
simulink_opts.build_solver_lib = true;
simulink_opts.generate_block = true;
simulink_opts.inputs.cost_W = true;
ocp.simulink_opts = simulink_opts;

% ====ソルバー生成====
ocp_solver = AcadosOcpSolver(ocp, simulink_opts);

以上のコードをMATLABで実行すると自分で指定した生成フォルダ c_generated_rocket1d が自動で作られ、そこにソルバーのコードなど生成物がどさっと格納されます。

生成されたファイルの中にSimulinkのS-Functionブロックを生成するためのスクリプト make_sfun.m があるので、c_generated_rocket1d に移動してこのファイルを実行します。

Simulinkブロックの生成

cd .\c_generated_rocket1d
make_sfun

するとフォルダ名を冠にしたSimulinkモデル rocket1d_ocp_solver_simulink_block.slx がc_generated_rocket1dフォルダに生成されます。

なお、ブロックはS-functionというカスタムコードをSimulinkに取り込んだ形式で実装されています。

上図はすでにSolverブロックに必要なIOを手動で繋いだ状態になっています。
各IOについては、get_acados_simulink_opts によって切り替えることが可能となっており、その詳細は以下のドキュメントに記載されています。

以上で準備が整ったので制御対象側のモデルも用意して閉ループのシミュレーションを実施して性能を確認してみます。

制御対象側のモデルはMATLAB FunctionブロックとIntegratorブロックを使ってモデリングしています。

MATLAB Functionブロックで実装した制御対象の状態方程式

function xdot = fcn(x,u)

v = x(2); m = x(3);
Tth = u;
g = 9.81;
m0 = 500;
thrustMax = 3*m0*g;
Isp = 350;

xdot = [ v;
        thrustMax*Tth/m - g;
        -thrustMax*Tth/(Isp*g) ];
end

初期高度100[m]、降下速度-20[m/s]、機体質量500[kg]を開始条件として、シミュレーションによって性能の確認を行います。
以下がシミュレーション結果です。

ご覧の通り、限られた燃料の中でスロットリングを調整しながら速度を減速させつつ、安全に着陸することに成功しています。

下図は、シミュレーション中におけるNMPCが掛かった毎制御サイクルにおけるCPU時間、そしてソルバーの実行ステータスの結果です。

ご覧の通り、計算時間はワーストでも5ms以内で終了しており、制御周期50msを十分に満足していることが分かります。
またソルバーのステータスは0（成功）となっています。
ちなみにステータスの種類は以下に記載されています。

NMPCモデルの自動Cコード生成

シミュレーションによって検証されたNMPCのブロックは最終的にターゲットとなる実機に組み込んで使用します。
acadosでは組み込みのワークフローをドキュメント化しており、dSPACE製品のターゲットで動作実績があるようです。

そこで、SimulinkモデルからのC/C++コード生成を行うためのツールボックス Embedded Coder を使い、acadosで作ったNMPCをソフトウェアとして使用できるようにします。

モデルからのコード生成を行うためにまずソルバーのライブラリなどをコンパイルし、それをカスタムコードとしてSimulinkモデルに反映します。

依存関係のあるカスタムソースは以下です。

・acados.lib
・blasfeo.lib
・hpipm.lib
・acados_solver_ *** .lib
・acados_ocp_solver_ *** .lib
・acados_sim_solver_ *** .lib
・acados_solver_sfunction_ *** .c
・acados_sim_solver_sfunction_ *** .c

一部のlibや.cファイルは生成するフォルダ名 *** に依存して名称が変更されますので注意してください。

ここで事前に生成されるCMakeListにてBuild Optionを編集し、あらかじめ生成されない依存関係のあるlibファイルを生成しておきます。

CMakeListの編集とminGWによるlibの生成 (***の部分は環境によってかえてください)

% コード生成したフォルダに移動
cd .\c_generated_rocket1d
!C:/***/mingw64/bin/mingw32-make.exe -j4

以上ができたら、続いてSimulinkのモデルに移動します。
まず、NMPCブロックだけのSimulinkモデルを用意し、モデルのコンフィギュレーションからEmbedded Coderを使用するように設定していきます。設定の方法については以下のブログが参考になります。

続いて、インラインでないS-Functionのサポートにチェックを入れます。

さらに続いて、今度はカスタムコードの設定に移動し、S-Functionのコード生成に必要となる依存関係のあるソースやインクルードを設定します。

***の部分は環境によって変更してください

以上で設定が完了です。あとはCtrl＋B（ビルドのショートカットコマンド）を実行するとモデルからの自動Cコード生成が始まります。

生成が完了すると以下のように生成したソースファイルの内容が確認できます。

モデルから生成されたソース類は「***_ert_rtw」（***はモデル名）というフォルダに以下のように格納されています。

この中にサンプルのメイン関数ert_main.cがあるので以下のように修正を加えます。

main関数

#include <stddef.h>
#include <stdio.h>
#include "codegenModel.h"

// parameter
#define TS 0.05
#define thrustMax 14715
#define Isp 350
#define g 9.81

// 状態量の初期値
static double rocket_z = 100; 
static double rocket_v = -20; 
static double rocket_m = 500; 

void rt_OneStep(FILE *fp)
{
    static boolean_T OverrunFlag = false;
    if (OverrunFlag) return;
    OverrunFlag = true;

    // 1. 入力セット
    codegenModel_U.x[0] = rocket_z;
    codegenModel_U.x[1] = rocket_v;
    codegenModel_U.x[2] = rocket_m;

    // 2. NMPC実行
    codegenModel_step();

    // 3. プラント更新（1D Rocket Model）
    double thrust = thrustMax * codegenModel_Y.u;
    double accel = (thrust / rocket_m) - g;
    rocket_v += accel * TS;
    rocket_z += rocket_v * TS;
    rocket_m -= (thrust / (Isp * g)) * TS; 

    // 4. 結果の表示（指定の形式を維持）
    printf("Time: %.2f | Altitude: %7.2f | Velocity: %7.2f | Mass: %7.2f | Tth: %7.2f | Status: %.0f | CPU TIME: %.6f \n",
           codegenModel_M->Timing.t[0], rocket_z, rocket_v, rocket_m, codegenModel_Y.u, codegenModel_B.status, codegenModel_B.cpu_time);
    
    // 5. CSV書き出し（printfと同じ順序で保存）
    if (fp != NULL) {
        fprintf(fp, "%.2f, %.2f, %.2f, %.2f, %.2f, %.0f, %.6f\n",
                codegenModel_M->Timing.t[0], rocket_z, rocket_v, rocket_m, codegenModel_Y.u, codegenModel_B.status, codegenModel_B.cpu_time);
    }

    OverrunFlag = false;
}

int main(int argc, const char *argv[])
{
    // NMPC関数の初期化
    codegenModel_initialize();

    // 結果をresult.csvに書き出し
    FILE *fp = fopen("result.csv", "w");
    if (fp == NULL) return 1;
    
    // ヘッダー書き込み
    fprintf(fp, "time,altitude,velocity,mass,tth,status,cpu_time\n"); 

    // 30秒間のシミュレーションループ
    int steps = 600;
    for(int i = 0; i < steps; i++) {
        rt_OneStep(fp);
        if (rtmGetErrorStatus(codegenModel_M) != NULL) break;
        if (rocket_z < 0.1) break; // 高度による停止判定
    }

    fclose(fp);

    // NMPC関数の終了処理
    codegenModel_terminate();
    printf("Simulation Finished.\n");
    return 0;
}

あとは Embedded Coder によってSimulnkモデルから自動生成された setup_mingw.bat と mkファイル（すべてモデル名_ert_rtwフォルダに存在） を実行することで実行アプリケーションを作成することができます。

MATLAB上でsystemコマンドを使って実行形式にビルド

system('setup_mingw.bat && gmake -f ***.mk') % ***はモデル名と一致

生成された.EXEの実行と結果の収集を行うため、MATLAB上で以下のsystemコマンドを使って検証してみます。

systemコマンドを使って実行アプリケーションを起動

% シミュレーション実行
system('***.exe'); 

% データの読み込み
data = readtable('result.csv');

figure('Color', 'w', 'Name', 'NMPC Rocket Landing Analysis');

% --- 高度と速度 ---
subplot(3,1,1);
yyaxis left
plot(data.time, data.altitude, 'LineWidth', 2);
ylabel('Altitude [m]');
yyaxis right
plot(data.time, data.velocity, '--', 'LineWidth', 1.5);
ylabel('Velocity [m/s]');
grid on; legend('Altitude', 'Velocity');

% --- 推力コマンドと質量 ---
subplot(3,1,2);
yyaxis left
stairs(data.time, data.tth, 'LineWidth', 1.5);
ylabel('Thrust Command (u)');
yyaxis right
plot(data.time, data.mass, 'LineWidth', 1.5);
ylabel('Mass [kg]');
grid on; legend('Thrust (u)', 'Mass');

% --- 計算負荷 (CPU TIME) ---
subplot(3,1,3);
plot(data.time, data.cpu_time * 1000, 'r', 'LineWidth', 1);
ylabel('CPU Time [ms]');
xlabel('Time [s]');
grid on; 
title('Controller Computation Overhead');

上記を実行し得られた結果を以下に掲載します。

結果からシミュレーションとほぼ等価な結果が得られており、ソルバーがきちんと機能していることが確認できます。
またCPU timeもmsオーダーと十分にリアルタイム性を備えていることが分かります。

まとめ

今回はacadosを使ってNMPCの設計をどのように行い、かつソフトウェアとして生成して検証するかまでの流れを試行してみました。一般に演算が重いとされるNMPCの実行がこんなに高速化できるとは正直驚きでした。

もちろん問題の複雑度合いやオーダー、実行するターゲット環境によってパフォーマンスは変動すると思いますが、NMPCをリアルタイム制御に取り入れるハードルを下げることができそうな感じです🚀