躍度最小モデル#

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

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

n個の変数があり,m個の制約条件がある等式制約二次計画問題を考える.xRn, 対称行列PRn×n, qRn, ARm×n, bRm.このとき,問題は次のようになる.

Minimize12xPx+qxsubject toAx=b

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

[PAA0][xλ]=[qb]

の解として与えられる.ここで λRm は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を用いた実装を参考にして作成した.

問題設定は以下のようにする. $minimizeu(t)u(t)2subject tox˙(t)=Ax(t)+Bu(t)$

ただし,2L2ノルムを意味し,A=[010001000],B=[001],x(t)=[x(t)x˙(t)x¨(t)],u(t)=x(t)とする.すなわち,制御信号u(t)は躍度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()
../_images/minimum-jerk_16_0.png

経由点を通る場合#

経由点問題(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()
../_images/minimum-jerk_20_0.png

参考文献#

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.