はじめに
本記事では、回転型並列二重倒立振子の振り上げ安定化について、MATLABコードによるシミュレーションを紹介します。制御系設計はいわゆる非線形最適制御に基づきますが、それらの解説はあまり深堀りしません。「非線形最適制御でこんなことができる」、「MATLABのSimscape Multibodyを使うと、こんな感じで可視化できる」という紹介のみとして、理論やコードそのものについては最低限だけ触れようと思います。
最終的に、下記のような動きを実現するコード・モデルについて紹介します。
質点振子モデルの場合
剛体振子モデルの場合
本記事のMATLABコード、Simulinkモデルは下記におきました。
本記事の理論面が気になる方は、末尾の参考文献、または下記のウェブページを参照ください。
制御手法の概要
先に概要を述べると、本記事では以下の方針で制御系を設計しています。
- 制御対象：回転型並列二重倒立振子(基本的には連続時間系とみなし、シミュレーション実装時に離散化する)
- 制御手法：非線形最適制御による軌道計画と、軌道周りで線形化した制御対象への時変ゲインの実装
- 軌道計画の数値解法：制御入力$u(t)$の時系列を未知量とみなし、評価関数を最小化する$u(t)$を勾配法(最急降下法)で求める
- 使用ツール：MATLAB, Simulink, Simscape Multibody(アニメーション表示以外は、MATLABのみで動きます)
制御対象の運動方程式
まず、質点振子モデルについて運動方程式を導出します。その後、剛体振子モデルについても同様に導出します。(というより、いきなり剛体モデルを作ろうとしたらハマったので、質点モデルを作りました)
質点モデルの運動方程式
座標系は下記のように取っています。オレンジの長い方を振子1、緑の短い方を振子2と呼び、中央グレーの回転する部分をアームと呼ぶことにします。
x-y平面で表すと下記です。
したがって、振子1の重心への座標は
\begin{pmatrix}
x(t) \\
y(t) \\
z(t)
\end{pmatrix}
=
\begin{pmatrix}
r \cos \theta - l_1 \sin \phi_1 \sin \theta \\
r \sin\theta + l_1 \sin \phi_1 \cos\theta \\
- l_1 \cos\phi_1
\end{pmatrix}
となり、時間微分は下記で得られます。これらは運動方程式の導出時に使います。
\begin{pmatrix}
\dot{x}(t) \\
\dot{y}(t) \\
\dot{z}(t)
\end{pmatrix}
=
\begin{pmatrix}
-r\dot{\theta}\sin\theta - l_1\left(\dot{\phi}_1\cos\phi_1\sin\theta + \dot{\theta}\sin\phi_1\cos\theta\right) \\
r\dot{\theta}\cos\theta + l_1\left(\dot{\phi}_1\cos\phi_1\cos\theta - \dot{\theta}\sin\phi_1\sin\theta\right) \\
l_1\dot{\phi}_1\sin\phi_1
\end{pmatrix}
※ ここでの$x$は状態方程式の状態ベクトルではなく、座標系の1つです。
質点モデルの運動方程式の導出
以下、ラグランジアン$L$を用いた運動方程式の導出過程を示します。
1. ラグランジアンの定義
ラグランジアン$L$は、運動エネルギー$T$とポテンシャルエネルギー$U$を用いて、下式で定義されます：
L = T - U
2. 運動エネルギーTの計算
質点の運動エネルギーは次式で与えられます：
T = \frac{1}{2}m|\mathbf{v}|^2
ここで、$|\mathbf{v}|^2 = \dot{x}^2 + \dot{y}^2 + \dot{z}^2$ です。
3. ポテンシャルエネルギーUの計算
重力によるポテンシャルエネルギーは、
U = mgz = -mgl_1 \cos\phi_1
4. ラグランジアンLの式展開
以上より、ラグランジアンは次のように表されます：
L = \frac{1}{2}m\left(\dot{x}^2 + \dot{y}^2 + \dot{z}^2\right) + mgl_1 \cos\phi_1
ここで、$\dot{x}$, $\dot{y}$, $\dot{z}$を先ほどの式で代入し整理することで、ラグランジアン$L$は下式で得られます。
\begin{align}
L &= \frac{1}{2}m\Big[
\left(-r\dot{\theta}\sin\theta - l_1(\dot{\phi}_1\cos\phi_1\sin\theta + \dot{\theta}\sin\phi_1\cos\theta)\right)^2
+ \left(r\dot{\theta}\cos\theta + l_1(\dot{\phi}_1\cos\phi_1\cos\theta - \dot{\theta}\sin\phi_1\sin\theta)\right)^2
+ (l_1\dot{\phi}_1\sin\phi_1)^2
\Big] + mgl_1\cos\phi_1 \\
&= \frac{1}{2}m\Big[
r^2\dot{\theta}^2 + l_1^2\dot{\phi}_1^2 + l_1^2\dot{\theta}^2\sin^2\phi_1 + 2r l_1 \dot{\theta}\dot{\phi}_1 \cos\phi_1
\Big] + mgl_1\cos\phi_1
\end{align}
5. 運動方程式の導出
$\phi_1$についてのオイラー・ラグランジュ方程式
\underbrace{\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}_1}\right)}_{\text {(1)}} - \underbrace{\frac{\partial L}{\partial \phi_1}}_{\text {(2)}} = -\frac{\partial D}{\partial \dot{\phi}_1}
に対する各項を求めます。右辺は散逸項で、結局$-c_1 \dot{\phi}_1$となります。
(1) の計算
\frac{\partial L}{\partial \dot{\phi}_1} = m \left[ l_1^2 \dot{\phi}_1 + r l_1 \dot{\theta} \cos\phi_1 \right]
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}_1}\right)
= m \left[ l_1^2 \ddot{\phi}_1 + r l_1 \frac{d}{dt}(\dot{\theta} \cos\phi_1) \right]
ここで
\frac{d}{dt}(\dot{\theta} \cos\phi_1) = \ddot{\theta} \cos\phi_1 - \dot{\theta} \dot{\phi}_1 \sin\phi_1
より
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}_1}\right)
= m \left[ l_1^2 \ddot{\phi}_1 + r l_1 \left( \ddot{\theta} \cos\phi_1 - \dot{\theta} \dot{\phi}_1 \sin\phi_1 \right) \right]
(2) の計算
\frac{\partial L}{\partial \phi_1} = m \left[ l_1^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - r l_1 \dot{\theta} \dot{\phi}_1 \sin\phi_1 \right] - m g l_1 \sin\phi_1
オイラー・ラグランジュ方程式
m \left[ l_1^2 \ddot{\phi}_1 + r l_1 \left( \ddot{\theta} \cos\phi_1 - \dot{\theta} \dot{\phi}_1 \sin\phi_1 \right) \right]
- \left\{ m \left[ l_1^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - r l_1 \dot{\theta} \dot{\phi}_1 \sin\phi_1 \right] - m g l_1 \sin\phi_1 \right\} = -c_1 \dot{\phi}_1
整理すると、$r l_1 \dot{\theta} \dot{\phi}_1 \sin\phi_1 $がちょうど消えて、
m \left[ l_1^2 \ddot{\phi}_1 + r l_1 \ddot{\theta} \cos\phi_1 - l_1^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 + g l_1 \sin\phi_1 \right] = -c_1 \dot{\phi}_1
さらに、項を右辺に寄せて
m l_1^2 \ddot{\phi}_1 = ml_1^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - mr l_1 \ddot{\theta} \cos\phi_1 - mg l_1 \sin\phi_1 -c_1 \dot{\phi}_1
として、$\phi_1$に関する運動方程式が得られました。
ここで、式の簡単化のための工夫を入れます。
$\ddot{\theta}$についても同様に運動方程式を求めると複雑化するので、$\ddot{\theta}(t)=u(t)$とし、アームの角加速度が直接制御入力で与えられるものとします。
これは、アームががっちりハイゲインで動くことで、振子角度や振子角速度の影響を受けない、というようなことを意味します。もちろん実物はそうもいきませんが、この仮定を設けることで運動方程式が一気に簡単になるため採用します。
結果、質点モデルの二重振子の運動方程式は以下のように表されます：
\begin{align}
m l_1^2 \ddot{\phi}_1 &= ml_1^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - mr l_1 u \cos\phi_1- mg l_1 \sin\phi_1 -c_1 \dot{\phi}_1 \\
\ddot{\phi}_1 &= \dot{\theta}^2 \sin\phi_1 \cos\phi_1 -\frac{r}{l_1} u \cos\phi_1- \frac{g}{l_1} \sin\phi_1 -\frac{c_1}{m l_1^2} \dot{\phi}_1 \\
&= \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - a_{p1} u \cos\phi_1- w_{p1} \sin\phi_1 - c_{p1} \dot{\phi}_1
\end{align}
最後の式で、適当な定数係数を置きました。
お疲れさまでした。あとは、$u$の運動方程式を省略したおかげで、振子2側の$\phi_2$も全く同様の運動方程式が得られます。
以上をまとめて、質点モデルの回転型並列二重倒立振子の運動方程式は下記となります。
\begin{pmatrix}
\dot{x}_1 \\
\dot{x}_2 \\
\dot{x}_3 \\
\dot{x}_4 \\
\dot{x}_5 \\
\dot{x}_6
\end{pmatrix}
=
\begin{pmatrix}
x_2 \\
u \\
x_4 \\
x_2^2\sin x_3\cos x_3 - a_{p1}u\cos x_3 - w_{p1}\sin x_3 - c_{p1}x_4 \\
x_6 \\
x_2^2\sin x_5\cos x_5 - a_{p2}u\cos x_5 - w_{p2}\sin x_5 - c_{p2}x_6
\end{pmatrix}
ここで、状態変数は下記の通りです：
- $x_1, x_2$: アームの位置と速度
- $x_3, x_4$: 第1リンクの角度と角速度
- $x_5, x_6$: 第2リンクの角度と角速度
また、定数の意味は下記の通りです：
- $a_{p1}$ : $\frac{r}{l_1}$
- $w_{p1}$ : $\frac{g}{l_1}$
- $c_{p1}$ : $\frac{c_1}{m l_1^2}$
$c_1$は振子1の回転角速度に対する減衰係数で、その他の定数の物理的な意味は倒立振子の図を参照してください。また、振子2側も同様です。
剛体モデルの運動方程式
剛体モデルの場合、質点モデルとの運動方程式の違いはラグランジアン$L$に$\frac{1}{2}I_1\dot{\phi_1}^2$の回転運動エネルギーが追加される点のみです。($I_1$は振子1の慣性モーメント)
ただし、もろもろの事情があって剛体振子1の重心までの距離ではなく、全長を用いて表すこととしました。そのため、$l_1$ではなく、剛体モデルの場合には$l_{r1}$と書くこととします。これによって、質点モデルの運動方程式が下記のように変わります。
\begin{align}
m \left(\frac{l_{r1}}{2}\right)^2 \ddot{\phi}_1 + I \ddot{\phi}_1&= m\left(\frac{l_{r1}}{2}\right)^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - mr \left(\frac{l_{r1}}{2}\right)u \cos\phi_1- mg \left(\frac{l_{r1}}{2}\right)\sin\phi_1 -c_1 \dot{\phi}_1
\end{align}
一方、質量$m$の一様な直方体の慣性モーメントは下記であらわされます。
I = \frac{1}{12} m(a^2 + b^2)
ここで、振子の長手方向は短手方向に比べて十分長いとみなすことで、下式が得られます。
I = \frac{1}{12} m l_{r1}^2
したがって剛体振子モデルの運動方程式は
\begin{align}
m \left(\frac{l_{r1}}{2}\right)^2 \ddot{\phi}_1 + \frac{1}{12} m l_{r1}^2\ddot{\phi}_1 &= m\left(\frac{l_{r1}}{2}\right)^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - mr \left(\frac{l_{r1}}{2}\right)u \cos\phi_1- mg \left(\frac{l_{r1}}{2}\right)\sin\phi_1 -c_1 \dot{\phi}_1 \\
\frac{m l_{r1}^2}{3} \ddot{\phi}_1 &= m\left(\frac{l_{r1}}{2}\right)^2 \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - mr \left(\frac{l_{r1}}{2}\right)u \cos\phi_1- mg \left(\frac{l_{r1}}{2}\right)\sin\phi_1 -c_1 \dot{\phi}_1 \\
\ddot{\phi}_1 &= \left(\frac{3}{4}\right) \dot{\theta}^2 \sin\phi_1 \cos\phi_1 - \left(\frac{3r}{2l_{r1}}\right)u \cos\phi_1- \left(\frac{3 g}{2l_{r1}}\right)\sin\phi_1 -\left(\frac{3c_1}{m l_{r1}^2}\right)\dot{\phi}_1
\end{align}
状態方程式としては下記になります。質点モデルとは係数が違うだけ、となっています。
\begin{pmatrix}
\dot{x}_1 \\
\dot{x}_2 \\
\dot{x}_3 \\
\dot{x}_4 \\
\dot{x}_5 \\
\dot{x}_6
\end{pmatrix}
=
\begin{pmatrix}
x_2 \\
u \\
x_4 \\
b_{r1} x_2^2\sin x_3\cos x_3 - a_{r1}u\cos x_3 - w_{r1}\sin x_3 - c_{r1}x_4 \\
x_6 \\
b_{r2} x_2^2\sin x_5\cos x_5 - a_{r2}u\cos x_5 - w_{r2}\sin x_5 - c_{r2}x_6
\end{pmatrix}
各定数の意味は下記の通りです：
- $a_{ri}$ : $\frac{3r}{2l_{ri}}$
- $b_{ri}$ : $\frac{3}{4}$
- $w_{ri}$ : $\frac{3g}{2l_{ri}}$
- $c_{ri}$ : $\frac{3c_i}{ml_{ri}^2}$
制御系設計
最初の方に述べた制御器について、具体的な設計は以下の手順で行います：
- 最適制御問題の定式化
- 数値解法（Solver）の実装
- 時変ゲインの計算
1. 最適制御問題の定式化
評価関数の設定
評価関数は以下の形式を考えます。
J = \phi(x(t_f)) + \int_{t_0}^{t_f}L(x(t), u(t), t) dt
ここで、$\phi(x(t_f))$は終端コスト、$L(x(t), u(t), t)$はランニングコストを表します。具体的には、終端コストとランニングコストを
\phi(x(t_f)) = \frac{1}{2}(x(t_f)-x_{\rm ref})^T S(x(t_f)-x_{\rm ref})
L(x(t), u(t), t) = \frac{1}{2}\{(x-x_{\rm ref})^T Q(x-x_{\rm ref}) + u^T R u\}
と2次形式で与えます。$S$は終端コストの重み行列、$Q$は状態偏差の重み行列、$R$は制御入力の重みです。この評価関数は、目標値との偏差と制御入力の大きさを同時に考慮しています。
さらに、実際には単純に上記の評価関数を最小化するのではなく、制御対象の動的特性を考慮する必要があります。そのため、ラグランジュの未定乗数法を用いて、状態方程式を制約条件として組み込みます。これにより、以下のような修正評価関数を導入します。
\bar{J} = \phi(x(t_f)) + \int_{t_0}^{t_f} \left\{L(x(t), u(t), t) + \lambda^T (f-\dot{x}) \right\}dt
上式で被積分関数として新たに追加した項は、$\dot{x}=f(x,u,t)$で制御対象が状態方程式に拘束されていることを表現しています。つまり、制御対象が物理法則(つまり運動方程式)に拘束・支配されている状況下での、評価関数の最小化を考えることとなります。この$\bar{J}$を最小化する制御入力$u(t)$の時系列を求めることが、最適制御問題です。
最適制御問題の解
最適制御問題の解の見通しを良くするために、下記のハミルトン関数$H$を定義します。
H(x, u, \lambda, t) = L(x, u, t) + \lambda^{T}f(x, u, t)
このハミルトン関数を用いると、最適制御問題の解は以下のポントリャーギンの最小原理による必要条件（状態方程式・随伴方程式・最適性条件）を満たします。
\begin{cases}
\dot{x}(t) = \left( \dfrac{\partial H}{\partial \lambda}\right)^{T}(x,u,\lambda,t) = f(x,u,t), & x(t_0) = x_0 \\
\dot{\lambda}(t) = -\left( \dfrac{\partial H}{\partial x}\right)^{T}(x,u,\lambda,t), & \lambda(t_f) = \left( \dfrac{\partial \phi}{\partial x}\right)^{T}(x(t_f)) \\
\dfrac{\partial H}{\partial u}(x,u,\lambda,t) = 0 &
\end{cases}
…なんで？ というところが最適制御の根幹をなす興味深い内容ですが、本記事では省略します。(考え方を変えると、ハミルトン・ヤコビ・ベルマン方程式などが出てきて重要ですが、本記事ではそれらも扱いません)
上式の特徴としては、
- 1行目の式は状態方程式を表しており、初期条件が決まっている。 → 初期値問題(IVP)と呼ぶ
- 2行目の式は、ラグランジュの運動方程式の拘束を表現する変数(随伴変数)の状態方程式を表しており、終端条件が決まっている。 → 終端値問題(FVP)と呼ぶ
という点が挙げられます。つまり、状態は初期値から順に積分し、随伴変数は終端値から逆向きに積分する2点境界値問題として解くのがポイントです。
2. 数値解法（Solver）の実装
初期値問題（IVP）の求解
初期値問題は4次のルンゲ・クッタ法を用いて解きます。時間ステップ$dt$における状態の更新は以下の式で行います：
\begin{align}
k_1 &= f(x_n, u_n)dt \\
k_2 &= f(x_n + \frac{k_1}{2}, u_n)dt \\
k_3 &= f(x_n + \frac{k_2}{2}, u_n)dt \\
k_4 &= f(x_n + k_3, u_n)dt \\
x_{n+1} &= x_n + \frac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4)
\end{align}
終端値問題（FVP）の求解
終端値問題における、随伴変数$\lambda$の時間発展は以下の式で与えられます：
\dot{\lambda} = -\frac{\partial H}{\partial x}
ここで、$H$はハミルトン関数で、以下のように定義されます：
H = \frac{1}{2}\{(x-x_{\rm ref})^T Q(x-x_{\rm ref}) + ru^2\} + \lambda^T f(x,u)
これに対する$\frac{\partial H}{\partial x}$はあらかじめ解析的に式を求めておき、Plant.mに記述しています。これを用いて、初期値問題とは時間軸を逆向きに解くことによって$\lambda$の時系列が求められます。
最急降下法による最適化
最適化は最急降下法を用いて行います。下記の更新則に基づき、評価関数$J$を最小化するための制御入力を算出します：
u^{k+1} = u_k + \alpha_k d_k
ここで、$d_k$は評価関数の勾配、$\alpha_k$はステップサイズです。勾配はハミルトン関数$H$の制御入力に関する偏微分から得られます：
d_k = -\frac{\partial H}{\partial u}
ステップサイズ$\alpha_k$は、探索方向$d_k$上に沿った評価関数の最小値を取る点を一次元探索することで求めています。(現状ここの計算がかなり重く、ホントはもっと良いやり方が多数あると思われます)
3. 時変ゲインの計算
最適軌道が求まった後、その軌道周りでの時変レギュレータゲインを計算します。これは微分リッカチ方程式（DRE）を後退積分することで求められます。
基本的な流れ
- 終端条件の設定：時間軸の終端でのリッカチ行列を設定
- 後退積分：DREを終端から初期時刻まで積分（時間を逆向きに進める）
- ゲイン計算：各時刻でリッカチ行列からフィードバックゲインを計算
DREの基本形は以下のようになります：
\dot{P} = PBR^{-1}B^TP - PA - A^TP - Q
ここで、$P$はリッカチ行列、$A$,$B$は線形化行列、$Q$,$R$は重み行列です。
実装上のポイント
- 4次ルンゲ・クッタ法による数値積分
- 負のタイムステップ（$-dt$）による後退積分
- 平衡点での無限時間安定化ゲインも同様の方法で計算
これにより、時変フィードバックゲイン$K(t)$が得られます。
シミュレーション結果
ここまで述べてきた最適軌道の計画と時変ゲインをもとに、動作シミュレーションをした結果が下記です。振子の角度$\phi_1$、$\phi_2$が2秒付近でπ=3.14…に向かい、それ以外はおおむねゼロになっています。アームの角度$\theta$はもろもろの誤差の影響でぴったりゼロにするのは難しいです(十分時間が経つとゼロに向かいます)。
軌道計画としては1.8秒で立たせるように設計しており、その時間までを拡大すると下記です。やはり、1.8秒で振子2つを立ち上げるような、反動をつけた動きになっていることがわかります。
Simscape Multibodyでのモデル作成
Simulinkで回転型並列二重倒立振子のモデルと、制御器を実装した内容が下図です。色を塗った部分がSimscape Multibodyを使用した、回転型並列二重倒立振子のモデルです。
Simscape Multibodyでは土台部分を原点として、土台→アーム→振子 という流れでリンク機構をつなぎました。慣れるまでは座標系で苦労しますが、Mechanics Explorerで接続部分を可視化していくと割とわかりやすいと思います。
なお、こうした振子やリンク機構を作る場合、入力の座標系→出力の座標系の変換を意識して、それらをサブシステム化しておくと扱いやすいです。下記も参照ください。
Simscape Multibodyでのモデル作成の補足
さらに本記事特有の内容を述べておくと、
↓の謎の定数はコメントにある通り、アームへの角加速度と、印可トルクをおおよそ一致させるための調整用のものです。
前述の運動方程式で「$\ddot{\theta}(t)=u(t)$で、アームの角加速度が直接制御入力で与えられる」という仮定を置きましたが、実際はそうはいきません。そこで、Simscape Multibodyのモデルへ事前にトルク「1」をアームに印可した際の角加速度を確認し、定常値が合うような定数を求めて設定しました。
また、剛体振子の場合、運動方程式には剛体としての振子の太さはでてきません。Simscape Multibodyではそれらの値を入れる必要があるので、剛体の密度を↓の式のべた書きによって帳尻合わせしています。
制御器についても補足しておくと、↓の部分で純粋な「制御器」に相当する部分はx_opt_ts：最適軌道、u_opt_ts：最適軌道を実現する制御入力、K_ts：モデル化誤差等を補償する最適ゲインだけです。上下にあるブロックはデバッグ用に作ったものを残しています。
倒立振子の制御は何かと動作が発散しがちなので、
- 振子の角度がπの状態から始めて、最適ゲインの定常値を使って安定化できるか(振子の安定化動作のみの確認)
- 指定した時刻で振子の角度がπになるような軌道生成ができているか(振り上げ軌跡のみの確認)
- 振り上げ時にはゲイン0、振り上げ完了後に最適ゲインを与えて振り上げ＆安定化できるか
これらを順番に確認したのち、最後に時変ゲインを適用する流れをふむと、問題が特定しやすかったです。
おわりに
本記事では、回転型の二重振子を振り上げて安定化するための最適制御について紹介しました。また、Simscape Multibodyを使用したモデル化についても述べました。最適制御での結果を参考にしつつ、強化学習などへトライする題材に活用できそうですね。
ちなみに今回、学生時代のC/C++コードをMATLAB化しました。当時は資料も少なくて今以上によくわかってなかったですが、非線形最適制御と一口に言っても色々な考え方があります。基本的には本記事の内容は一番初歩的・愚直に解いたもので、現代版としてはもっといいやり方が多数あるかと思います。とはいえ「Toolboxなしでここまでできる」という点が、どなたかの参考になれば幸いです。
参考文献
- 大塚敏之, "非線形最適制御入門", コロナ社, 2011