読者です 読者をやめる 読者になる 読者になる

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の値から、下記の式でゲインを計算できます。

f:id:meison_amsl:20161105032948p:plain

 

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

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

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

 

一つ重要な点として、

線形方程式のA,B,Cの要素やコスト行列が変化しない場合、

LQRのゲインは変わりません。

従って、下記のシミュレーションでも実施している通り、

リッカチの方程式を解いてゲインを計算するのは、

モデルが変わらない場合、初めの一回だけでOKです。

Kと現在の状態x_tを掛け合わせるだけで、

その時々の最適入力を計算することができます。

 

PythonによるLQRの制御シミュレーション

下記が簡単なLQRのPython制御シミュレーションのコードです。

#! /usr/bin/python 
# -*- coding: utf-8 -*- 
u""" 
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 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 = np.matrix(la.solve_discrete_are(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 LQRController(x,u):
    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=LQRController(x,u)
        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