数式処理ソフトウェアで立式した運動方程式をSimulinkで実行する

この記事は制御工学アドベントカレンダー15日目の記事です．

制御工学で避けては通れないのが運動方程式の立式です．
プラントモデリングではもちろんのこと，現代制御に代表されるモデルベース制御では制御ロジックを構築するために運動方程式（状態方程式）が必要になります．

運動方程式の立式というと，ほぼラグランジュ法（オイラー＝ラグランジュ方程式）一択のような気がしています．

いまいちこのあたりの用語を掴みきれていないのですが，汎函数と呼ばれる“函数を変数とした函数”を変分（汎函数での微分）することで運動方程式を求めるといった流れです．

たとえば，$f(t) = at$は函数と呼ばれ，$\frac{df(t)}{dt}$という操作が微分と呼ばれることはご存知のとおりです．
それと似たような関係で，$I[f(t)] = Af(t)$を汎函数と呼び，$\frac{dI[f(t)]}{df(t)}$を変分と呼ぶようです．

物体の運動の場合，最も必要エネルギの少ない方向に物体が運動するという原理があるらしく（最小作用の原理？），以下の形式で与えられます．

$$I[ y ] = \int^{b}_{a}f( x, y, y' ) dx$$

特に運動の場合は，右辺$f(x,y,y')$をラグランジアン$L$というエネルギ量で与えるようです．
合っている自信はないですが，以下の式のようなイメージです．（時間微分なのでドットで書いています）

$$I[ x ] = \int^{b}_{a}L( t, x, \dot{x} ) dt$$

これはある時刻$a$から$b$までのエネルギの合計となります．
上の式が最小になるようにすれば良いということで，$I[x]$の変分（微分）がゼロとなるような$x$（停留値）を求めれば良いことになります．

この解（停留値）は決まっていて，これをオイラー＝ラグランジュ方程式と呼びます．
$$\frac{d}{dt}\left( \frac{\partial L}{\partial \dot{x}} \right) - \left( \frac{\partial L}{\partial x} \right) = 0$$

解の形式が決まっているのでシーケンシャルに解けて便利なのですが，非常に計算量が多いです．

前置きが長くなりましたがこれを数式処理ソフトウェアで楽にしようというのが今回のコンセプトです．

---

この記事では数式処理ソフトウェアとして以下を使用しています．

• wxMaxima（Windows版Maxima，無償）
• Symbolic Math Toolbox（MATLAB，有償）

恐らく運動方程式を求めたあとはMATLAB/SimulinkとかPyControlとかでシミュレーションすることになると思うのですが，結論としてはwxMaxima優位といった印象です．

---

今回，2重振り子を例として，Simulinkでのシミュレーションまでやってみようかと思います．

ラグランジュ法では物体の運動エネルギ，位置エネルギの関係を与えてあげることが必要になります．
逆に言うとエネルギまで出せてしまえばあとはソフトウェア任せにできます．

事前にやってみたところ，wxMaximaのインストールから結果確認まで45分程度で終わりました．

---

まずはエネルギ（ラグランジアン）の導出です．
このあたりは以前の倒立振子シミュレーション(MATLAB/Simulink)とほぼ同じです．

$l_{gj}$は回転軸から重心までの距離，$l_{j}$は回転軸からリンク先端までの距離，$m_{j}$は質量，$J_{j}$は重心周りの慣性モーメント，$g$は重力加速度です．
添字1はリンク1（根元側），添字2はリンク2（先端側）に関する変数または定数です．

重心位置
$$x_{g1} = l_{g1} \sin\theta_{1} \\ y_{g1} = l_{g1} \cos\theta_{1} \\ x_{g2} = l_{1} \sin\theta_{1} + l_{g2} \sin( \theta_{1} + \theta_{2} ) \\ y_{g2} = l_{1} \cos\theta_{1} + l_{g2} \cos( \theta_{1} + \theta_{2} )$$

ただし，上向きが$\theta_{j}=0$です．

運動エネルギ
$$K_{1} = \frac{1}{2} m_{1} \dot{ x_{g1} }^2 + \frac{1}{2} m_{1} \dot{ y_{g1} }^2 + \frac{1}{2} J_{1} \dot{ \theta_{1} }^2 \\ K_{2} = \frac{1}{2} m_{2} \dot{ x_{g2} }^2 + \frac{1}{2} m_{2} \dot{ y_{g2} }^2 + \frac{1}{2} J_{2} ( \dot{ \theta_{1} } + \dot{ \theta_{2} ) }^2 \\ K = K_{1} + K_{2}$$

位置エネルギ
$$U_{1} = m_{1}gy_{g1} \\ U_{2} = m_{2}gy_{g2} \\ U = U_{1} + U_{2}$$

ラグランジアン
$$L = K - U$$

さて，自力で出す必要があるのはここまでです．
手でやるとここからが非常に長いですが，ソフトウェアの力を借ります．

---

Symbolic Math Toolboxで解く

MATLABのToolboxのひとつであるSymbolic Math ToolboxにはMuPADというソフトウェアが入っています．
これを使っていきます．

MATLABスクリプト内でも代数計算はできるのですが，上記のようなオイラー＝ラグランジュ方程式の求解はできませんでした．
具体的には"加速度について連立して解けない"と言った感じです．（運動方程式自体は出る）
このあたり方法をご存じの方いましたらご教示ください．

MuPADでは，“数式モード”と“テキストモード”があります．
テキストモードでいくら数式を書いても文字として表示されるだけです．
Ctrl+Iで数式モードに変えましょう．

• Ctrl+Iで数式モードに変わります．Ctrl+Tでテキストモードに変わります．

• 数式モードではReturnキー（Enterキー）で数式を確定します．
  Shift+Returnで式を確定せずに改行できます．
  文末はセミコロン;です．

• 殆どの式はa:=bのように:=を利用して代入します．

• a[1]とすることで，$a_{1}$のような下付き文字を作れます．
  このようにすることでMATLABコードにした際にa(1)のように配列で出力されます．

• f(t)のように宣言すると，f()がtの函数であることを記述できます．
  運動方程式の変数は全てこのように宣言します．

---

変数や定数は，先ほどの図のように宣言します．

th1 := th[1](t);
lg1 := lg[1];

重心位置は，求めた式から以下のように記述します．

xg1 := lg1 * sin( th1 );

lg1，th1は上で定義しているため，Returnすると代入した結果が表示されます．

速度$\dot{x_{gj}}(t)$は，角度$x_{gj}(t)$の時間$t$による微分であるため，diff()函数を使って記述します．

dxg1 := diff( xg1, t );

角速度についても同様です．

dth1 := diff( th1, t );

ついでに角加速度も宣言しておきます．
最後に項をまとめるためです．

ddth1 := diff( dth1, t );

---

運動エネルギは上記で求めた速度，角速度を使って表します．

K1 := (1/2) * m1 * dxg1^2 + (1/2) * m1 * dyg1^2 + (1/2) * J1 * dth1^2;

位置エネルギについても同様です．

U1 := m1 * g * yg1;

さいごに，ラグランジアンを宣言します．

L := K - U

---

ここから，$\theta_{1}(t)$について解きます．
実はMuPADでは，上記で述べたようなオイラー＝ラグランジュ方程式を直接解くことができません．

速度による微分$\frac{\partial L}{\partial \dot{x}}$ができないのです．
このため，元の汎函数の解として運動方程式を求めます．

functinoalDerivative( f, y )という汎函数の微分用MATLAB函数を使用します．

MathWorksのウェブページ
functionalDerivative - MathWorks

ここではLがfに相当し，th1がyに相当します．つまり次のように記述します．
なぜか符号が逆向きに出てくるのでマイナスを付けておきます．

buff1 := -functionalDerivative( L, th1 );

式が複雑怪奇なので項をまとめます．

Eq1 := collect( simplify( buff1 ), [ ddth1, dth1, th1, ddth2, dth2, th2 ] );

ここまでで，以下のような重心1周りの運動方程式が得られました．

重心2周りの運動方程式はth2について同様に解くことで得られます

buff2 := -functionalDerivative( L, th2 );
Eq2 := collect( simplify( buff2 ), [ ddth1, dth1, th1, ddth2, dth2, th2 ] );

運動方程式という意味では，これで完成です．

---

ただ，このままSimulinkで組むと代数ループになってしまいます．
上の式をよくみるとひとつの式に加速度であるddth1の項とddth2の項が存在しています．

重心1周りの運動方程式と重心2周りの運動方程式を連立させて，ddth1とddth2について解いて上げる必要があります．
（実はSimulinkでは代数ループでも回せますが，速度が遅くなるなどのデメリットがあります）

式を連立させて解くにはsolve( eq, x )函数を使います．
それぞれの運動方程式がEq1，Eq2として求められているため，これをddth1，ddth2について解きます．

answers := solve( [ Eq1, Eq2 ], [ ddth1, ddth2 ] );

実はこの式は解くととんでもないことになります．

頭が痛くなってきました．
MuPADでは，一定の変数について値域を定めて簡略化するという処理を挟むようです．

たとえば$f(x,y)=ax+by$が解であるとした場合，以下のように場合分けして処理するようです．
（詳細は未検証ですが）
$$\begin{array}{lll} f( x ) = ax, & if & b = 0 \\ f( y ) = by, & if & a = 0 \\ f( x, y ) = ax + by, & & otherwise \end{array}$$

このため，解の中から適切なものを選択する必要があります．
大体の場合，$\sigma_{j} \neq 0$となっている解が良さそうです．

このあたりがwxMaxima推しの要因です．

---

今回は上から2つめの式が良さそうなので，これを使います．
answers変数の構造がややこしいのですが，answers[2][1][1][1]がddth1（つまり左辺），answers[2][1][1][2]がddth1について解かれた解（つまり右辺）へのアクセス方法となります．

同様に，answers[2][1][2][2]がddth2について解かれた解になります．

最後に，これらを綺麗にまとめます．

eqth1 := collect( simplify( answers[2][1][1][2] ), [ dth1, dth2 ] );
eqth2 := collect( simplify( answers[2][1][2][2] ), [ dth1, dth2 ] );

ようやく望みの解が見つかりました．

これをMATLAB函数形式で出力します．

generate::MATLAB( eqth1, NoWarning );

"  t0 = (sin(th(2)(t)*2.0)*diff(th(1)(t),t)^2*l(1)^2*lg(2)^2*m(2)^2+g*sin(th(1)(t))*l(1)*lg(2)^2*m(2)^2-g*sin(th(1)(t)+th(2)(t)*2.0)*l(1)*lg(2)^2*m(2)^2+sin(th(2)(t))*diff(th(1)(t),t)^2*l(1)*lg(2)^3*m(2)^2*2.0+sin(th(2)(t))*diff(th(2)(t),t)^2*l(1)*lg(2)^3*m(2)^2*2.0+g*sin(th(1)(t))*J(2)*l(1)*m(2)*2.0+g*sin(th(1)(t))*J(2)*lg(1)*m(1)*2.0+g*sin(th(1)(t))*lg(1)*lg(2)^2*m(1)*m(2)*2.0+sin(th(2)(t))*diff(th(1)(t),t)*diff(th(2)(t),t)*l(1)*lg(2)^3*m(2)^2*4.0+sin(th(2)(t))*diff(th(1)(t),t)^2*J(2)*l(1)*lg(2)*m(2)*2.0+sin(th(2)(t))*diff(th(2)(t),t)^2*J(2)*l(1)*lg(2)*m(2)*2.0+sin(th(2)(t))*diff(th(1)(t),t)*diff(th(2)(t),t)*J(2)*l(1)*lg(2)*m(2)*4.0)/(J(1)*J(2)*2.0+J(2)*l(1)^2*m(2)*2.0+J(2)*lg(1)^2*m(1)*2.0+J(1)*lg(2)^2*m(2)*2.0+l(1)^2*lg(2)^2*m(2)^2+lg(1)^2*lg(2)^2*m(1)*m(2)*2.0-cos(th(2)(t)*2.0)*l(1)^2*lg(2)^2*m(2)^2);\n"

上のような出力が得られるので，先頭のt0や最後の\nなどを削除してMATLAB Function内に書き込みます．
また，diff( th(1)(t), t )は微分の意味であるため，エディタの置換などを利用してdth1にでも変えておきます．

自分は便宜上次のように置換しています．
結果はwxMaximaと一緒に下の方で述べます．

th(1)(t) -> th1
th(2)(t) -> th2
diff(th(1)(t),t) -> dth1
diff(th(2)(t),t) -> dth2
(1) -> 1
(2) -> 2

長い戦いでしたがこれにて完成です．

---

wxMaximaで解く

次に無償の数式処理ソフトウェアであるwxMaximaで同じことをやってみます．
Windows, Linux, Macintoshすべてに環境があるのでなかなかよいです．
（Windows版のMaximaをwxMaximaと呼ぶようです）

wxMaximaのウェブページ
Maxima, a Computer Algebra System - Maxima

MaximaでもMuPADと同じく，“数式モード”と“テキストモード”があります．
普通に書くと数式モードなので，テキストを書きたいときにはCtrl+1でテキストモードに変えましょう．

• 普通に記述数と数式モードです．Ctrl+Tでテキストモードに変わります．

• 数式モードではShift+Returnキー（Shift+Enterキー）で数式を確定します．
  Returnで式を確定せずに改行できます．（MuPADと逆です！）

• 文末はセミコロン;です．
  セミコロンの前にドルマーク$を入れるとその行は非表示出力できます．

• 殆どの式はa:bのように:を利用して代入します．
  これもMuPADとは違うため注意が必要です．

• a_1とすることで，$a_{1}$のような下付き文字を作れます．
  コードにした際にはa_1と出力されます．

• depends( th1, t )のように宣言すると，th1がtの函数であることを記述できます．
  運動方程式の変数は全てこのように宣言します．非常に重要．

---

変数や定数は，先ほどの図のように宣言します．

th1 : theta_1;
depends( th1, t );
lg1 : l_g1;

重心位置は，求めた式から以下のように記述します．

xg1 : lg1 * sin( th1 );

lg1，th1は上で定義しているため，Shift+Returnすると代入した結果が表示されます．

速度$\dot{x_{gj}}(t)$は，角度$x_{gj}(t)$の時間$t$による微分であるため，diff()函数を使って記述します．

dxg1 : diff( xg1, t );

角速度についても同様です．

dth1 : diff( th1, t );

wxMaximaでは特に角加速度を宣言する必要はありません．

---

運動エネルギは上記で求めた速度，角速度を使って表します．

K1 : (1/2) * m1 * dxg1^2 + (1/2) * m1 * dyg1^2 + (1/2) * J1 * dth1^2;

位置エネルギについても同様です．

U1 : m1 * g * yg1;

さいごに，ラグランジアンを宣言します．

L : K - U;

---

ここから，$\theta_{1}(t)$について解きます．
wxMaximaでは速度での微分$\frac{\partial L}{\partial \dot{x}}$が可能なため，オイラー＝ラグランジュ方程式そのままで記述します．

diff( diff( L, dth1 ), t ) - diff( L, th1 );
Eq1 : trigrat( % );

• trigrat()は三角函数を簡約化するための函数です．
  wxMaximaでは三角函数だけは自動で簡約化されないようなので覚えておくと便利です．

• %は直前の計算結果を指します．

これで上図のような結果が得られます．
$\theta$などの記号が使えるので見やすくていいですね．

---

次にこれを加速度について解きます．
非常に簡単で，以下の1行で済みます．

answers : solve( [ Eq1, Eq2 ], [ diff( dth1, t ), diff( dth2, t ) ] );

ありがたいことに一意解です．

---

最後にこの運動方程式のコード化ですが，数式の部分をコピーして貼り付けると以下のような状態で貼り付けられます．

(g*l_1*l_g2^2*m_2^2*cos(theta_2)*sin(theta_2+theta_1)+(-l_1*l_g2^3*m_2^2-J_2*l_1*l_g2*m_2)*sin(theta_2)*('diff(theta_2,t,1))^2+(-2*l_1*l_g2^3*m_2^2-2*J_2*l_1*l_g2*m_2)*('diff(theta_1,t,1))*sin(theta_2)*('diff(theta_2,t,1))+((-l_1*l_g2^3*m_2^2-J_2*l_1*l_g2*m_2)*('diff(theta_1,t,1))^2-l_1^2*l_g2^2*m_2^2*('diff(theta_1,t,1))^2*cos(theta_2))*sin(theta_2)+(-g*l_1*l_g2^2*m_2^2+(-g*l_g1*l_g2^2*m_1-J_2*g*l_1)*m_2-J_2*g*l_g1*m_1)*sin(theta_1))/(l_1^2*l_g2^2*m_2^2*cos(theta_2)^2-l_1^2*l_g2^2*m_2^2+(-l_g1^2*l_g2^2*m_1-J_1*l_g2^2-J_2*l_1^2)*m_2-J_2*l_g1^2*m_1-J_1*J_2)

先程と同様に，diff()などを置換します．

theta_1 -> th1
theta_1 -> th2
'diff(theta_1,t,1) -> dth1
'diff(theta_2,t,2) -> dth2
_（アンダースコア） -> 消去

wxMaxima編もこれで終了です．
こちらの方が個人的に使い易く感じました．

---

Simulinkで実装してみます

単にMATLAB Function内にコードを書き込んだだけです．
ScopeはMuPADで出力した結果とwxMaximaで出力した結果を重ねたものですが，両者に全く差はありません．
運動自体が合っているかはともかく，ソフトウェアの出力としては合っているようです．

これまでwxMaximaを毛嫌いしていた（一度挫折した）のですが，これからはどんどん使っていこうと思います．

---

この結果はSimulink Project形式でGithubにアップロードしています．
DoublePendulum

---

他の対象に適用しようとする人へ
今回はあえて粘性（散逸エネルギ）を排除しているのですが，理由としてはMATLABのfunctionalDerivative()で散逸エネルギが考慮できないためです．

散逸エネルギを考慮したオイラー＝ラグランジュ方程式は以下ですが，functionalDerivative()の対象となる形式ではなくなっています．

$$\frac{d}{dt}\left( \frac{\partial L}{\partial \dot{x}} \right) - \left( \frac{\partial L}{\partial x} \right) + \left( \frac{\partial F}{\partial \dot{x}} \right) = 0$$

統一的に扱う理論もあるようなのですが，よく分かっていないのと込み入った話になりそうだったので排除しました．
wxMaximaでは速度での微分が可能なため，独立に$F$を立てて$\dot{x}$で微分すれば大丈夫です．

結局，粘性は$\frac{1}{2}c\dot{x}^2$とすることが殆どだと思うので，手計算で$\frac{\partial F}{\partial \dot{x}}=c\dot{x}$と与えてしまえばそれまでですが．

見た目や汎用性の面でもwxMaximaをおすすめしておきます．