線形モデル予測制御(Liner MPC)のシミュレーション

モデル予測制御とは

非線形モデル予測制御におけるCGMRES法をpythonで実装する
にモデル予測制御が詳しく書いてあります。
私は、上述のサイトと「モデル予測制御～制約下での最適制御」amazonを読んで書いております。

線形レギュレータ系（サーボ系）と線形モデル予測制御のちがい

線形システムにはリカッチ方程式で解いた、フィードバックゲインでの制御で十分では？という声が聞こえそうですが、MPCの利点は入力飽和を扱えることにあります。飽和したからと言って制御ができなくなるわけではないですが、飽和してしまうと「もっと入力がないと不安定化してしまうよぉ～」という状態で対応できなくなってしまいます。MPCでは、あと少しで入力が飽和してしまう際に、早い段階で入力を飽和させておくことで、「もっと入力がないと不安定化してしまうよぉ～」という状態を回避できます。このような動作は非線形制御であり、線形システムに対しての制御ですが、コントローラは非線形制御器となります。

対象システム

状態方程式（離散済み）

x(k+1) = Ad*x(k) +Bd*u(k)

自由応答

MPCでは入力を前ステップ（離散化時間前）の入力からの変動入力を入力項になるよう式を変形します。つまり、

u(k) = u(k-1) + \Delta u(k)

$\Delta u(k)$が変動入力で、対象システムに代入すると

x(k+1) = Ad*x(k) +Bd*u(k-1)+Bd* \Delta u(k)

になります。もう一ステップ進めると

\begin{align}
x(k+2) &= Ad*(Ad*x(k) +Bd*u(k-1)+Bd* \Delta u(k)) + Bd*u(k+1)\\
&= Ad^2*x(k) +Ad*Bd*u(k-1) + Ad*Bd* \Delta u(k) + Bd*(u(k)+\Delta u(k+1) )\\
&= Ad^2*x(k) +Ad*Bd*u(k-1) + Ad*Bd* \Delta u(k) + Bd*(u(k-1)+\Delta u(k)+\Delta u(k+1) )\\
&= Ad^2*x(k) +(Ad*Bd+Bd) *u(k-1) + (Ad*Bd+Bd)* \Delta u(k) + Bd*\Delta u(k+1) \\
\end{align}

となります。以上を繰り返すと、Nステップ後の状態を、初期状態$x(k)$と、初期入力$u(k-1)$、変動入力$\Delta u(k)$ ~ $\Delta u(k+N-1)$で表すことができます。

 \left[
    \begin{array}{ccc}
      x(k+1)\\
x(k+2)\\
\vdots\\
x(k+N)\\
    \end{array}
  \right]
=
 \left[
    \begin{array}{ccc}
      Ad\\
Ad^2\\
\vdots\\
Ad^N\\
    \end{array}
  \right]
x(k)
+
 \left[
    \begin{array}{ccc}
   Bd\\
Ad\cdot Bd + Bd\\
Ad^2 \cdot Bd +Ad\cdot Bd + Bd\\
\vdots\\
\sum^{N-1}_{i=0} Ad^i \cdot Bd
    \end{array}
  \right]
u(k-1)\\
+
 \left[
    \begin{array}{ccc}
   Bd & 0 &\cdots& 0\\
Ad\cdot Bd + Bd&Bd&\cdots&0\\
Ad^2 \cdot Bd +Ad\cdot Bd + Bd &Ad\cdot Bd + Bd&\cdots&0\\
\vdots&\vdots&\vdots&\vdots\\
\sum^{N-1}_{i=0} Ad^i \cdot Bd &\cdots&\cdots&0
    \end{array}
  \right]
\cdot
\left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
    \end{array}
  \right]

この式を

 \left[
    \begin{array}{ccc}
      x(k+1)\\
x(k+2)\\
\vdots\\
x(k+N)\\
    \end{array}
  \right]
=
 \Phi
x(k)
+
 \Upsilon
u(k-1)
+
\Theta
\Delta U\\

\Delta U = \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
    \end{array}
  \right]

とします。

そして、初期状態$x(k)$は計測されたもの、初期入力$u(k-1)$は前のステップで入力したものなので、今のステップ（$u(k)$を決定するステップ）では動きません。ここで、変動入力$\Delta u(k)$ ~ $\Delta u(k+N-1)$がすべて$0$だった時の応答を自由応答といいます。

評価関数

図のように設定値軌道$s(k)$と参照軌道$r(k)$を設定します。

• 設定値軌道は最終的にここに到達してほしい軌道です
• 参照軌道は設定値軌道に到達する過程を設定しています

通常のサーボ系・レギュレータ系では基本的に設定値軌道しかありません。MPCでも$r(k)=s(k)$としても問題ありません。
なぜ参照軌道$r(k)$があるかというと、MPCでの評価関数$V$は

V = \Sigma^{N}_{j=1} |r(k+j)-x(k+j)|^2_{Q(j)} + \Sigma^{N-1}_{j=0} |\Delta u(k+j)|^2_{R(j)}

としており、
重み$Q$は追従誤差重み
重み$R$は変化抑制因子
であり、入力の大きさに対する重みがありません（つけることは多分可能）。よって、早く追従させたいなどの指定がしにくいのと、MPCはプロセス系の目標値追従制御のために最初作られため、過度応答について指定できたほうが都合がよかったのではないかと思います。最適レギュレータの$Q,R$指定は実際に制御するとどうなるかよくわからないという欠点があります。
Rは入力の微分値に重みをかけているようにも見えます。QとRの指定をどのように指定するのがよいかは、ちょっと何とも言えませんが実例で最後にいろいろ試します。

参照軌道の設定について

現在時刻$k$での誤差$\varepsilon (k)$を計算します

\varepsilon (k) = s(k) - x(k)

参照軌道はいろいろな指定方法があると思いますが、指数関数的になると予想して時定数で指定すると次のようになります

r(k+i) = s(k+i) - exp( -i \frac{T_{d}}{T_{ref}})\varepsilon(k)

誤差が指数関数的に減少し、$T_{ref} = T_{d}$の時、1ステップ後には誤差は$exp(-1) $倍、つまり約三分の一になるように$x(k+i)$を持っていきたいという指定です。

ただし、状態変数の変化は指数関数的に減少していくとは限らないので変な応答になる可能性があります。

QP問題

MPCでは評価関数・拘束条件をまとめてQP問題に変換し最適入力を決定します。
QP問題とは

\begin{align}
    \min_{x\in\mathbb{R}^{n}}
    & & &
    \frac{1}{2} x^TPx+q^Tx    \\\
    \mathrm{subject\ to}
    & & &
    Ax=b \\\
    & & &
    Gx\leq{h}
\end{align}

と記述される二次計画問題です。
Pythonの数理最適化用ライブラリCVXOPTの使用例を参考に解きました。

評価関数をQP問題へ

評価関数をQP問題に変換することはそこまで難しくありません。

\begin{align}
V &= \Sigma^{N}_{j=1} |r(k+j)-x(k+j)|^2_{Q(j)} + \Sigma^{N-1}_{j=0} |\Delta u(k+j)|^2_{R(j)}\\
&= (T-X)^T Q (T-X) + \Delta U ^T R \Delta U
\end{align}

T=  \left[
    \begin{array}{ccc}
      r(k+1)\\
r(k+2)\\
\vdots\\
r(k+N)\\
    \end{array}
  \right] \\

X=\left[
    \begin{array}{ccc}
      x(k+1)\\
x(k+2)\\
\vdots\\
x(k+N)\\
    \end{array}
  \right]   \\

の$x$に

 \left[
    \begin{array}{ccc}
      x(k+1)\\
x(k+2)\\
\vdots\\
x(k+N)\\
    \end{array}
  \right]
=
 \Phi
x(k)
+
 \Upsilon
u(k-1)
+
\Theta
\Delta U\\

\Delta U = \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
    \end{array}
  \right]

を代入し

\begin{align}
V&= (T-(\Phi
x(k)
+
 \Upsilon
u(k-1)
+
\Theta
\Delta U))^T Q (T-(\Phi
x(k)
+
 \Upsilon
u(k-1)
+
\Theta
\Delta U)) + \Delta U ^T R \Delta U\\

&= \Delta U ^T (R + \Phi^T Q \Phi) \Delta U -2 \left[ T-\Phi x(k) -\Upsilon u(k-1) \right]^T Q \Theta \Delta U + const.
\end{align}

$const.$は$x(k),u(k-1)$の部分は固定なので定数とみなせるため、省略しました。
上の$V$の式は、$ \Delta U $を変数として、次の二次計画問題の式

\begin{align}
    \min_{x\in\mathbb{R}^{n}}
    & & &
    \frac{1}{2} x^TPx+q^Tx    \\\
    \mathrm{subject\ to}
    & & &
    Ax=b \\\
    & & &
    Gx\leq{h}
\end{align}

において、

P = R + \Phi^T Q \Phi \\
q =-2 Q \left[ T-\Phi x(k) -\Upsilon u(k-1) \right]  \\

に相当します

拘束条件

次に拘束条件を作成します。

u_{min} \leq u(k+i) \leq u_{max}　(i=0,\cdots ,N-1)

という拘束条件を満たしたい場合、これを$ \Delta U $を変数とした不等式に変換します。評価関数の変数が$ \Delta U $のため必要になります。

\begin{align}
u(k) &= u(k-1) + \Delta u(k) \\
u(k+1) &= u(k-1) + \Delta u(k) +\Delta u(k+1) \\
\vdots\\
u(k+N-1) &= u(k-1) + \Sigma^{N-1}_{i=0}\Delta u(k+i)\\
\end{align}

という関係があります。これを行列表現すると

 \left[
    \begin{array}{ccc}
       u(k)\\
  u(k+1)\\
\vdots\\
  u(k+N-1)\\
1\\
    \end{array}
  \right]
 =
\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

 \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
u(k-1)\\
1\\
    \end{array}
  \right]

と書けます。これを

G=\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

とおきます。

次に、

u_{min} \leq u(k+i) \leq u_{max}　(i=0,\cdots ,N-1)

を

AU \leq 0\\
U=\left[
    \begin{array}{ccc}
       u(k)\\
  u(k+1)\\
\vdots\\
  u(k+N-1)\\
1\\
    \end{array}
  \right]

の形に変えます。不等式は

u(k+i)-u_{max} \leq 0\\
-u(k+i) +u_{min} \leq 0

といえるので

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&-u_{max} \\
-1&0& 0&0&\cdots &0&u_{min}\\
0&1& 0&0&\cdots &0&-u_{max}\\
0&-1& 0&0&\cdots &0&u_{min}\\
\vdots\\
0&0& 0&0&\cdots &1&-u_{max}\\
0&0& 0&0&\cdots &-1&u_{min}\\
    \end{array}
  \right]


\left[
    \begin{array}{ccc}
       u(k)\\
  u(k+1)\\
\vdots\\
  u(k+N-1)\\
1\\
    \end{array}
  \right]

 \leq 0\\

と書けます。先ほどの

 \left[
    \begin{array}{ccc}
       u(k)\\
  u(k+1)\\
\vdots\\
  u(k+N-1)\\
1\\
    \end{array}
  \right]
 =
\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

 \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
u(k-1)\\
1\\
    \end{array}
  \right]

を代入し

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

 \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
u(k-1)\\
1\\
    \end{array}
  \right]

\leq 0

を得ます。最後に$\Delta U$の不等式に変換し完成です。

\Delta U = \left[
    \begin{array}{ccc}
      \Delta u(k+1)\\
 \Delta u(k+2)\\
\vdots\\
 \Delta u(k+N)\\
    \end{array}
  \right]

なので、上の不等式から$u(k-1),1$に相当する列を右辺に移項し

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0 \\
1&1& 0&0&\cdots &0&\\
1&1& 1&0&\cdots &0\\
\vdots\\
1&1& 1&1&\cdots &1\\
0&0& 0&0&\cdots &0\\
    \end{array}
  \right]

\Delta U

\leq \\

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

\left[
    \begin{array}{ccc}
   1 &0 \\
1 &0\\
1 &0\\
\vdots\\
1 &0\\
0 &1\\
    \end{array}
  \right]
\left[
    \begin{array}{ccc}
   u(k-1)\\
1\\
    \end{array}
  \right]

となり、

A\Delta U \leq b

の形にできました。

評価関数の二次計画問題への変換まとめ

\begin{align}
V &= \Sigma^{N}_{j=1} |r(k+j)-x(k+j)|^2_{Q(j)} + \Sigma^{N-1}_{j=0} |\Delta u(k+j)|^2_{R(j)}\\
\end{align}

という評価関数に対し

\min_{\Delta U} V \\
\mathrm{subject\ to} \\
    A\Delta U \leq{b}

ただし

\begin{cases}
V= \Delta U^T P \Delta U + q^T \Delta U &\\
P = R + \Phi^T Q \Phi &\\
q =-2 Q \left[ T-\Phi x(k) -\Upsilon u(k-1) \right] &\\
A=\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

\left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0 \\
1&1& 0&0&\cdots &0&\\
1&1& 1&0&\cdots &0\\
\vdots\\
1&1& 1&1&\cdots &1\\
0&0& 0&0&\cdots &0\\
    \end{array}
  \right]\\
b= \left[
    \begin{array}{ccc}
   1&0 &0&0 &\cdots&0&1 &0 \\
1&1& 0&0&\cdots &0&1 &0\\
1&1& 1&0&\cdots &0&1 &0\\
\vdots\\
1&1& 1&1&\cdots &1&1 &0\\
0&0& 0&0&\cdots &0&0 &1\\
    \end{array}
  \right]

\left[
    \begin{array}{ccc}
   1 &0 \\
1 &0\\
1 &0\\
\vdots\\
1 &0\\
0 &1\\
    \end{array}
  \right]
\left[
    \begin{array}{ccc}
   u(k-1)\\
1\\
    \end{array}
  \right]
\end{cases}

という計画問題に変換されました。

チューニングパラメータ

ここまでの議論でチューニング可能なパラメータについて

• $Q$
• $R$
• $r(k+i)$
• 予測範囲　$N$　(ここまで、Nステップ後まで入力が常に変わるとしたが、計算負荷を減少させるため $\Delta u(k+i) = 0 $(i=自由) のようにおいてもよい)

となる。

シミュレーション

Python3 + spyder環境で実行しました。cvxoptをインストール必要があります。

\frac{d}{dt} x = 
\left[
    \begin{array}{ccc}
-c&k\\
1& 0\\
    \end{array}
  \right]
x
+
\left[
    \begin{array}{ccc}
  1\\
0\\
    \end{array}
  \right]
u

という連続システムを。$0.01s$で離散化して制御対象としました。
各種定数は次のようになります。

\begin{eqnarray}
-1<u<1\\
c=1\\
k=1\\
Q = eye(2*N)\\
R = eye(2*N)\\
x(0) = [1,0]^T\\
N=10    \\
simulation-step = 1000
\end{eqnarray}

参照軌道$r$は

\begin{eqnarray}
r(k+i) = s(k)- \varepsilon(k)* \lambda^i\\
\lambda = 0.9\\
s(k)=0\\
\end{eqnarray}

としました。

こんな感じでとりあえず、不安定システムを安定化できています。

上が状態変数$x$,下が入力$u$、横軸は時刻ですが、単位は秒でなくステップです。

"""
LMPC シミュレーション
"""
import numpy as np
import scipy
from scipy import signal 

import numpy
import cvxopt
from cvxopt import matrix

import matplotlib.pyplot as plt

c=1
k=1
A=np.array([[-c, k],[1,0]]) #不安定
B=np.array([[1],[0]])
C=np.array([0,1])
D=np.array([1])

dt = 0.01
dsys = scipy.signal.cont2discrete((A,B,C,D),dt,method ='zoh')

Ad = dsys[0]
Bd = dsys[1]
Cd = dsys[2]

print("----dicred-----")
print(Ad)
print(Bd)
print(Cd)

N=10

"""
各種定数

x(k+1)     
x(k+2)  =  Phi * x(k)  + Ganma * u(k-1) + Theta Δu(k ~ k+N-1)
:
x(k+N)

"""
Adi = Ad
Phi = Ad
for i in range(N-1):
    Adi = np.dot(Adi,Ad)
    Phi = np.append(Phi,Adi)
    

Phi = np.reshape(Phi,(2*N,2))
print("----Phi-----")
print(Phi)

Ganma = Bd
Adi = Ad
for i in range(N-1):
    Ganma = np.append(Ganma,np.dot(Adi,Bd))
    Adi = np.dot(Adi,Ad)
    
Ganma = np.reshape(Ganma,(2*N,1))
print("----Ganma-----")
print(Ganma)

zero = np.zeros((2,1))
Theta = Ganma
Ganma2 = Ganma                      #Theta作る用
for i in range(N-1):
    Ganma2 = np.append(zero,Ganma2,0)   #上にゼロをつける
    Ganma2 = Ganma2[0:2*N:]             #下を削除
    Theta = np.append(Theta,Ganma2,1)
    
Ganma = np.reshape(Ganma,(2*N,1))       #不必要かも
print("----Ganma-----")
print(Ganma)

"""
二次計画問題係数
"""
Q = np.zeros((2*N,2*N))
R = np.zeros((N,N))
for i in range(2*N):
    Q[i][i] = 1
for i in range(N):
    R[i][i] = 1
    
H=np.dot(np.dot(Theta.T,Q),Theta) + R
gbt = -2*np.dot(Theta.T,Q)

"""
拘束条件
umin < u < umax とする

F [u   < 0
   1]

FFu*u < FFp
"""
umax = 1
umin = -1
F=np.zeros((2*N,N+1))
for i in range(N):
    F[2*i][i]=1
    F[2*i+1][i]=-1
    F[2*i][N]=-1*umax
    F[2*i+1][N]=umin

FF=np.zeros((2*N,N+1))
for i in range(2*N):       #制約の数
    for j in range(N):
        hoge = 0
        for k in range(j,N):
            hoge = hoge + F[i][k]
            
        FF[i][j] = hoge
        
    FF[i][N] = F[i][N]    #定数部
FFu = FF[:,0:N]
FFp = -1*FF[:,N]
FFp = FFp.reshape(2*N,1)

#シミュレーション開始
xk_plant = np.array([[1],[0]])  #x(k)
uk_1 = 0                        #u(k-1)
set_point =  np.array([[0],[0]]) #最終目標値

save_x = np.array(xk_plant)
save_u = np.array([uk_1])
#save_r

cvxopt.solvers.options['show_progress'] = False  #計算状況　非表示
Time = 1000                    #Time＊dt  [s]
for i in range(Time):
    
    """
    目標値ベクトル  T
    """
    lamda  = 0.9
    epsilonk_1 = lamda * (set_point - xk_plant)
    T=np.zeros((2*N,1))
    for i in range(N):
        T[2*i,0]= (set_point - epsilonk_1)[0,0]
        T[2*i+1,0]= (set_point - epsilonk_1)[1,0]
        epsilonk_1 = lamda * epsilonk_1
             
    
    """
    誤差ベクトル
    """
    Epsilon = T - np.dot(Phi,xk_plant) - np.dot(Ganma,uk_1)
    
    """
    ソルバーにより解く
    入力決定
    """
    g=np.dot(gbt,Epsilon)
    gm = matrix(g)
    Hm = matrix(H)
    FFum = matrix(FFu)
    #FFpm = matrix(FFp)
    FFpm = matrix(FFp - uk_1*FF[:,0].reshape(2*N,1) )  #こっちが正しい
    
    sol=cvxopt.solvers.qp(Hm,gm,FFum,FFpm)
    
    uk = sol["x"][0]+uk_1
    
    """
    モデルに入力を加える
    """
    
    xk_1_plant = np.dot(Ad,xk_plant) + np.dot(Bd,uk)
    
    """
    保存
    """
    save_x = np.append(save_x,xk_1_plant,1)
    save_u = np.append(save_u,np.array([uk]),0)
    
    """
    更新
    """
    xk_plant = xk_1_plant
    uk_1 = uk





#グラフ化
fig = plt.figure(figsize=(12, 8)) #...1

# Figure内にAxesを追加()
ax = fig.add_subplot(211) #...2
ax2 = fig.add_subplot(212) #...2
t=np.arange(Time+1)
ax.plot(t,save_x[0,:])
ax.plot(t,save_x[1,:])
ax2.plot(save_u)
plt.show()

各種定数変更

次に定数を変更した場合の応答を見てみます

Nを変更

N=10    \\

N=20\\

N=100\\

N=500\\

さすがに計算がかなり重くなった。

わかること

• Nを増やすほど最初の入力の上がり方が急増
• Nを増やすほど入力の減少が速い
• Nを増やすほど入力の振動が高周波になっていく
• Nを増やすほど入力の振動の振幅が大きくなる
• Nを増やすほど状態変数の減少が速い
• Nが小さいと入力の飽和が途切れるタイミングが早い

参照軌道の設定

参照軌道の設定を、追従軌道と一致させる。つまり、

s(k+i) = r(k+i)

とする。

N=10    \\

N=20\\

N=100\\

N=500\\

ほぼ差がありません

###λの数値

N=100\\,
\lambda=0.3\\

\lambda=0.9\\

\lambda=0.99\\

0.9と0.3でほとんど差がない
0.99は振動的であまりよくない

追記　
Q,Rの指定がよくなく、参照軌道に追従できていないため、この比較はあまり意味がない
Q=5000,R=1で再度シミュレーションした

1:λ=0.3

2:λ=0.7

3:λ=0.9

4:λ=0.99

0.3->0.9まで収束速度が速くなっていき、0.9あたりを超えると収束速度がどんどん遅くなっていった。

本来は大きくなるほど収束速度が速くなっていくべきなので、予想と反している。理由は不明。
状態変数の変動は単純な指数関数減少では表せないので（例、オレンジの線が一度増加するような応答を示すこと）λの大きさで予想通りの指定にならないのではないかと思う。

R,Qの数値

\begin{eqnarray}
Q = eye(2*N)\\
R = eye(2*N)\\
N=100    \\
simulation-step = 1000\\
\lambda = 0.9
\end{eqnarray}

1:Q=1,R=1

2:Q=5,R=1

3:Q=50,R=1

4:Q=100,R=1

5:Q=500,R=1

6:Q=5e4,R=1

7:Q=5e5,R=1

• どんどん、状態量振動がなくなり、収束が早くなる
• Qをかなり大きくすると、今度は入力が振動的になる
• 入力の-1から＋側への反転時のピークの大きさがQが大きいほど大きくなっていく

LQRによる制御と比較

ここで、入力飽和を考えず離散リカッチ方程式を解いて求めた、フィードバックゲインでの制御結果は次のようになります。

Q=eye(1)\\
R=1

Q=eye(10)\\,
R=1

Q=eye(100)\\,
R=1

Q=eye(1e4)\\,
R=1

Q=eye(10^{-6}),
R=1

Qを上げるほど収束速度が速くなっているのがわかると思います.$Q=1e4$以上では、ほとんど変化がなくなりました。
これまでのMPCでの制御結果で同じようなグラフがあったので、MPCは大きく制御則を変えるものでもないかもしれません。

MPCの制御結果で、Q=5e4,R=1とした次の結果がありますが

200stepあたりで入力を正側に持っていって、収束を思いっきり速めています。LQRの評価関数では入力の大きさに重みがかかりますが、MPCにはかからないためこういった入力の仕方が生まれるのかもしれません。

追記
Qの重みの取り方を変えれば同じような応答になるものが見つかりました。

Q=\left[
    \begin{array}{ccc}
1&0\\
0&1000\\
    \end{array}
  \right],
R=1

とすると

LQRは、QとRの大きさの比ばかり意識してテストしてましたが、Qの対角成分のそれぞれの大きさにも注意すると、より収束速度を良好にできるのだと理解しました。

以下に、LQRのシミュレーションコードを載せます。

LQR.py

import numpy as np
import scipy
from scipy import signal 

import numpy
import cvxopt
from cvxopt import matrix

import matplotlib.pyplot as plt

c=1
k=1
A=np.array([[-c, k],[1,0]]) #不安定
B=np.array([[1],[0]])
C=np.array([0,1])
D=np.array([1])

dt = 0.01
dsys = scipy.signal.cont2discrete((A,B,C,D),dt,method ='zoh')

Ad = dsys[0]
Bd = dsys[1]
Cd = dsys[2]

print("----dicred-----")
print(Ad)
print(Bd)
print(Cd)


umax = 1
umin = -1

"""
レギュレータ　リカッチを解く
"""
q=1
Qq = np.array([[q,0],[0 ,q]])
Rr = np.array([1])

X = scipy.linalg.solve_discrete_are(Ad,Bd,Qq,Rr)
print(X)
hoge = np.dot(np.dot(Ad.T,X),Ad) + Qq 
hoge2 = np.dot(np.dot(Ad.T,X),Bd)
hoge3 = np.linalg.inv(Rr+np.dot(np.dot(Bd.T,X),Bd))
hoge = hoge +  np.dot(np.dot ( hoge2 ,hoge3 ) ,hoge2.T) 
print(hoge)

K=-1*np.dot(hoge3,hoge2.T)
print("---K----")
print(K)

#シミュレーション開始
xk_plant = np.array([[1],[0]])  #x(k)
uk_1 = 0                        #u(k-1)
set_point =  np.array([[0],[0]]) #最終目標値

save_x = np.array(xk_plant)
save_u = np.array([uk_1])
#save_r


Time = 1000                    #Time＊dt  [s]
for i in range(Time):
    
    uk = np.dot(K,xk_plant)[0,0]
    
    if uk>umax:
        uk = umax
    elif uk<umin:
        uk = umin

        
    
    xk_1_plant = np.dot(Ad,xk_plant) + np.dot(Bd,uk)
    
    """
    保存
    """
    save_x = np.append(save_x,xk_1_plant,1)
    save_u = np.append(save_u,np.array([uk]),0)
    
    """
    更新
    """
    xk_plant = xk_1_plant
    uk_1 = uk





#グラフ化
fig = plt.figure(figsize=(12, 8)) #...1

# Figure内にAxesを追加()
ax = fig.add_subplot(211) #...2
ax2 = fig.add_subplot(212) #...2
t=np.arange(Time+1)
ax.plot(t,save_x[0,:])
ax.plot(t,save_x[1,:])
ax2.plot(save_u)
plt.show()