最適フィードバック制御モデル#

最適フィードバック制御モデルの構造#

最適フィードバック制御モデル (optimal feedback control; OFC) の特徴として目標軌道を必要としないことが挙げられる.Kalman フィルタによる状態推定と線形2次レギュレーター(LQR: linear-quadratic regurator) により推定された状態に基づいて運動指令を生成という2つの流れが基本となる.

系の状態変化#

Dynamicsxt+1=Axt+But+ξt+i=1cεtiCiutFeedbackyt=Hxt+ωt+i=1dϵtiDixtCost per stepxtQtxt+utRut

LQG#

加法ノイズしかない場合(C=D=0),制御問題は線形2次ガウシアン(LQG: linear-quadratic-Gaussian)制御と呼ばれる.

運動制御 (Linear-Quadratic Regulator)#

ut=Ltx^tLt=(R+BSt+1B)1BSt+1ASt=Qt+ASt+1(ABLt)st=tr(St+1Ωξ)+st+1;sT=0

ST=Q

状態推定 (Kalman Filter)#

x^t+1=Ax^t+But+Kt(ytHx^t)+ηtKt=AΣtH(HΣtH+Ωω)1Σt+1=Ωξ+(AKtH)ΣtA

この場合に限り,運動制御と状態推定を独立させることができる.

一般化LQG#

状態および制御依存ノイズがある場合,

実装#

ライブラリの読み込みと関数の定義.

using Base: @kwdef
using Parameters: @unpack
using LinearAlgebra, Kronecker, Random, BlockDiagonals, PyPlot
rc("axes.spines", top=false, right=false)
rc("font", family="Arial") 
@kwdef struct Reaching1DModelParameter
    n = 4 # number of dims
    p = 3 # 
    i = 0.25 # kgm^2, 
    b = 0.2 # kgm^2/s
    ta = 0.03 # s
    te = 0.04 # s
    L0 = 0.35 # m

    bu = 1 / (ta * te * i)
    α1 = bu * b
    α2 = 1/(ta * te) + (1/ta + 1/te) * b/i
    α3 = b/i + 1/ta + 1/te

    A = [zeros(p) I(p); -[0, α1, α2, α3]']
    B = [zeros(p); bu]
    C = [I(p) zeros(p)]
    D = Diagonal([1e-3, 1e-2, 5e-2])

    Y = 0.02 * B
    G = 0.03 * I(n)
end

@kwdef struct Reaching1DModelCostParameter
    n = 4
    dt = 1e-2 # sec
    T = 0.5 # sec
    nt = round(Int, T/dt) # num time steps
    Q = [zeros(nt-1, n, n); reshape(Diagonal([1.0, 0.1, 1e-3, 1e-4]), (1, n, n))]
    R = 1e-4 / nt
    
    init_pos = -0.5
    x₁ = [init_pos; zeros(n-1)]#zeros(n)
    Σ₁ = zeros(n, n)
end
Reaching1DModelCostParameter

Qの値は各時刻において一般座標 (位置,速度,加速度,躍度)のそれぞれを0にするコストに対する重みづけである.例えば,速度も0にすることを重視すれば2番目の係数を上げる.

SΣは各時点での値を一時的にしか必要としないので更新する.

function LQG(param::Reaching1DModelParameter, cost_param::Reaching1DModelCostParameter)
    @unpack n, p, A, B, C, D, G = param
    @unpack Q, R, x₁, Σ₁, dt, nt = cost_param

    A = I + A * dt
    B = B * dt
    C = C * dt
    D = sqrt(dt) * D
    G = sqrt(dt) * G
    
    L = zeros(nt-1, n) # Feedback gains
    K = zeros(nt-1, n, p) # Kalman gains
    S = copy(Q[end, :, :]) # S_T = Q
    Σ = copy(Σ₁);

    for t in 1:nt-1
        K[t, :, :] = A * Σ * C' / (C * Σ * C' + D) # update K
        Σ = G + (A - K[t, :, :] * C) * Σ * A'      # update Σ
    end 

    cost = 0
    for t in nt-1:-1:1
        cost += tr(S * G)
        L[t, :] = (R + B' * S * B) \ B' * S * A # update L
        S = Q[t, :, :] + A' * S * (A - B * L[t, :]')     # update S
    end
    
    # adjust cost
    cost += x₁' * S * x₁
    return L, K, cost
end
LQG (generic function with 1 method)

シミュレーション#

信号依存ノイズ Yが入っている場合はLQGとは異なってくる.

ut=Ltx^tLt=(BSt+1xB+R+nCn(St+1x+St+1e)Cn)1BSt+1xAStx=Qt+ASt+1x(ABLt);STx=QTSte=ASt+1xBLt+(AKtH)St+1e(AKtH);STe=0st=tr(St+1xΩξ+St+1e(Ωξ+Ωη+KtΩωKt))+st+1;sn=0.
x^t+1=Ax^t+But+Kt(ytHx^t)Kt=AΣteH(HΣteH+Ωω)1Σt+1e=(AKtH)ΣteA+nCnLtΣtx^LtCn;Σ1e=Σ1Σt+1x^=KtHΣteA+(ABLt)Σtx^(ABLt);Σ1x^=x^1x^1
function gLQG(param::Reaching1DModelParameter, cost_param::Reaching1DModelCostParameter, maxiter=200, ϵ=1e-8)
    @unpack n, p, A, B, C, D, Y, G = param
    @unpack Q, R, x₁, Σ₁, dt, nt = cost_param

    A = I + A * dt
    B = B * dt
    C = C * dt
    D = sqrt(dt) * D
    G = sqrt(dt) * G
    Y = sqrt(dt) * Y
    
    L = zeros(nt-1, n) # Feedback gains
    K = zeros(nt-1, n, p) # Kalman gains
    
    cost = zeros(maxiter)
    for i in 1:maxiter
         = copy(Q[end, :, :])
        Sᵉ = zeros(n, n)
        Σˣ̂ = x₁ * x₁' # \Sigma TAB \^x TAB \hat TAB
        Σᵉ = copy(Σ₁)
        
        for t in 1:nt-1
            K[t, :, :] = A * Σᵉ * C' / (C * Σᵉ * C' + D)

            AmBL = A - B * L[t, :]'
            LΣˣ̂L = L[t, :]' * Σˣ̂ * L[t, :]

            Σˣ̂ = K[t, :, :] * C * Σᵉ * A' + AmBL * Σˣ̂ * AmBL'
            Σᵉ = G + (A - K[t, :, :] * C) * Σᵉ * A' + Y * LΣˣ̂L * Y'
        end
        
        for t in nt-1:-1:1
            cost[i] += tr( * G + Sᵉ * (G + K[t, :, :] * D * K[t, :, :]'))
            
            L[t, :] = (R + B' *  * B + Y' * ( + Sᵉ) * Y) \ B' *  * A

            AmKC = A - K[t, :, :] * C
            Sᵉ = A' *  * B * L[t, :]' + AmKC' * Sᵉ * AmKC
             = Q[t, :, :] + A' *  * (A - B * L[t, :]')
        end
        
        # adjust cost
        cost[i] += x₁' *  * x₁ + tr(( + Sᵉ) * Σ₁)
        if i > 1 && abs(cost[i] - cost[i-1]) < ϵ
            cost = cost[1:i]
            break
        end
    end
    return L, K, cost
end
gLQG (generic function with 3 methods)

状態ノイズがある場合に関してはTodorovのMATLABコードを参照.

位置は目標位置を基準とする座標で表現し,位置が0になるように運動を行う.状態の中に標的位置を含めコストパラメータを修正することで初期位置を基準とする座標系での運動を記述できる.モデルに関してはTodorov2005を参照.

function simulation(param::Reaching1DModelParameter, cost_param::Reaching1DModelCostParameter, 
                    L, K; noisy=false)
    @unpack n, p, A, B, C, D, Y, G = param
    @unpack Q, R, x₁, dt, nt = cost_param
    
    X = zeros(n, nt)
    u = zeros(nt)
    X[:, 1] = x₁ # m; initial position (target position is zero)

    if noisy
        sqrtdt = dt
         = zeros(n, nt)
        [1, 1] = X[1, 1]
        for t in 1:nt-1
            u[t] = -L[t, :]' * [:, t]
            X[:, t+1] = X[:,t] + (A * X[:,t] + B * u[t]) * dt + sqrtdt * (Y * u[t] * randn() + G * randn(n))
            dy = C * X[:,t] * dt + D * sqrtdt * randn(n-1)
            [:, t+1] = [:,t] + (A * [:,t] + B * u[t]) * dt + K[t, :, :] * (dy - C * [:,t] * dt)
        end
    else
        for t in 1:nt-1
            u[t] = -L[t, :]' * X[:, t]
            X[:, t+1] = X[:, t] + (A * X[:, t] + B * u[t]) * dt
        end
    end
    return X, u
end
simulation (generic function with 1 method)
function simulation_all(param, cost_param, L, K)
    Xa, ua = simulation(param, cost_param, L, K, noisy=false);
    
    # noisy
    nsim = 10
    XSimAll = []
    uSimAll = []
    for i in 1:nsim
        XSim, u = simulation(param, cost_param, L, K, noisy=true);
        push!(XSimAll, XSim)
        push!(uSimAll, u)
    end
    
    # visualization
    @unpack dt, T = cost_param
    tarray = collect(dt:dt:T)
    label = [L"Position ($m$)", L"Velocity ($m/s$)", L"Acceleration ($m/s^2$)", L"Jerk ($m/s^3$)"]

    fig, ax = subplots(1, 3, figsize=(10, 3))
    for i in 1:2
        for j in 1:nsim
            ax[i].plot(tarray, XSimAll[j][i,:]', "tab:gray", alpha=0.5)
        end

        ax[i].plot(tarray, Xa[i,:], "tab:red")
        ax[i].set_ylabel(label[i]); ax[i].set_xlabel(L"Time ($s$)"); 
        ax[i].set_xlim(0, T); ax[i].grid()
    end

    for j in 1:nsim
        ax[3].plot(tarray, uSimAll[j], "tab:gray", alpha=0.5)
    end
    ax[3].plot(tarray, ua, "tab:red")
    ax[3].set_ylabel(L"Control signal ($N\cdot m$)"); ax[3].set_xlabel(L"Time ($s$)"); 
    ax[3].set_xlim(0, T); ax[3].grid()

    tight_layout()
end
simulation_all (generic function with 1 method)
param = Reaching1DModelParameter()
cost_param = Reaching1DModelCostParameter();
L, K, cost = LQG(param, cost_param);
simulation_all(param, cost_param, L, K)
../_images/optimal-feedback-control_19_0.png
L, K, cost = gLQG(param, cost_param);
simulation_all(param, cost_param, L, K)
../_images/optimal-feedback-control_20_0.png

参考文献#