倒立振子制御をMPC（モデル予測制御）で動かしたい
MPCを使った組み込み制御（具体的には倒立振子ロボットの制御）を行おうと思い、調べて見たところ、cvxpyを使ってpython上で書くのが比較的簡単そうなのが判りました。ですが、ロボットに搭載するCPU(intel Celeron N5105）で実行してみたところ1フレームの計算に0.1秒以上かかってしまうことが判り、やはりC++などで書く必要があるか、と思いC++の最適化ライブラリであるCPPADを使って同様の事を行う事にしました。
ちなみにざっくり調べて比べてみたPythonでの処理時間のCPUによる差はこんな感じ。
| Intel Celeron N5105 | Intel 第5世代i7 | Intel 第13世代i9 |
|---|---|---|
| 約0.1秒 | 約0.05秒 | 約0.02秒 |
比較に使用したプログラムはいつもお世話になっているMyEnigmaさんのModel Predictive Controlによる倒立振子制御Pythonプログラムに掲載されているサンプルプログラムです。
実機制御にどれくらいの制御周波数が必要なのかわからないですが、最低でも100Hz、できれば200Hz程度は試せる環境が欲しいのでPythonじゃダメかなという感じです。C++化で１フレーム0.005秒で計算できればまずはヨシです。
運動方程式
倒立振子の運動方程式はあちこちに出ているので解説しませんが、Wikipediaの倒立振子の項で詳しく解説されています。Wikipedita 倒立振子
また、MyEnigmaさんが上記の記事を書くきっかけになった記事はこちらです。
モデル予測制御による倒立振子
最適化ライブラリ
とにかく
{\bf x}_{t+1} = A{\bf x}_{t} + Bu_t
という状態方程式で
J = {\bf X}_{t+1}^T {\bf Q} {\bf X}_{t+1} + u_tRu_t
における$J$を最小化していきます。
${\bf Q}$や$R$を調整することで制御の微調整を行います。
最小化の計算は凸最適化で解くそうです。凸最適化についてもMyEnigmaにわかりやすく解説がされています。凸最適化(Convex Optimization)の基礎
この記事に凸最適化ツールがたくさん紹介されています。今回の目的は実機でのMPCを使った倒立振子なので組み込み系って事になります。ただ、組み込みと言っても普通のマイコンで処理できるはずはなく、組み込みのために環境構築が難しかったり記述が複雑になると私のような独学系素人には手に負えなくなるのでできれば事例がたくさんある、もしくはドキュメントが豊富なそして環境構築が簡単なパッケージが良いと考えました。
しかし、C++言語などでMPCを記述しているサンプルはネットでは見かける事は出来ず、最適化問題のどこからどこが自分の問題に重なっているのかもわからず、ドキュメントは何を書いているのかわからず、途方に暮れてしまいました。
結局、いつもお世話になっているROS講座の記事にROS講座126 最適化計算をするというものを見つけ、IFOPTという最適化ツールがあることを知り、そこから手繰るようにCoin-OR FoundationのCppADパッケージに至りました。
CppADパッケージは自動微分ツールという呼び名になっていますが、最適化には偏微分が必須で、自動的に偏微分してくれるこのパッケージは最適化ツールを内包しているようです。
CppADを使ったMPCの作例として
より良い制御を目指して〜モデル予測制御の導入〜
より良い制御を目指して〜モデル予測制御（実装編）〜
も参考にさせてもらいました。
準備
必要なツールをインストールします。環境はUbuntu20.04です。
# sudo apt install -y build-essential
# sudo apt install -y coinor-libipopt1v5
# sudo apt install -y coinor-libipopt-dev
# sudo apt install -y cppad
もしコンパイルがうまくいかなかったら
# sudo apt install -y ros-noetic-ifopt
を入れてください。自分の環境ではifoptを入れてしまっていたのでlibipoptだけで大丈夫かどうか確認できていません。
matplotlibを使ったアニメーションを表示させるためのツールを入れます。
もちろんMPCの計算には不要です。
# sudo apt install python2.7-dev
# sudo apt install python-tk
python2.7用のpip(pip2)を入れます。
# curl -O https://files.pythonhosted.org/packages/27/79/8a850fe3496446ff0d584327ae44e7500daf6764ca1a382d2d02789accf7/pip-20.3.4-py2.py3-none-any.whl
# sudo python2.7 pip-20.3.4-py2.py3-none-any.whl/pip install --no-index pip-20.3.4-py2.py3-none-any.whl
python2.7用のmatplotlibを入れます。
# sudo pip2 install matplotolib
C++からmatplolibを使えるようにするツールを入れます。
# git clone https://github.com/lava/matplotlib-cpp.git
コーディング
ソースコードです
#include <iostream>
#include <cppad/ipopt/solve.hpp>
#include <Eigen/Dense>
#include "./matplotlib-cpp/matplotlibcpp.h"
#include <time.h>
#include <math.h>
namespace plt = matplotlibcpp;
using CppAD::AD;
class FG_eval {
public:
typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
FG_eval(const Eigen::Matrix<double, 4, 4> _Q,
double _R,
const Eigen::Matrix<double, 4, 4> _A,
const Eigen::Matrix<double, 4, 1> _B,
const size_t _T )
: Q(_Q), R(_R), A(_A), B(_B) , T(_T){}
void operator()(ADvector& fg, const ADvector& x)
{ assert( fg.size() == 4 * (T + 1) + 1 );
assert( x.size() == (4 + 1) * T + 4 );
for(int t = 0; t < T; t++){
//cost function
fg[0] += x[(t+1)*5] * (Q(0,0) * x[(t+1)*5] + Q(0,1) * x[(t+1)*5+1] + Q(0,2) * x[(t+1)*5+2] + Q(0,3) * x[(t+1)*5+3]) +
x[(t+1)*5+1] * (Q(1,0) * x[(t+1)*5] + Q(1,1) * x[(t+1)*5+1] + Q(1,2) * x[(t+1)*5+2] + Q(1,3) * x[(t+1)*5+3]) +
x[(t+1)*5+2] * (Q(2,0) * x[(t+1)*5] + Q(2,1) * x[(t+1)*5+1] + Q(2,2) * x[(t+1)*5+2] + Q(2,3) * x[(t+1)*5+3]) +
x[(t+1)*5+3] * (Q(3,0) * x[(t+1)*5] + Q(3,1) * x[(t+1)*5+1] + Q(3,2) * x[(t+1)*5+2] + Q(3,3) * x[(t+1)*5+3]) +
x[t*5+4] * R * x[t*5+4];
//constraint function
fg[t*4+1] = (A(0,0) * x[t*5] + A(0,1) * x[t*5+1] + A(0,2) * x[t*5+2] + A(0,3) * x[t*5+3]) + B(0) * x[t*5+4] - x[(t+1)*5];
fg[t*4+2] = (A(1,0) * x[t*5] + A(1,1) * x[t*5+1] + A(1,2) * x[t*5+2] + A(1,3) * x[t*5+3]) + B(1) * x[t*5+4] - x[(t+1)*5+1];
fg[t*4+3] = (A(2,0) * x[t*5] + A(2,1) * x[t*5+1] + A(2,2) * x[t*5+2] + A(2,3) * x[t*5+3]) + B(2) * x[t*5+4] - x[(t+1)*5+2];
fg[t*4+4] = (A(3,0) * x[t*5] + A(3,1) * x[t*5+1] + A(3,2) * x[t*5+2] + A(3,3) * x[t*5+3]) + B(3) * x[t*5+4] - x[(t+1)*5+3];
}
fg[T*4+1] = x[0];
fg[T*4+2] = x[1];
fg[T*4+3] = x[2];
fg[T*4+4] = x[3];
return;
}
Eigen::Matrix<double, 4, 4> Q;
double R;
Eigen::Matrix<double, 4, 4> A;
Eigen::Matrix<double, 4, 1> B;
size_t T;
};
void draw_cart(double xt, double theta, double l_bar)
{
double cart_w = 1.0;
double cart_h = 0.5;
double radius = 0.1;
std::vector<double> cx{ -cart_w / 2.0, cart_w / 2.0, cart_w / 2.0, -cart_w / 2.0, -cart_w / 2.0};
std::vector<double> cy{ 0.0, 0.0, cart_h, cart_h, 0.0};
for(auto &d : cx){
d = d + xt;
}
for(auto &d : cy){
d = d + radius * 2.0;
}
std::vector<double> bx{ 0.0, l_bar * sin(-theta)};
std::vector<double> by {cart_h, l_bar * cos(-theta) + cart_h};
for(auto &d : bx){
d = d + xt;
}
for(auto &d : by){
d = d + radius * 2.0;
}
std::vector<double> ox, oy;
for(int i = 0; i <= 20; i++){
double ang = M_PI * 2.0 * i / 20.0;
ox.push_back(radius * cos(ang));
oy.push_back(radius * sin(ang));
}
std::vector<double> rwx, rwy, lwx, lwy;
for(auto d : ox){
rwx.push_back(d + cart_w / 4.0 + xt);
lwx.push_back(d - cart_w / 4.0 + xt);
}
for(auto d : oy){
rwy.push_back(d + radius);
lwy.push_back(d + radius);
}
std::vector<double> wx, wy;
for(auto d : ox){
wx.push_back(d + bx[1]);
}
for(auto d : oy){
wy.push_back(d + by[1]);
}
plt::plot(cx, cy, "-k");
plt::plot(bx, by, "-k");
plt::plot(rwx, rwy, "-b");
plt::plot(lwx, lwy, "-b");
plt::plot(wx, wy, "-b");
plt::axis("equal");
}
bool calc_mpc(void)
{ bool ok = true;
size_t i;
size_t T = 30;
typedef CPPAD_TESTVECTOR( double ) Dvector;
// number of independent variables (domain dimension for f and g)
size_t nx = (4 + 1) * T + 4;
// number of constraints (range dimension for g)
size_t ng = 4 * (T + 1);
// initial value of the independent variables
Dvector x0(5);
Dvector xi(nx);
x0[0] = 0.0;
x0[1] = 0.0;
x0[2] = 0.5;
x0[3] = 0.0;
xi[0] = x0[0];
xi[1] = x0[1];
xi[2] = x0[2];
xi[3] = x0[3];
// lower and upper limits for x
double lim[] = {10.0, 10.0, 2.0, 2.0, 50.0};
Dvector xl(nx), xu(nx);
for(i = 0; i < nx; i++)
{
xl[i] = -lim[i % 5];
xu[i] = lim[i % 5];
}
// lower and upper limits for g
Dvector gl(ng), gu(ng);
double delta_t = 0.1;
double l_bar = 2.0;
double M =1.0;
double m = 0.3;
double g = 9.8;
Eigen::Matrix<double, 4, 4> A;
Eigen::Matrix<double, 4, 1> B;
Eigen::Matrix<double, 4, 4> Q;
double R;
A << 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, m*g/M, 0.0,
0.0, 0.0, 0.0, 1.0,
0.0, 0.0, g*(M+m)/(l_bar*M), 0.0;
A = Eigen::Matrix<double, 4, 4>::Identity() + delta_t * A;
B << 0.0, 1.0/M, 0.0, 1.0/(l_bar*M);
B = delta_t * B;
Q << 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0;
R = 0.01;
// object that computes objective and constraints
FG_eval fg_eval(Q, R, A, B, T);
// options
std::string options;
//options += "Retape true\n";
options += "Sparse true forward\n";
options += "Sparse true reverse\n";
// turn off any printing
options += "Integer print_level 0\n";
//options += "String sb yes\n";
// maximum number of iterations
options += "Integer max_iter 30\n";
// approximate accuracy in first order necessary conditions;
// see Mathematical Programming, Volume 106, Number 1,
// Pages 25-57, Equation (6)
options += "Numeric tol 1e-6\n";
// derivative testing
//options += "String derivative_test second-order\n";
// maximum amount of random pertubation; e.g.,
// when evaluation finite diff
options += "Numeric point_perturbation_radius 0.\n";
// place to return solution
CppAD::ipopt::solve_result<Dvector> solution;
for(int i = 0; i < 50; i++){
Eigen::Matrix<double, 4, 1> X;
X << xi[0], xi[1], xi[2], xi[3];
for(int t = 0; t < T; t++){
X = A * X + B * xi[4];
xi[(t+1)*5 -1] = 0;
xi[(t+1)*5 + 0] = X(0);
xi[(t+1)*5 + 1] = X(1);
xi[(t+1)*5 + 2] = X(2);
xi[(t+1)*5 + 3] = X(3);
}
//制約値
for(int t = 0; t < T; t++)
{
gl[4*t] = 0.0; gu[4*t] = 0.0;
gl[4*t+1] = 0.0; gu[4*t+1] = 0.0;
gl[4*t+2] = 0.0; gu[4*t+2] = 0.0;
gl[4*t+3] = 0.0; gu[4*t+3] = 0.0;
}
gl[4*T] = xi[0]; gu[4*T] = xi[0];
gl[4*T+1] = xi[1]; gu[4*T+1] = xi[1];
gl[4*T+2] = xi[2]; gu[4*T+2] = xi[2];
gl[4*T+3] = xi[3]; gu[4*T+3] = xi[3];
clock_t start = clock();
// solve the problem
CppAD::ipopt::solve<Dvector, FG_eval>(
options, xi, xl, xu, gl, gu, fg_eval, solution
);
clock_t end = clock();
double time = (double)(end - start) / CLOCKS_PER_SEC;
std::cout << "elapsed time : " << time << std::endl;
if(solution.status == CppAD::ipopt::solve_result<Dvector>::success){
Eigen::Matrix<double, 4, 1> X;
X << xi[0], xi[1], xi[2], xi[3];
X = A * X + B * solution.x[4];
xi[0] = X(0);
xi[1] = X(1);
xi[2] = X(2);
xi[3] = X(3);
std::cout << i << "," << xi[0] << "," << xi[1] << "," << xi[2] << "," << xi[3] << "," << solution.x[4] << std::endl;
plt::clf();
draw_cart(xi[0], xi[2], l_bar);
plt::xlim(-5.0, 2.0);
plt::pause(0.01);
}else{
ok = false;
break;
}
}
return ok;
}
int main(){
if(calc_mpc()){
std::cout << "ok" << std::endl;
}else{
std::cout << "ng" << std::endl;
}
plt::show();
}
簡単に解説します。
class FG_val operator()(fg, x)
最適化したい関数が定義される。CppADにはADfunという仕組みがあり、これが自動微分の要となっています。この機能をつかうため、最適化の対象式や対象変数はAD<>(大抵はAD<$double$>だと思う）を使って表す必要がある、と思う。
fg[0]に代入される計算式がコスト関数
fg[1]〜fg[ng]に代入される計算式が制約関数
x[0]〜x[nx-1] 最適化対象の変数
CppAD::ipopt::solve<>()の引数
options solveのオプション指定
xi[] x[]に引き渡される変数値の初期値
xl[] x[]の最低制約値
xu[] x[]の最大制約値
gl[] fg[1]〜fg[ng]の最低制約値
gu[] fg[1]〜fg[ng]の最大制約値
fg_val class FG_valのインスタンス
solutions 最適化結果
以上の設定を行い、solve()を実行すればうまくすれば最適化の結果が出ます。
では、これに状態方程式を使ったMPCを当てはめてみます。
MPCでは
{\bf x}_{t+1} = A{\bf x}_{t} + Bu
この式をある程度の未来時間（予測ホライズン）まで計算し、そこまですべての計算を最適化して直近の入力値$u$を決定します。
解析時間単位$dt=0.1$とし、3秒先までを対象としたとき予測ホライズン数$T$は30になります。
つまり30個の状態方程式の$J$の合計がコスト関数になり、
30個の状態方程式と${\bf x}$の初期値${\bf x}_{0}$が制約関数になります。
ただし、solve()が扱えるのはスカラーリストだけのようなので行列式やベクトル値はスカラー式に展開する必要があるようです。
結果として、最適化対象の変数値x[]の数は $(4+1) \times T +4$ (${\bf x}$が4で$u$が1、$T+1$には$u$が無いため）
制約関数は$4 \times (T+1)$ fg[]の数はコスト関数fg[0]を加えて$4 \times (T+1)+1$
予測ホライズン数$T$は30なので、最適化対象の変数は154個、コスト関数は1（常に1）、制約関数は124個となります。
倒立振子制御なんていう簡単な問題のはずなのに結構な計算量になるわけです。
制約値について
制約関数は等式と不等式があります。等式の場合はgl[]とgu[]に同じ値をいれます。
等式の場合、
g = x_0 + x_1
だとすると
gl[0] = g;
gu[0] = g;
//class FG_valにて
fg[1] = x[0] + x[1];
不等式の場合、
gl \leq x_2 + x_3 \leq gu
の場合、
gl[1] = gl;
gu[1] = gu;
//class FG_valにて
fg[2] = x[2] + x[3];
という感じ
ところがMPCの場合は、等式の左辺が最適化対象なのでこの記載ではだめです。
x_6 = x_4 + x_5
だとすると
0 = x_4 + x_5 - x_6
として
gl[2] = 0;
gu[2] = 0;
//class FG_valにて
fg[3] = x[4] + x[5] - x[6];
とします。ここがわからずに少し悩みました。
状態変数の制約値
メカや物理制限からくる制約値を設定すればその条件を満たしつつ最適化する、、、んだそうです。
optionsについて
よくわかりません。他のサンプルや試行錯誤で現状の設定になっています。
options += "Integer print_level 0\n";
メッセージレベルです。0で余計なこと喋らないモードです。この行をコメントアウトすると最適化の途中経過などが見れます。私にはほとんど意味はわかりませんが、線形ソルバーが動いてるんだーとか、最適化ループが10回で終わったのかーとか、ちゃんと等式として認識されているな、とかがわかります。
options += "Integer max_iter 30\n";
最適化の繰り返し回数上限です。これを超えると問題が解けなかった事になります。難しい問題の場合は増やしていきます。倒立振子問題なら30でお釣りがきます。
matplotlib-cppについて
pythonのグラフ化ツールmatplotlibがC++環境でも使える便利なヘッダーファイルです。
MyEnigmaさんで使い方含め紹介されています。C++のコードから簡単にmatplotlibを使ってグラフを作成する方法
ですが、MyEnigmaさんのgitから落とすと少々古いので色々インプリがたりません。（画面のクリアができなかったり）オリジナルの方を使いますが、少し修正が必要です。
と言ってもこれだけですが。
//修正前
#include <Python.h>
//修正後
#include <python2.7/Python.h>
ではコンパイルします。
# g++ cart_pole_mpc.cpp -DHAVE_CSTDDEF -DWITHOUT_NUMPY -I/usr/include/eigen3 -lipopt -lstdc++ -lpython2.7 -std=c++11 -o cart_pole_mpc
実行します。
# ./cart_pole_mpc
結果です。
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************
elapsed time : 0.056218
0,-3,3.3868,-0.5,1.4484,35.338
elapsed time : 0.01621
1,-2.66132,4.04614,-0.35516,1.53307,8.06344
elapsed time : 0.016858
2,-2.25671,3.79577,-0.201853,1.23386,-1.45954
elapsed time : 0.017578
3,-1.87713,3.29518,-0.0784675,0.884653,-4.41246
elapsed time : 0.017154
4,-1.54761,2.77447,0.00999776,0.585851,-4.97636
elapsed time : 0.017304
5,-1.27016,2.30625,0.0685829,0.35664,-4.71159
elapsed time : 0.016248
6,-1.03954,1.90672,0.104247,0.190478,-4.19699
elapsed time : 0.01621
7,-0.848867,1.573,0.123295,0.0747017,-3.64363
elapsed time : 0.016058
8,-0.691567,1.29677,0.130765,-0.00300195,-3.12485
elapsed time : 0.016125
9,-0.56189,1.06896,0.130465,-0.0528334,-2.66257
elapsed time : 0.016192
10,-0.454994,0.881334,0.125181,-0.0827163,-2.25978
elapsed time : 0.016082
11,-0.366861,0.726864,0.11691,-0.0986125,-1.91273
elapsed time : 0.016173
12,-0.294175,0.599672,0.107048,-0.104923,-1.61564
elapsed time : 0.016174
13,-0.234207,0.494907,0.0965561,-0.104852,-1.36237
elapsed time : 0.016157
14,-0.184717,0.408581,0.0860709,-0.100702,-1.14713
elapsed time : 0.016208
15,-0.143859,0.337421,0.0760007,-0.0941076,-0.964654
elapsed time : 0.016207
16,-0.110117,0.278738,0.06659,-0.0862087,-0.810272
elapsed time : 0.016609
17,-0.0822428,0.230326,0.0579691,-0.0777857,-0.679894
elapsed time : 0.016371
18,-0.0592103,0.190372,0.0501905,-0.0693574,-0.569961
elapsed time : 0.016313
19,-0.040173,0.157389,0.0432548,-0.0612559,-0.477397
elapsed time : 0.016084
20,-0.0244342,0.13015,0.0371292,-0.0536805,-0.399557
elapsed time : 0.016157
21,-0.0114192,0.107648,0.0317611,-0.0467379,-0.334173
elapsed time : 0.016252
22,-0.000654354,0.0890554,0.0270873,-0.0404715,-0.279309
elapsed time : 0.016064
23,0.00825118,0.0736876,0.0230402,-0.0348825,-0.233314
elapsed time : 0.01593
24,0.0156199,0.0609826,0.0195519,-0.0299454,-0.194789
elapsed time : 0.015942
25,0.0217182,0.0504764,0.0165574,-0.025618,-0.162544
elapsed time : 0.016661
26,0.0267658,0.0417866,0.0139956,-0.0218498,-0.135576
elapsed time : 0.016103
27,0.0309445,0.0345978,0.0118106,-0.0185863,-0.113035
elapsed time : 0.016007
28,0.0344043,0.0286496,0.00995198,-0.0157733,-0.0942057
elapsed time : 0.016121
29,0.0372692,0.0237269,0.00837465,-0.0133581,-0.0784853
elapsed time : 0.016294
30,0.0396419,0.0196524,0.00703884,-0.0112918,-0.065367
elapsed time : 0.016079
31,0.0416072,0.0162793,0.00590966,-0.00952932,-0.054425
elapsed time : 0.016325
32,0.0432351,0.0134865,0.00495673,-0.00802998,-0.0453022
elapsed time : 0.016068
33,0.0445838,0.0111739,0.00415373,-0.00675749,-0.037699
elapsed time : 0.016574
34,0.0457011,0.00925863,0.00347798,-0.00567979,-0.0313645
elapsed time : 0.016028
35,0.046627,0.00767228,0.00291,-0.00476876,-0.0260888
elapsed time : 0.016227
36,0.0473942,0.00635821,0.00243313,-0.00399989,-0.0216961
elapsed time : 0.032613
37,0.0480301,0.00526958,0.00203314,-0.00335197,-0.0180397
elapsed time : 0.017035
38,0.048557,0.00436762,0.00169794,-0.00280671,-0.014997
elapsed time : 0.016717
39,0.0489938,0.00362027,0.00141727,-0.0023484,-0.0124655
elapsed time : 0.016395
40,0.0493558,0.00300096,0.00118243,-0.00196359,-0.0103598
elapsed time : 0.016276
41,0.0496559,0.00248773,0.000986072,-0.00164081,-0.00860866
elapsed time : 0.016614
42,0.0499047,0.00206237,0.000821991,-0.00137032,-0.00715262
elapsed time : 0.024028
43,0.0501109,0.00170982,0.000684959,-0.00114382,-0.00594216
elapsed time : 0.025443
44,0.0502819,0.0014176,0.000570578,-0.000954298,-0.00493603
elapsed time : 0.016093
45,0.0504237,0.00117536,0.000475148,-0.000795832,-0.00409985
elapsed time : 0.016058
46,0.0505412,0.000974556,0.000395565,-0.000663413,-0.003405
elapsed time : 0.016055
47,0.0506386,0.000808084,0.000329224,-0.000552822,-0.00282768
elapsed time : 0.016121
48,0.0507195,0.000670069,0.000273941,-0.00046051,-0.00234806
elapsed time : 0.016162
49,0.0507865,0.000555643,0.00022789,-0.000383492,-0.00194965
ok
測定結果
python版で比較した時と同様に比較してみます。
| Intel Celeron N5105 | Intel 第5世代i7 | Intel 第13世代i9 |
|---|---|---|
| 約0.025秒 | 約0.018秒 | 約0.008秒 |
i9で解いても目標に至らず。Celeronだと0.025秒くらいかかるようです。違うライブラリだとどうだろう？とか興味がでてきますが深追いはやめておきます。
ちなみに台車の位置がずれたままで終了してしまいますが、これは評価用変数${\bf Q}$の位置パラメータがゼロになっているためで、
{\bf Q} =
\begin{bmatrix}
1&0&0&0\\
0&1&0&0\\
0&0&1&0\\
0&0&0&0
\end{bmatrix}
とすることで元の位置に収束します。（シミュレーション時間が5秒では足りないかもしれないですが）
まとめ
当初の目的の処理時間は達成していないのですが、実機実装、実機駆動へと進みます。できれば40Hzくらいでも制御できてほしいなぁという期待を込めて。ダメなら他のライブラリを試してみるなどしてみます。
MPCってなんというかガンガン計算して答え出してる感じでチートっぽいですが、これができるのも最適化の技術、そして最適化パッケージがあるからなわけで、フリーでこのような価値のあるライブラリが使える環境がとてもありがたいです。
状態方程式を使った最適化だけでなく、キネマティックな条件での最適化など、直感的に問題設定してあとは最適化しちゃえという使い方もできるので趣味のロボット制御なんかにももっと使われると良いと思います。
MPCなんて全くもって新しい技術ではなく（ここにわかりやすくまとめてもらってます。モデル予測制御（MPC）による軌道追従制御）リアルタイム制御に使えるようになったのは最近で、さらには私のような素人が扱えるようになったのはもっと最近かなと。これから参考にできる作例が増えてくると良いなあと思います。
CppADライブラリでMPCの計算ができる事はわかったのだが、行列やベクトルで式をかけないのはちょっと面倒。関数の書き方でなんとかできそうな気もするのだが、行列で処理する正規の方法があるのかもしれないし無理やりに書くのは単にオーバーヘッドになるだけなので試してません。もっと複雑な問題だと書き下すのがしんどい（＝間違えやすい）と思うので行列で処理する方法があったら教えてください。
最後に、この記事で何度も引用させてもらったMyEnigmaさん、Qiitaの寄稿者の方々には本当にいつも大変お世話になっています。足を向けて眠れません。ありがとうございます。
たぶん立って寝なきゃダメな感じです。