倒立振子を線形二次レギュレーター(LQR)で”簡単に”立たせる話

kogakudanshi.hatenablog.jp

前回でシミュレーターができたのでやっと制御できます。
自分で問題を作って自分で解く、虚無ですね。

振り子を立たせるだけでなく、台車位置もゼロに保つようにしてみたいと思います。入力は台車にはたらく力とします。つまり制御対象の状態変数が複数あるのに対して入力は1つの劣駆動システムになります。

運動方程式の線形化と状態方程式・観測方程式

$\left \lbrace \begin{array}{l}(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F\\ \cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0\end{array}\right.$

棒の傾きが微小の時 $\theta\rightarrow0$ を仮定して線形化します。

$\ddot{x}=-\frac{3mg}{4M+m}\theta+\frac{4}{4M+m}F$

$\ddot{\theta}=\frac{6\left(M+m\right)g}{\left(4M+m\right)l}\theta-\frac{6}{\left(4M+m\right)l}F$

状態変数を定義します。

$x=\begin{bmatrix} x \\ \theta \\ \dot{x} \\ \dot{\theta}\end{bmatrix}$

上の線形化した式を用いて状態方程式を作ります。

$\dot{x}=Ax+Bu$

また観測方程式は以下のようになります。シミュレーターなので全ての状態変数が観測できるものとします。

これでシステムが把握できたので制御を組んでいきます。

線形二次レギュレーター

LQRでは以下の評価関数を最小化するゲイン $K$ を求めます。

$J=\int_0^\infty(x^T(t)Qx(t)+u^T(t)Ru(t))dt$

$Q$ は状態に関する重み行列、 $R$ は入力に関する重み行列です。ざっくりとしたイメージでいうと状態変数 $x$ の各成分が0から離れていて、その時間が長いほど $x^T(t)Qx(t)$ の値が大きくなります。また入力 $u$ が0から離れていて、その時間が長いほど $u^T(t)Ru(t)$ の値が大きくなります。

また対角行列である $Q$ の各行が状態変数の各成分の重みになっているので、優先して収束させたい状態変数のと同じ行の対角成分を大きくすることで過渡特性をチューニングすることができます。また $Q$ に比べて $R$ の値を大きくすると多少収束に時間がかかってもいいので入力を減らせ、という省エネ命令になるわけです。

入力は以下のように状態変数にゲインをかけた値になります。

$u=-Kx$

つまりこの $K$ をうまく決めてやると $J$ がより小さくなる、つまり目標値から外れている時間が短くなり、またより少ない入力でそれを達成できるわけです。

最適ゲインの算出

それじゃあ $K$ はどのように決めたらいいのか……というふうに話が進むわけですが「簡単に」と銘打っているように、ここでは自分では求めません。当たり前です。

具体的にはmatlabにおんぶに抱っこします。

m = 1;
M = 4;
l = 4;
g = 9.8;

A = [0 0 1 0; 0 0 0 1; 0 -3*m*g/(4*M+m) 0 0; 0 6*(M+m)*g/(4*M+m)/l 0 0]
B = [0; 0; 4/(4*M+m); -6/(4*M+m)/l]
Q = eye(4)
R = 1

lqr(A,B,Q,R)

はい出ました。

簡単ですね。

4つの数に左から順に $x,\theta,\dot{x},\dot{\theta}$ をかけた値の合計を求め、-1をかけた値を制御入力 $F$ とします。前回作ったシミュレーターに制御入力を加えた結果が以下です。

これに対して先程のmatlabのシステムに同じ初期値を与えたときの応答が以下です。

上から一番目と二番目、 $x,\theta$ のグラフはほぼ一致していて、どちらも15秒前後で静定していることがわかると思います。一方で $\dot{\theta}$ はまだ誤差が残っています。もう少し動かすとゼロにはなりますが、制御モデルが線形化されているのに対してシミュレーターが非線形であるためと考えられます。

より高速に $x$ をゼロにするため $Q$ の1行1列目を1から5にしてみます。