MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

線形二次レギュレータ(Linear-Quadratic Regulator:LQR)の概要とPythonサンプルコード

Design and Implementation: Linear Quadratic Regulator

Design and Implementation: Linear Quadratic Regulator

目次

はじめに

最近、ロボットの制御や経路生成の勉強をしているのですが、

しばしば出てくる技術として、

線形二次レギュレータ(Linear-Quadratic Regulator:LQR)があります。

今回はこのLQRの概要とLQRによる

簡単なPython制御シミュレーションコードを紹介したいと思います。

LQRの概要

LQRは最適制御と呼ばれれる制御手法の一つです。

下記のような線形システムに対して、

f:id:meison_amsl:20161105031626p:plain

下記のようなコスト関数を最小するための最適入力u*を計算することができます。

f:id:meison_amsl:20161105031924p:plain

最適入力u*は状態ベクトルxに対するゲインKを使って、

下記の式で計算できます。

f:id:meison_amsl:20161105032041p:plain

 

このゲインKは下記の式で計算できます

まず初めにリッカチ方程式を解きます。

上記のコスト関数におけるリッカチの微分方程式の解き方としては、

Xの初期値をQとして、

下記の式でXを複数回更新し、

Xが変化しなくなるまで計算した値を利用します。

 

解のXの値から、下記の式でゲインを計算できます。

f:id:meison_amsl:20161105032948p:plain

 

つまり、自分の解きたい問題に対して、

上手く上記の式にモデルを設定することで、

解析的に最適入力を計算することができるのです。

 

一つ重要な点として、

線形方程式の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に収束していることがわかります。

入力も安定していることがわかります。

f:id:meison_amsl:20161104151628p:plain

 

Githubリポジトリ

上記のコードは下記で公開しています。

github.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Design and Implementation: Linear Quadratic Regulator

Design and Implementation: Linear Quadratic Regulator