倒立振子シミュレーション(MATLAB/Simulink)

せっかく個人用（学生用）のMATLABを買ったので，それを使ってみましょう的な記事．
制御系の研究ではお馴染みの倒立振子のシミュレーションをやってみます．

動画はシミュレーションの様子．
実は，ToolBoxにSimscapeというものがありまして，その中のSimMechanicsではMATLAB上で3Dソリッドモデルを作るだけで物理シミュレーションができたりします．

ただし，今回は普通のSimulinkのみで行います（笑）

やっぱり解析的に色々するには，どちらにしろ運動方程式も立てないといけないので．
大体の場合において，制御をしようと思うと，運動方程式のパラメータから
制御系の設計パラメータを求める必要が出てきます．
そんな理由から，運動方程式を立てて，制御パラメータを決めていく方法で今回は制御系の設計を行います．

まずは倒立振子の運動方程式から．

倒立振子にも色々ありますが，今回は車輪型についてです．
製品では，ヴィストン株式会社のbeuto balancerなどがあります．
下の図のように，車輪と振り子部分から構成されていて，下の車輪が動くことで上の本体のバランスをとって倒立させるというものです．

制御すべきは，本体の位置と角度で，入力は車輪にあるモータへのトルク入力となっています．
つまりは，モータのトルク入力（$\simeq$電流入力）を与えて，倒立振子を倒さずに好きなところに移動させるという制御です．
ここで，$\theta$は本体の地面に対する角度，$\phi$は初期タイヤ角度からの変位量です．
$\phi$に関しては，ロボットの特性上，角度センサで地面との相対角度を取得して，ロータリーエンコーダでタイヤの回転角度を取得することが多いのでこのような設定としています．
（ロータリーエンコーダでは基本的にプログラム実行時の初期角度からの変化量しか取得できないので．）

この運動方程式の導出方法はいろいろあるかと思いますが，今回はラグランジュ法を使用していきます．

1. まず，タイヤの回転軸中心$(x_c, y_c)$に関しては次の関係が得られます．
   $$\begin{equation}\left\{ \begin{array}{ll} x_c = r(\theta + \phi) & \dot{x}_c = r(\dot{\theta} + \dot{\phi})\\ y_c = 0 & \dot{y}_c = 0 \end{array}\right.\end{equation}$$
2. 次に，振子部分の重心位置$(x_p, y_p)$について，以下の関係が得られます．
   $$\begin{equation}\left\{\begin{array}{ll} x_p = x_c+l\sin\theta & \dot{x}_p = r(\dot{\theta} + \dot{\phi}) + l\dot{\theta}\cos\theta \\ y_p = y_c+l\cos\theta & \dot{y}_{p} = -l\dot{\theta}\sin\theta \end{array}\right.\end{equation}$$
3. これらの関係から，振り子部分の並進方向の運動エネルギ$K_{p1}$は，
   $$\begin{equation}\begin{aligned} K_{p1} &= \frac{1}{2}m(\dot{x}_p^2 + \dot{y}_p^2) \\ &= \frac{1}{2}m\left(r^2(\dot{\theta}+\dot{\phi})^2+2lr(\dot{\theta}+\dot{\phi})\dot{\theta}\cos\theta+l^2\dot{\theta}^2\cos^2\theta+l^2\dot{\theta}^2\sin^2\theta\right) \\ &=\frac{1}{2}m\left(r^2\dot{\theta}^2+2lr\dot{\theta}^2\cos\theta+2lr\dot{\phi}\dot{\theta}\cos\theta+2r^2\dot{\theta}\dot{\phi}+r^2\dot{\phi}^2 \right.\\ &+\left. l^2\dot{\theta}^2\cos^2\theta+l^2\dot{\theta}^2\sin^2\theta \right) \end{aligned} \end{equation}$$
4. 振り子の回転方向の運動エネルギ$K_{p2}$は，
   $$\begin{equation} K_{p2} = \frac{1}{2}J_p\dot{\theta}^2 \end{equation}$$
5. タイヤの並進方向の運動エネルギ$K_{c1}$は，
   $$\begin{equation} \begin{aligned} K_{c1} &= \frac{1}{2}M(\dot{x}_c^2 + \dot{y}_c^2) \\ &= \frac{1}{2}M\left( r^2(\dot{\theta}+\dot{\phi})^2 \right) \\ &= \frac{1}{2}Mr^2\dot{\theta}^2+Mr^2\dot{\theta}\dot{\phi}+\frac{1}{2}Mr^2\dot{\phi}^2 \end{aligned} \end{equation}$$
6. タイヤの回転方向の運動エネルギ$K_{c2}$は，
   $$\begin{equation} K_{c2} = \frac{1}{2}J_c(\dot{\phi}+\dot{\theta})^2 \end{equation}$$
7. モータ電機子の回転エネルギ$K_{c3}$は以下のように求められます．
   ここで，$i$はモータの減速比です．
   $$\begin{equation} K_{c3} = \frac{1}{2}J_m(i\dot{\phi}+\dot{\theta})^2 \end{equation}$$

よって，系全体の運動エネルギ$K$，位置エネルギ$U$，散逸エネルギ$F$は，以下のように得られます．

$$\begin{equation} \begin{aligned} K &= K_{p1}+K_{p2}+K_{c1}+K_{c2}+K_{c3} \\ U &= mgy_p+Mgy_c \\ F &= \frac{1}{2}c\dot{\phi}^2 \end{aligned} \end{equation}$$

ここから，ラグランジアン$L=K-U$を求めます．（長い）

$$\begin{equation} \begin{aligned} L &= K - U \\ &= \frac{1}{2}m\left( r^2\dot{\theta}^2+2r^2\dot{\theta}\dot{\phi}+r^2\dot{\phi}^2+l^2\dot{\theta}^2\cos^2\theta +l^2\dot{\theta}^2\sin^2\theta \right) \\ &+ \frac{1}{2}J_p\dot{\theta}^2+\frac{1}{2}Mr^2\dot{\theta}^2+Mr^2\dot{\theta}\dot{\phi}+\frac{1}{2}Mr^2\dot{\phi}^2 \\ &+ \frac{1}{2}J_c(\dot{\phi}+\dot{\theta})^2+\frac{1}{2}J_m(i\dot{\phi}+\dot{\theta})^2-mgl\cos\theta+mlr\dot{\theta}^2\cos\theta+mlr\dot{\phi}\dot{\theta}\cos\theta \\ \\ &= \frac{1}{2}m\left( r^2\dot{\theta}^2+2r^2\dot{\theta}\dot{\phi}+r^2\dot{\phi}^2+l^2\dot{\theta}^2\cos^2\theta +l^2\dot{\theta}^2\sin^2\theta \right) \\ &+ \frac{1}{2}J_p\dot{\theta}^2+\frac{1}{2}Mr^2\dot{\theta}^2+Mr^2\dot{\theta}\dot{\phi}+\frac{1}{2}Mr^2\dot{\phi}^2 \\ &+\frac{1}{2}Jc(\dot{\phi}^2+2\dot{\phi}\dot{\theta}+\dot{\theta}^2)+\frac{1}{2}J_m(i^2\dot{\phi}^2+2i\dot{\phi}\dot{\theta}+\dot{\theta}^2) \\ &-mg\cos\theta+mlr\dot{\theta}^2\cos\theta+mlr\dot{\phi}\dot{\theta}\cos\theta \end{aligned} \end{equation}$$

これより，ラグランジアンの変数ごとの偏微分を求めます．

まずは変数$\phi$について．
$$\begin{equation} \begin{aligned} \frac{\partial L}{\partial \dot{\phi}} &= mr^2\dot{\theta}+mr^2\dot{\phi}+Mr^2\dot{\theta}+Mr^2\dot{\phi}+J_c\dot{\phi}+J_c\dot{\theta}+i^2J_m\dot{\phi}+iJ_m\dot{\theta} \\ &+mlr\dot{\theta}\cos\theta \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right) &= mr^2\ddot{\theta}+mr^2\ddot{\phi}+Mr^2\ddot{\theta}+Mr^2\ddot{\phi}+J_c\ddot{\phi}+J_c\ddot{\theta}+i^2J_m\ddot{\phi}+iJ_m\ddot{\theta} \\ &+mlr\ddot{\theta}\cos\theta-mlr\dot{\theta}^2\sin\theta \\ \frac{\partial L}{\partial \phi} &= 0 \\ \frac{\partial F}{\partial \dot{\phi}} &= c\dot{\phi} \end{aligned} \end{equation}$$
次に変数$\theta$について．
$$\begin{equation} \begin{aligned} \frac{\partial L}{\partial \dot{\theta}} &= mr^2\dot{\theta}+mr^2\dot{\phi}+ml^2\dot{\theta}\cos^2\theta+ml^2\dot{\theta}^2\sin^2\theta+J_p\dot{\theta}+Mr^2\dot{\theta}+Mr^2\dot{\phi} \\ &+J_c\dot{\phi}+J_c\dot{\theta}+iJ_m\dot{\phi}+J_m\dot{\theta}+2mlr\dot{\theta}\cos\theta+mlr\dot{\phi}\cos\theta \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) &= mr^2\ddot{\theta}+mr^2\ddot{\phi}+ml^2\ddot{\theta}\cos^2\theta-2ml^2\dot{\theta}^2\sin\theta\cos\theta+ml^2\ddot{\theta}\sin^2\theta \\ &+2ml^2\dot{\theta}^2\cos\theta\sin\theta+J_p\ddot{\theta}+Mr^2\ddot{\theta}+Mr^2\ddot{\phi} \\ &+J_c\ddot{\phi}+J_c\ddot{\theta}+iJ_m\ddot{\phi}+J_m\ddot{\theta}+2mlr\ddot{\theta}\cos\theta-2mlr\dot{\theta}^2\sin\theta \\ &+mlr\ddot{\phi}\cos\theta-mlr\dot{\phi}\dot{\theta}\sin\theta \\ \frac{\partial L}{\partial \theta} &= -ml^2\dot{\theta}^2\cos\theta\sin\theta+ml^2\dot{\theta}^2\cos\theta\sin\theta+mgl\sin\theta-mlr\dot{\theta}^2\sin\theta \\ &-mlr\dot{\phi}\dot{\theta}\sin\theta \\ \frac{\partial F}{\partial \dot{\theta}} &= 0 \end{aligned} \end{equation}$$

これまでにラグランジアンの偏微分が全て求まったので，運動方程式を導出します．

まず，変数$\phi$についての式から，
$$\begin{equation} \begin{aligned} &\left((M+m)r^2+mlr\cos\theta+J_c+iJ_m \right)\ddot{\theta}-mlr\sin\theta\cdot\dot{\theta}^2 \\ &+\left((M+m)^2+J_c+i^2J_m\right)\ddot{\phi}+c\dot{\phi}=au \end{aligned} \end{equation}$$
次に，変数$\theta$についての式から，
$$\begin{equation} \begin{aligned} &\left((M+m)r^2+2mlr\cos\theta+ml^2+J_p+J_c+J_m\right)\ddot{\theta}-mlr\dot{\theta}^2\sin\theta-mgl\sin\theta \\ &+\left((M+m)r^2+mlr\cos\theta+J_c+iJ_m\right)\ddot{\phi}=0 \end{aligned} \end{equation}$$

これで，車輪型倒立振子の運動方程式の導出が完了しました．
ただし，$u$はモータに流れる電流，$a$はモータのトルク定数であり，$au$は入力トルクを表します．

このままでは使い勝手が悪いので，まずは倒立状態$(\theta\simeq0)$付近かつ，静止状態$(\dot{\theta}^2\simeq0)$で線形化します．
このとき，$\sin\theta\simeq\theta$，$\cos\theta\simeq1$です．
$$\begin{equation} \left\{\begin{array}{l} \left((M+m)r^2+mlr+J_c+iJ_m \right)\ddot{\theta}+\left((M+m)^2+J_c+i^2J_m\right)\ddot{\phi}+c\dot{\phi}=au \\ \\ \left((M+m)r^2+2mlr+ml^2+J_p+J_c+J_m\right)\ddot{\theta}-mgl\theta \\+\left((M+m)r^2+mlr+J_c+iJ_m\right)\ddot{\phi}=0 \end{array} \right. \end{equation}$$
次に，状態方程式（行列形式）ベースに書き直します．
$$\begin{equation} \left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]\left[\begin{array}{c} \ddot{\theta} \\ \ddot{\phi} \end{array}\right]+\left[\begin{array}{cc} 0 & c \\ 0 & 0 \end{array}\right]\left[\begin{array}{c} \dot{\theta} \\ \dot{\phi} \end{array}\right]+\left[\begin{array}{cc} 0 & 0 \\ -mgl & 0 \end{array}\right]\left[\begin{array}{c} \theta \\ \phi \end{array}\right]=\left[\begin{array}{c} a \\ 0 \end{array}\right]u \end{equation}$$
ただし，
$$\begin{equation} \begin{aligned} \alpha_{11} &= (M+m)r^2+mlr+J_c+iJ_m \\ \alpha_{12} &= (M+m)r^2+J_c+i^2J_m \\ \alpha_{21} &= (M+m)r^2+2mlr+ml^2+J_p+J_c+J_m \\ \alpha_{22} &= (M+m)r^2+mlr+J_c+iJ_m \end{aligned} \end{equation}$$
更に変形して，
$$\begin{equation} \begin{aligned} \left[\begin{array}{c} \ddot{\theta} \\ \ddot{\phi} \end{array}\right]&=-\left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{cc} 0 & c \\ 0 & 0 \end{array}\right]\left[\begin{array}{c} \dot{\theta} \\ \dot{\phi} \end{array}\right]-\left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{cc} 0 & 0 \\ -mgl & 0 \end{array}\right]\left[\begin{array}{c} \theta \\ \phi \end{array}\right] \\ &+\left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{c} a \\ 0 \end{array}\right]u \end{aligned} \end{equation}$$
これらをまとめると，以下の状態方程式が得られます．
$$\begin{equation} \begin{aligned} \left[\begin{array}{c} \dot{\theta} \\ \dot{\phi} \\ \ddot{\theta} \\ \ddot{\phi} \end{array}\right]&=\left[\begin{array}{cc} 0 \hspace{6em} 0 & \hspace{2em} 1 \hspace{5em} 0 \\ 0 \hspace{6em} 0 & \hspace{2em} 0 \hspace{5em} 1 \\ \left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{cc} 0 & 0 \\ -mgl & 0 \end{array}\right]& -\left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{cc} 0 & c \\ 0 & 0 \end{array}\right] \end{array}\right]\left[\begin{array}{c} \theta \\ \phi \\ \dot{\theta} \\ \dot{\phi} \end{array}\right] \\ &+\left[\begin{array}{c} 0 \\ 0 \\ \left[\begin{array}{cc} \alpha_{11} & \alpha_{12} \\ \alpha_{21} & \alpha_{22} \end{array}\right]^{-1}\left[\begin{array}{c} a \\ 0 \end{array}\right] \end{array}\right]u \end{aligned} \end{equation}$$
ただし，
$m=0.686$[kg]，$M=0.0605$[kg]，
$J_p=3.16\times10^{-3}$[kg$\cdot$m$^{2}$]，$J_c=5.35\times10^{-6}$[kg$\cdot$m$^{2}$]，$J_m=1.30\times10^{-7}$[kg$\cdot$m$^{2}$]，
$l=0.148$[m]，$r=0.02$[m]，$c=1.00\times10^{-4}$，$i=30$，$a=5.4580$

次に，制御系の設計について．

今回は単純な状態フィードバックを使用しますが，この場合，全状態変数が観測できる必要があります．
現実には，ロボット本体の角度$\theta$とタイヤの角度$\phi$を測定し，他の2変数は推定することが多いかと思いますが，
シミュレーションですので，ここでは全状態変数が観測可能なものとして扱います．
つまり，状態方程式の$C$および$D$の項は以下のとおりです．
$$\begin{equation} C=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right],D=\left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \end{array}\right] \end{equation}$$
状態フィードバックの係数は，最適制御で行います．
最適制御では，各状態変数と入力に対する「重み」を設定して，以下の評価関数が最小となるような状態フィードバックの係数を求めます．
$$\begin{equation} \int_0^\infty\left(x^{T}Qx+u^{T}Ru\right)dt \end{equation}$$
状態変数に対する重みは$Q$，入力に対する重みは$R$で表すことが多いかと思います．
今回は，例として以下の$Q$と$R$を設定しました．
$$\begin{equation} Q = \left[\begin{array}{cccc} 100 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right], R = 500 \end{equation}$$
このように表記した場合，$Q$の左上からそれぞれ，$\theta$，$\phi$，$\dot{\theta}$，$\dot{\phi}$に対する重みとなっています．
つまり，車体の角度$\theta$を素早くゼロに近づけ，安定化させることを重要視した重み，ということになります．
また，$R$は入力に対する重みで，これを大きくすると入力をできるだけ小さく抑えたい，ということに相当します．
（このあたりの記述はあまり正確ではないかもしれません．）
MATLABを使う場合，この最小化は関数として用意されているので，次の記述だけで済みます．

gain = lqr(A, B, Q, R);

この$A$，$B$は状態方程式の$A$行列と$B$行列です．
ここから，状態フィードバックの係数がgainとして得られます．

gain = [-6.6700 -0.0447 -1.0000 -0.0583]

初期角度を5 deg.として安定化させ，5秒後にタイヤ4回転分だけ前に進めた時のシミュレーションが以下のようになります．

しっかりと安定化でき，目標の位置にも追従できていることがわかります．
12秒時点で入力を切っているため，そこからは転倒してしまっています．
トップの動画は，これをProcessingで可視化したものです．
倒立振子の安定化制御はわりと色んな所で見るので試しにやってみた次第です．
近いうちに実機の方にも実装できるといいなあ．