目次
はじめに
最近、ロボットの制御や経路生成の勉強をしているのですが、
しばしば出てくる技術として、
線形二次レギュレータ(Linear-Quadratic Regulator:LQR)があります。
今回はこのLQRの概要とLQRによる
簡単なPython制御シミュレーションコードを紹介したいと思います。
LQRの概要
LQRは最適制御と呼ばれれる制御手法の一つです。
下記のような線形システムに対して、
下記のようなコスト関数を最小するための最適入力u*を計算することができます。
最適入力u*は状態ベクトルxに対するゲインKを使って、
下記の式で計算できます。
このゲインKは下記の式で計算できます
まず初めにリッカチ方程式を解きます。
上記のコスト関数におけるリッカチの微分方程式の解き方としては、
Xの初期値をQとして、
下記の式でXを複数回更新し、
Xが変化しなくなるまで計算した値を利用します。
解のXの値から、下記の式でゲインを計算できます。
つまり、自分の解きたい問題に対して、
上手く上記の式にモデルを設定することで、
解析的に最適入力を計算することができるのです。
一つ重要な点として、
線形方程式のA,B,Cの要素やコスト行列が変化しない場合、
LQRのゲインは変わりません。
従って、下記のシミュレーションでも実施している通り、
リッカチの方程式を解いてゲインを計算するのは、
モデルが変わらない場合、初めの一回だけでOKです。
Kと現在の状態x_tを掛け合わせるだけで、
その時々の最適入力を計算することができます。
PythonによるLQRの制御シミュレーション
下記が簡単なLQRのPython制御シミュレーションのコードです。
""" Linear-Quadratic Regulator sample code author Atsushi Sakai """ import matplotlib.pyplot as plt import numpy as np import scipy.linalg as la simTime = 3.0 dt = 0.1 # x[k+1] = Ax[k] + Bu[k] # y[k] = Cx[k] A = np.matrix([[1.1, 2.0], [0, 0.95]]) B = np.matrix([0.0, 0.0787]).T C = np.matrix([-2, 1]) Kopt = None def observation(x): y = C * x ry = float(y[0]) return (ry) def process(x, u): x = A * x + B * u return (x) def solve_DARE(A, B, Q, R): """ solve a discrete time_Algebraic Riccati equation (DARE) """ X = Q maxiter = 150 eps = 0.01 for i in range(maxiter): Xn = A.T * X * A - A.T * X * B * la.inv(R + B.T * X * B) * B.T * X * A + Q if (abs(Xn - X)).max() < eps: X = Xn break X = Xn return Xn def dlqr(A, B, Q, R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] # ref Bertsekas, p.151 """ # first, try to solve the ricatti equation X = solve_DARE(A, B, Q, R) # compute the LQR gain K = np.matrix(la.inv(B.T * X * B + R) * (B.T * X * A)) eigVals, eigVecs = la.eig(A - B * K) return K, X, eigVals def lqr_control(x): global Kopt if Kopt is None: Kopt, X, ev = dlqr(A, B, C.T * np.eye(1) * C, np.eye(1)) u = -Kopt * x return u def main(): time = 0.0 u_history = [] y_history = [] time_history = [] x = np.matrix([3, 1]).T u = np.matrix([0, 0, 0]) while time <= simTime: u = lqr_control(x) u0 = float(u[0, 0]) x = process(x, u0) y = observation(x) u_history.append(u0) y_history.append(y) time_history.append(time) time += dt plt.plot(time_history, u_history, "-r", label="input") plt.plot(time_history, y_history, "-b", label="output") plt.grid(True) plt.xlim([0, simTime]) plt.legend() plt.show() if __name__ == '__main__': main()
コードの冒頭で線形モデルのパラメータを設定し、
dlqrという関数で最適制御のためのゲインを計算しています。
dlqrは下記のMATLABの関数と入出力が合うように、
実装されています。
下記が上記のコードのシミュレーション結果です。
出力値が0に収束していることがわかります。
入力も安定していることがわかります。
Githubリポジトリ
上記のコードは下記で公開しています。
LQRのパラメータのチューニング方法
下記の資料で、
LQRのパラメータQとRのチューニングの方法が提案されています。
チューニング方法1
Q = I, R = ρIとして、 (Iは単位行列)
ρをチューニングして良い性能のパラメータを見つける。
チューニング方法2
QとRの対角行列の値を、その変数の許容値で設定する。
例えば、x1が距離で0.01m制御誤差が発生してもいい場合、
対応するQの対角成分q1は、その二乗値とする。
例: q1 = (0.01)2
またRに定数ρをつけて、入力と状態のバランスを調整する。
チューニング方法3
値を小さくしたい状態を集めたベクトルをzとし、
z = Hxを決定して、Q = H'H, R = ρIとする。
ρは状態と入力のバランスパラメータである。
参考資料
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。