躍度最小モデル

躍度最小モデル (minimum-jerk model; [Flash and Hogan, 1985])を実装する．解析的に求まるが以下では二次計画法を用いて数値的に求める．

等式制約下の二次計画法 (Equality Constrained Quadratic Programming)

$n$個の変数があり，$m$個の制約条件がある等式制約二次計画問題を考える．$\mathbf{x}\in {\mathbb{R}}^n$, 対称行列$\mathbf{P}\in {\mathbb{R}}^{n\times n}$, $\mathbf{q}\in {\mathbb{R}}^n$, $\mathbf{A}\in {\mathbb{R}}^{m\times n}$, $\mathbf{b}\in {\mathbb{R}}^m$．このとき，問題は次のようになる．

$$\begin{array}{r}\begin{array}{rl}& \text{Minimize}\frac{1}{2}{\mathbf{x}}^{\mathrm{\top }}\mathbf{P}\mathbf{x}+{\mathbf{q}}^{\mathrm{\top }}\mathbf{x}\\ & \text{subject to}\mathbf{A}\mathbf{x}=\mathbf{b}\end{array}\end{array}$$

Lagrangeの未定乗数法を用いると解は

$$\begin{array}{r}\left[\begin{array}{cc}\mathbf{P}& {\mathbf{A}}^{\mathrm{\top }}\\ \mathbf{A}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{x}\\ \lambda \end{array}\right]=\left[\begin{array}{c}-\mathbf{q}\\ \mathbf{b}\end{array}\right]\end{array}$$

の解として与えられる．ここで $\lambda \in {\mathbb{R}}^m$ はLagrange乗数のベクトルである．

using LinearAlgebra, Random, ToeplitzMatrices, PyPlot
rc("axes.spines", top=false, right=false)

# Equality Constrained Quadratic Programming
function solveEqualityConstrainedQuadProg(P, q, A, b)
    """
    minimize   : 1/2 * x'*P*x + q'*x
    subject to : A*x = b
    """
    K = [P A'; A zeros(size(A)[1], size(A)[1])] # KKT matrix
    sol = K \ [-q; b] 
    return sol[1:size(A)[2]]
end

solveEqualityConstrainedQuadProg (generic function with 1 method)

ちなみにjuliaでは $Ax=b$の解を出すとき，x=A^-1 * bよりもx=A \ bとした方がよい．

P = diagm([1.0, 0.0])
q = [3.0, 4.0]
A = [1.0, 1.0]'
b = [1.0]
x = solveEqualityConstrainedQuadProg(P, q, A, b);

躍度最小モデルの実装

1次元における運動を考えよう．この仮定ではサッカードするときの眼球運動などが当てはまる．以下では [Yazdani et al., 2012] での問題設定を用いる．Toeplitz行列を用いた実装はYazdaniらのPythonでcvxoptを用いた実装を参考にして作成した．

問題設定は以下のようにする． $$\begin{array}{rl}& \underset{u(t)}{\mathrm{minimize}}\Vert u(t){\Vert }_2\\ & \text{subject to}\dot{\mathbf{x}}(t)=A\mathbf{x}(t)+Bu(t)\end{array}$$

ただし，$\Vert \cdot {\Vert }_2$は$L_2$ノルムを意味し，$A=\left[\begin{array}{lll}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right],\mathbf{x}(t)=\left[\begin{array}{l}x(t)\\ \dot{x}(t)\\ \ddot{x}(t)\end{array}\right],u(t)=\stackrel{⃛}{x}(t)$とする．すなわち，制御信号$u(t)$は躍度$\stackrel{⃛}{x}(t)$と等しいとする．

T = 1.0 # simulation time (sec)
dt = 1e-2 # time step (sec)
nt = Int(T/dt) # number of samples
trange = range(0, 1, length=nt); # range of time 

row_jerk = [[-1, 3, -3, 1]; zeros(nt-4)]
col_jerk = [-1; zeros(nt-4)];
D_jerk = Toeplitz(col_jerk, row_jerk);

実際にはD_jerkには(1/dt)^3を乗じるべきであるが，二次計画法の数値的な安定性のために結果の描画の際にのみ乗じる．

init_pos = [1; zeros(nt-1)]'
final_pos = [zeros(nt-1); 1]'
init_vel = [[-1, 1]; zeros(nt-2)]'
final_vel = [zeros(nt-2); [-1, 1]]'
init_accel = [[1, -2, 1]; zeros(nt-3)]'
final_accel = [zeros(nt-3); [1, -2, 1]]';

Aeq = [init_pos; final_pos; init_vel; final_vel; init_accel; final_accel];

beq = zeros(6) # (init or final) or (pos, vel, acc) = 2*3
beq[1] = 0     # initial position (m)
beq[2] = 2;    # final position (m) 

二次計画法を解く．

sol_pos = solveEqualityConstrainedQuadProg(D_jerk' * D_jerk, zeros(nt), Aeq, beq);

位置解を速度，加速度，躍度に変換する．

# set D_vel and D_accel
row_vel = [[-1, 1]; zeros(nt-2)]
col_vel = [-1; zeros(nt-2)]
D_vel = (1/dt) * Toeplitz(col_vel, row_vel);

row_accel = [[1,-2,1]; zeros(nt-3)] 
col_accel = [1; zeros(nt-3)]
D_accel = (1/dt)^2 * Toeplitz(col_accel, row_accel);

# compute solution of vel, accel and jerk
sol_vel = D_vel * sol_pos;
sol_accel = D_accel * sol_pos;
sol_jerk = (1/dt)^3 * D_jerk * sol_pos;

結果を描画する．

figure(figsize=(8, 4))
subplot(2,2,1)
plot(trange, sol_pos)
ylabel(L"Position ($m$)"); grid()

subplot(2,2,2)
plot(trange[1:nt-1], sol_vel)
ylabel(L"Velocity ($m/s$)"); grid()

subplot(2,2,3)
plot(trange[1:nt-2], sol_accel)
ylabel(L"Acceleration ($m/s^2$)"); xlabel("Time (s)"); grid()

subplot(2,2,4)
plot(trange[1:nt-3], sol_jerk)
ylabel(L"Jerk ($m/s^3$)"); xlabel("Time (s)"); grid()

tight_layout()

経由点を通る場合

経由点問題(via-point problem)を考える．

via_point_pos = zeros(nt)'
via_point_pos[Int(nt/2)] = 1; # via point timing

Aeq2 = [init_pos; final_pos; via_point_pos; init_vel; final_vel; init_accel; final_accel];

beq2 = zeros(7) # (init or final) or (pos, vel, acc) + via_point_pos = 2*3 + 1 = 7 
beq2[1] = 2     # inital position (m)
beq2[2] = 4     # final position (m)
beq2[3] = 6;    # via point position (m)

6

sol2_pos = solveEqualityConstrainedQuadProg(D_jerk' * D_jerk, zeros(nt), Aeq2, beq2);
sol2_vel = D_vel * sol2_pos;
sol2_accel = D_accel * sol2_pos;
sol2_jerk = (1/dt)^3 * D_jerk * sol2_pos;

figure(figsize=(8, 4))
subplot(2,2,1)
plot(trange, sol2_pos)
ylabel(L"Position ($m$)"); grid()

subplot(2,2,2)
plot(trange[1:nt-1], sol2_vel)
ylabel(L"Velocity ($m/s$)"); grid()

subplot(2,2,3)
plot(trange[1:nt-2], sol2_accel)
ylabel(L"Acceleration ($m/s^2$)"); xlabel("Time (s)"); grid()

subplot(2,2,4)
plot(trange[1:nt-3], sol2_jerk)
ylabel(L"Jerk ($m/s^3$)"); xlabel("Time (s)"); grid()

tight_layout()

参考文献

FH85

T Flash and N Hogan. The coordination of arm movements: an experimentally confirmed mathematical model. J. Neurosci., 5(7):1688–1703, July 1985.

YGHHN12

Mehrdad Yazdani, Geoffrey Gamble, Gavin Henderson, and Robert Hecht-Nielsen. A simple control policy for achieving minimum jerk trajectories. Neural Netw., 27:74–80, March 2012.