更新
2023/3/17
①Multiple-Shooting C/GMRES法のコードにおいて、一部インデックスに誤りがあったので、修正しました。
②ニュートン・GMRES法のコードを修正しました。コントローラー関数において、誤ってtemp.Uの定義をニュートンステップループの中に入れておりました。間違ったコードだとニュートンステップが一回しか行われないのと同じになってしまっていました。また、同じくコントローラー関数内のArnoldi法において、$F$の計算における引数が間違っておりました。こちらを修正したところC/GMRES法と同様に安定した制御となりました。
挨拶
はじめまして。制御工学が専門（と言っていいかは実は微妙な立ち位置なのですが）の大学生です。自分の研究分野では非線形最適制御が必要となる事が多く、その実装練習として差動二輪車の最適制御をいくつか実装しました。ひとまずC/GMRES法、Multiple-Shootingを用いたC/GMRES法、ニュートン・GMRES法の3つの手法でのコード例を載せます。他にも出来次第追加で載せるかもしれません。間違い等ありましたら、お手数ですがコメント等で教えて頂けると助かります。
環境
MATLAB R2022b
各制御手法について
それぞれの制御手法についてざっくり説明します。厳密には制御手法ではなく最適化アルゴリズムと言った方が良いのかもしれません。これらの手法について、自分は以下の書籍及び論文を参考にしました。
- 非線形最適制御入門
- 実時間最適化による制御の実応用
- Multiple ShootingとContinuation/Krylov法を用いた実時間アルゴリズムによる劣駆動ホバークラフトの非線形Receding Horizon制御
また、実装について、以下の記事を参考にしました。
とても分かりやすい記事です。自分のコードはこれらの記事で公開されているコードが基になっています。
C/GMRES法
各制御ステップで最適入力の時間微分を計算し、そこから入力値を実時間でオイラー積分する事で間接的に最適入力を更新して行く手法です。
Multiple-Shootingを用いたC/GMRES法
最適化手法にMultiple-Shootingというものがあり、それとC/GMRESを組み合わせて定式化したものらしいです。最適入力だけでなく、状態変数や随伴変数も実時間でのオイラー積分で更新する手法です。
ニュートン・GMRES法
ニュートン法とGMRES法を掛け合わせた手法です。通常のニュートン法と違い、最適入力そのものではなく、最適入力の変化量を計算します。
差動二輪車モデルについて
turtlebotを想像して頂ければ大丈夫です。状態変数は位置$(x,y)$と$x$軸から反時計回りに見た傾き$\theta$を合わせた$(x,y,\theta)^{T}$とし、入力は左右の車輪の回転角速度$(\omega_{r},\omega_{l})^{T}$としました。このモデルについては以下の書籍を参考にしました。
コード
拘束条件は考えていません。もし拘束条件を追加したければ評価関数にバリア関数の項を加える事で対応できるかと思います（その場合ローカル関数も修正する必要があります）。その他諸々のパラメーターは自分が適当に決めただけなのであまり深い意図は無いです。
また、Multiple-Shootingを用いたC/GMRES法は向きを変える事を抑制する項（正確にはその重み）を評価関数に追加しています。
C/GMRES法
close all; clear all; format compact; beep off;%#ok<CLALL>
Ts = 0.05; %サンプリング周期[sec]
t_sim_end = 20; %シミュレーション時間
tsim = 0:Ts:t_sim_end; %シミュレーションの基準となる時刻
%コントローラーのハイパーパラメーター
cgmres.ht = Ts; %前進差分近似の時間幅[sec]
cgmres.zeta = 1/Ts; %操作量の安定化ゲインζ
cgmres.tf = 1.0; %予測時間の最終長さ[sec]
cgmres.alpha = 0.5; %予測時間の上昇ゲイン
cgmres.N = 20; %予測区間の分割数
% 評価関数中の重み
cgmres.Q = 100 * [1 0 0; 0 1 0; 0 0 0];
cgmres.R = eye(2);
cgmres.S = 200 * [1 0 0; 0 1 0; 0 0 0];
%車のパラメーター
car.R = 0.05; %車輪半径[m]
car.T= 0.2; %車輪と車輪の間の幅(車体の横幅)[m]
car.r = car.R/2; %よく使う値を先に計算
car.rT = car.R/car.T;
% 初期条件
car.x = [0;0;0];
cgmres.u = [0;0];
car.len_x = length(car.x);
cgmres.len_u = length(cgmres.u);
%car.X = zeros(car.len_x*cgmres.N,1); %コントローラに与える初期状態(今回はプラントの初期状態も同値とした)
cgmres.U = zeros(cgmres.len_u*cgmres.N,1); %コントローラに与える初期操作量
cgmres.dU = 0;
%目標地点
cgmres.x_ob = [2,2,0]';
% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
%シミュレーションループ
for i = 1:length(tsim)
log.u(:,i) = cgmres.u;
log.x(:,i) = car.x;
car.x
%シミュレーション計算（最後だけは計算しない）
if i ~= length(tsim)
[t,xi] = ode45( @(t,xi) two_wheel_car(t,xi,cgmres.u,car.R,car.T),...
[tsim(i) tsim(i+1)], car.x, opts);
else
break
end
Tl = cgmres.tf*(1-exp(-cgmres.alpha*Ts*i));
%C/GMRES法コントローラーを関数として実装
[U, dU] = CGMRES_control(car,cgmres,Tl);
cgmres.u = U(1:2);
cgmres.U = U;
cgmres.dU = dU;
car.x = xi(end,:)';
end
%グラフ化
figure();
subplot(5,1,1);
plot(tsim,log.u(1,:)); ylabel('u1'); xlabel('time[s]');
subplot(5,1,2);
plot(tsim,log.u(2,:)); ylabel('u2'); xlabel('time[s]');
subplot(5,1,3);
plot(tsim,log.x(1,:)); ylabel('x1'); xlabel('time[s]');
subplot(5,1,4);
plot(tsim,log.x(2,:)); ylabel('x2'); xlabel('time[s]');
subplot(5,1,5);
plot(tsim,log.x(3,:)); ylabel('x3'); xlabel('time[s]');
%C/GMRES法コントローラー
function [U, dU] = CGMRES_control(car,cgmres,Tl)
dt = Tl/cgmres.N; %制御ホライズン内での1ステップを計算
car.dx = func(car.x,cgmres.u,car.R,car.T);
%Fの各種を計算
Fux = CalcF(cgmres.U+cgmres.ht*cgmres.dU, car.x+cgmres.ht*car.dx, cgmres, car, dt);
Fx = CalcF(cgmres.U, car.x+cgmres.ht*car.dx, cgmres, car, dt);
F = CalcF(cgmres.U, car.x, cgmres, car, dt);
%クリロフ部分空間の最初の基底を求める
left = (Fux - Fx)/cgmres.ht;
right = -cgmres.zeta*F - ((Fx-F)/cgmres.ht);
r0 = right - left;
%1つ目の基底ベクトル
Vm(:,1) = r0 ./ norm(r0);
% Arnoldi process(Hm_を作成する)
Hm = zeros(3,3);
% GMRESの繰り返し回数(基底ベクトルの数)
m = cgmres.len_u * cgmres.N;
%Arnoldi法
for j=1:m
Fux = CalcF(cgmres.U + ( Vm(:,j) * cgmres.ht ) , ...
car.x + (car.dx * cgmres.ht),cgmres,car,dt);
Av = ( ( Fux - Fx ) / cgmres.ht ); %Fxtは関数Fの状態微分
%Line 3
for k=1:j
Hm(k,j)=Av'*Vm(:,k);
end
%Line 4
temp_vec = 0;
for k = 1:j
temp_vec = temp_vec + Hm(k,j).*Vm(:,k);
end
v_hat = Av - temp_vec;
%Line 5
Hm(j+1,j) = norm(v_hat);
Vm(:,j+1) = v_hat ./ Hm(j+1,j);
end
[UTMat,Omega] = ToUTMat(Hm);
e = zeros( k + 1, 1 );
e(1) = 1;
gm_ = norm(r0)*e;
for k=1:length(Omega)
gm_ = Omega{k}*gm_;
end
min_y = zeros(length(UTMat)-1,1); %解の保存先
for k=length(UTMat)-1 :-1:1
min_y(k) = (gm_(k) - UTMat(k,:) * min_y)/UTMat(k,k);
end
du_new = cgmres.dU + Vm(:,1:m)*min_y;
dU = du_new;
U = cgmres.U + dU .* cgmres.ht;
end
%Fの計算
function F = CalcF(U,x,cgmres,car,dt)
%Uを成形
U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
%xの予測計算＆Bの値の再利用
[X, B_all] = Forward(x, U_temp, dt, car, cgmres);
%随伴変数の計算
Lambda = Backward(X, U_temp, dt, car, cgmres);
%Fのサイズを定める
F = zeros(cgmres.len_u*cgmres.N,1);
%Fの算出
for i=1:cgmres.N
F(cgmres.len_u*(i-1)+1:cgmres.len_u*i) = CalcHu(U_temp(:,i), Lambda(:,i), cgmres.R, B_all(:,cgmres.len_u*(i-1)+1:cgmres.len_u*i));
end
end
%xの予測計算
function [X, B_all] = Forward(x,U,dt,car,cgmres)
X = zeros(car.len_x,cgmres.N+1); %Xのサイズを定義
B_all = zeros(car.len_x,cgmres.len_u*(cgmres.N)); %B_allのサイズを定義
X(:,1) = x;
for i = 2:cgmres.N+1
[dx, B] = funcB(X(:,i-1),U(:,i-1),car.r,car.rT);
X(:,i) = X(:,i-1) + dt*dx;
B_all(:,cgmres.len_u*(i-2)+1:cgmres.len_u*(i-1)) = B;
end
end
%随伴変数の計算
function Lambda = Backward(X, U, dt, car, cgmres)
Lambda = zeros(car.len_x, cgmres.N);
Lambda(:,cgmres.N) = cgmres.S*(X(:,cgmres.N+1)-cgmres.x_ob);
for i = cgmres.N-1:-1:1
Lambda(:,i) = Lambda(:,i+1) + CalcHx(X(:,i+1),cgmres.x_ob,U(:,i+1),Lambda(:,i+1),cgmres.Q,car.R)*dt;
end
end
%dH/du(の転置)
function dHdu = CalcHu(u,lambda,R,B)
dHdu = R*u + B'*lambda;
end
%dH/dx(の転置)
function dHdx = CalcHx(x,x_ob,u,lambda,Q,car_R)
dfdx = Calcfx(x,u,car_R);
dHdx = Q*(x-x_ob) + dfdx'*lambda;
end
%df/dx(状態方程式の状態変数微分)
function dfdx = Calcfx(x,u,car_R)
dfdx = [0 0 -(car_R/2)*sin(x(3))*(u(1)+u(2)); 0 0 (car_R/2)*cos(x(3))*(u(1)+u(2)); 0 0 0];
end
function [UTMat,Omega] = ToUTMat(H)
%UNTITLED HをGivens回転でもって上三角行列に変換する
m = length(H)-1; %Givens回転する回数
for i=1:m
nu = sqrt(H(i,i)^2 + H(i+1,i)^2);
c_i = H(i,i)/nu;
s_i = H(i+1,i)/nu;
Omega{i} = diag(ones(m+1,1));
Omega{i}(i:i+1,i:i+1) = [c_i,s_i
-s_i,c_i];
H = Omega{i} * H;
end
UTMat = H;
end
%差分駆動型二輪車のモデル(ode用)
function dxi = two_wheel_car(t,xi,u,car_R,T)
dxi = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dxi(1) = r*cos_*u(1) + r*cos_*u(2);
dxi(2) = r*sin_*u(1) + r*sin_*u(2);
dxi(3) = rT*u(1) - rT*u(2);
end
%二輪車の状態方程式
function dx = func(xi,u,car_R,T)
dx = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dx(1) = r * cos_ * (u(1) + u(2));
dx(2) = r * sin_ * (u(1) + u(2));
dx(3) = rT * (u(1) - u(2));
end
%二輪車の状態方程式(CalcFで使う用)
function [dx,B] = funcB(xi,u,car_r,car_rT)
cos_ = cos(xi(3));
sin_ = sin(xi(3));
B = [car_r*cos_ car_r*cos_; car_r*sin_ car_r*sin_; car_rT -car_rT];
dx = B*u;
end
Multiple-Shooting C/GMRES法
close all; clear all; format compact; beep off;%#ok<CLALL>
Ts = 0.05; %サンプリング周期[sec]
t_sim_end = 20; %シミュレーション時間
tsim = 0:Ts:t_sim_end; %シミュレーションの基準となる時刻
%コントローラーのハイパーパラメーター
cgmres.ht = Ts; %前進差分近似の時間幅[sec]
cgmres.zeta = 1/Ts; %操作量の安定化ゲインζ
cgmres.tf = 1.0; %予測時間の最終長さ[sec]
cgmres.alpha = 0.5; %予測時間の上昇ゲイン
cgmres.N = 20; %予測区間の分割数
% 評価関数中の重み
cgmres.Q = -10 * [1 0 0; 0 1 0; 0 0 0];
cgmres.R = 1 * eye(2) + 1 * [1 -1; -1 1];
cgmres.S = 150 * [1 0 0; 0 1 0; 0 0 0];
%車のパラメーター
car.R = 0.05; %車輪半径[m]
car.T= 0.2; %車輪と車輪の間の幅[m]
car.r = car.R/2; %よく使う値を先に計算
car.rT = car.R/car.T;
% 初期条件
car.x = [0;0;0];
cgmres.u = [0;0];
%目標地点
cgmres.x_ob = [2,2,0]';
car.len_x = length(car.x);
cgmres.len_u = length(cgmres.u);
%随伴変数の初期条件
lambda_zero = cgmres.S*(car.x - cgmres.x_ob);
Lambda_zero = repmat(lambda_zero,1,cgmres.N);
car_X_zero = zeros(car.len_x,cgmres.N);
X_zero = [car_X_zero;Lambda_zero];
car.dx = zeros(car.len_x,1);
car.X = reshape(X_zero,[2*car.len_x*cgmres.N,1]); %コントローラに与える初期状態
cgmres.U = zeros(cgmres.len_u*cgmres.N,1); %コントローラに与える初期操作量
car.dX = zeros(2*car.len_x*cgmres.N,1);
cgmres.dU = zeros(cgmres.len_u*cgmres.N,1);
% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
%シミュレーションループ
for i = 1:length(tsim)
log.u(:,i) = cgmres.u;
log.U(:,i) = cgmres.U;
log.dU(:,i) = cgmres.dU;
log.x(:,i) = car.x;
log.X(:,i) = car.X;
log.dX(:,i) = car.dX;
car.x
%シミュレーション計算（最後だけは計算しない）
if i ~= length(tsim)
[t,xi] = ode45( @(t,xi) two_wheel_car(t,xi,cgmres.u,car.R,car.T),...
[tsim(i) tsim(i+1)], car.x, opts);
else
break
end
time = Ts*i;
%Multiple-Shooting C/GMRES法コントローラーを関数として実装
[U, X, dU, dX] = MS_CGMRES_control(car,cgmres,time);
cgmres.u = U(1:2);
cgmres.U = U;
cgmres.dU = dU;
car.x = xi(end,:)';
car.dx = func(car.x,cgmres.u,car.R,car.T);
car.X = X;
car.dX = dX;
end
%グラフ化
figure();
subplot(5,1,1);
plot(tsim,log.u(1,:)); ylabel('u1'); xlabel('time[s]');
subplot(5,1,2);
plot(tsim,log.u(2,:)); ylabel('u2'); xlabel('time[s]');
subplot(5,1,3);
plot(tsim,log.x(1,:)); ylabel('x1'); xlabel('time[s]');
subplot(5,1,4);
plot(tsim,log.x(2,:)); ylabel('x2'); xlabel('time[s]');
subplot(5,1,5);
plot(tsim,log.x(3,:)); ylabel('x3'); xlabel('time[s]');
%Multiple-Shooting C/GMRES法コントローラー
function [U, X, dU, dX] = MS_CGMRES_control(car,cgmres,time)
%Tl = cgmres.tf*(1-exp(-cgmres.alpha*time));
%dt = Tl/cgmres.N; %制御ホライズン内での1ステップを計算
car.dx = func(car.x,cgmres.u,car.R,car.T);
%vを計算
G1 = CalcG(cgmres.U, car.X, car.x+car.dx*cgmres.ht, cgmres,...
car, time);
K1 = CalcK(cgmres.U+cgmres.dU*cgmres.ht, G1, car.x+car.dx*cgmres.ht,...
cgmres, car, time);
v = (K1 - car.X)/cgmres.ht;
%v3を計算
v2 = - cgmres.zeta*CalcG(cgmres.U, car.X, car.x, cgmres, car, time)...
- (CalcG(cgmres.U, car.X, car.x+car.dx*cgmres.ht,...
cgmres, car, time) ...
- CalcG(cgmres.U, car.X, car.x, cgmres, car, time))/cgmres.ht;
G2 = CalcG(cgmres.U+cgmres.dU*cgmres.ht, car.X,...
car.x+car.dx+cgmres.ht, cgmres, car, time);
K2 = CalcK(cgmres.U+cgmres.dU*cgmres.ht, G2+v2*cgmres.ht,...
car.x+car.dx*cgmres.ht, cgmres, car, time);
v3 = (K2 - car.X)/cgmres.ht;
%v4を計算
v4 = v + v3;
%Fの計算
FUXxt = CalcF(cgmres.U+cgmres.dU*cgmres.ht, car.X+v4*cgmres.ht,...
car.x+car.dx*cgmres.ht, cgmres, car);
FXx = CalcF(cgmres.U, car.X+car.dX*cgmres.ht,...
car.x+car.dx*cgmres.ht, cgmres, car);
F = CalcF(cgmres.U, car.X, car.x, cgmres, car);
%初期残差の計算
r0 = -(FUXxt - F)/cgmres.ht - cgmres.zeta*F;
%1つ目の基底ベクトル
Vm(:,1) = r0 ./ norm(r0);
% Arnoldi process(Hm_を作成する)
Hm = zeros(3,3);
% GMRESの繰り返し回数(基底ベクトルの数)
m = cgmres.len_u * cgmres.N;
%Arnoldi法
for j=1:m
FUXx = CalcF(cgmres.U + ( Vm(:,j) * cgmres.ht ) , ...
car.X+car.dX*cgmres.ht ,car.x+car.dx*cgmres.ht,...
cgmres, car);
Av = ( ( FUXx - FXx ) / cgmres.ht ); %Fxtは関数Fの状態微分
%Line 3
for k=1:j
Hm(k,j)=Av'*Vm(:,k);
end
%Line 4
temp_vec = 0;
for k = 1:j
temp_vec = temp_vec + Hm(k,j).*Vm(:,k);
end
v_hat = Av - temp_vec;
%Line 5
Hm(j+1,j) = norm(v_hat);
Vm(:,j+1) = v_hat ./ Hm(j+1,j);
end
[UTMat,Omega] = ToUTMat(Hm);
e = zeros( k + 1, 1 );
e(1) = 1;
gm_ = norm(r0)*e;
for k=1:length(Omega)
gm_ = Omega{k}*gm_;
end
min_y = zeros(length(UTMat)-1,1); %解の保存先
for k=length(UTMat)-1 :-1:1
min_y(k) = (gm_(k) - UTMat(k,:) * min_y)/UTMat(k,k);
end
du_new = cgmres.dU + Vm(:,1:m)*min_y;
dU = du_new;
U = cgmres.U + dU .* cgmres.ht;
%Condensing
G3 = CalcG(cgmres.U, car.X, car.x, cgmres, car, time);
K3 = CalcK(cgmres.U+dU*cgmres.ht,...
(1-cgmres.zeta*cgmres.ht)*G3, car.x+car.dx*cgmres.ht,...
cgmres, car, time);
dX_new = (K3 - car.X)/cgmres.ht;
dX = dX_new;
X = car.X + dX*cgmres.ht;
end
%Fの計算
function F = CalcF(U,X,x,cgmres,car)
U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
X_temp = reshape(X,[car.len_x,2*cgmres.N]);
%Fのサイズを定義
F = zeros(cgmres.len_u*cgmres.N,1);
%Fの算出
F(1:cgmres.len_u) = CalcHu(U_temp(:,1), x, X_temp(:,2), cgmres.R, car);
for i=2:cgmres.N
F(cgmres.len_u*(i-1)+1:cgmres.len_u*i) = CalcHu(U_temp(:,i), X_temp(:,2*i-3), X_temp(:,2*i), cgmres.R, car);
end
end
%Gの計算
function G = CalcG(U,X,x,cgmres,car,time)
Tl = cgmres.tf*(1-exp(-cgmres.alpha*time));
dt = Tl/cgmres.N;
U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
X_temp = reshape(X,[car.len_x,2*cgmres.N]);
%Gのサイズの定義
G = zeros(2*car.len_x*cgmres.N,1);
%Gの計算
G(1:car.len_x) = X_temp(:,1) - x - func(x,U_temp(:,1),car.R,car.T)*dt;
G(car.len_x+1:car.len_x*2) = X_temp(:,2) - X_temp(:,4) - CalcHx(X_temp(:,1),cgmres.x_ob,U_temp(:,2),X_temp(:,4),cgmres.Q,car.R)*dt;
for i=2:1:cgmres.N-1
G(car.len_x*(2*i-2)+1:car.len_x*(2*i-1)) = X_temp(:,2*i-1) - X_temp(:,2*(i-1)-1) - func(X_temp(:,2*(i-1)-1),U_temp(:,i),car.R,car.T)*dt;
G(car.len_x*(2*i-1)+1:car.len_x*(2*i)) = X_temp(:,2*i) - X_temp(:,2*(i+1)) - CalcHx(X_temp(:,i),cgmres.x_ob,U_temp(:,i+1),X_temp(:,2*(i+1)),cgmres.Q,car.R)*dt;
end
G(car.len_x*(2*cgmres.N-2)+1:car.len_x*(2*cgmres.N-1)) = X_temp(:,2*cgmres.N-1) - X_temp(:,2*(cgmres.N-1)-1) - func(X_temp(:,2*(cgmres.N-1)-1),U_temp(:,cgmres.N),car.R,car.T)*dt;
G(car.len_x*(2*cgmres.N-1)+1:car.len_x*(2*cgmres.N)) = X_temp(:,2*cgmres.N) - cgmres.S*(X_temp(:,2*cgmres.N-1)-cgmres.x_ob);
end
%Kの計算
function K = CalcK(U,Z,x,cgmres,car,time)
Tl = cgmres.tf*(1-exp(-cgmres.alpha*time));
dt = Tl/cgmres.N;
U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
Z_temp = reshape(Z,[car.len_x,2*cgmres.N]);
%Kのサイズを定義
K = zeros(2*car.len_x*cgmres.N,1);
%xの計算
X_pre = ForwardK(x,U_temp,Z_temp,dt,car,cgmres);
%随伴変数の計算
Lambda = BackwardK(X_pre,U_temp,Z_temp,dt,car,cgmres);
%Kの計算
K_m = [X_pre;Lambda];
K = reshape(K_m,[2*car.len_x*cgmres.N,1]);
end
%Kでのxの計算
function X_pre = ForwardK(x,U,Z,dt,car,cgmres)
X_pre = zeros(car.len_x,cgmres.N); %X_preのサイズを定義
dx = func(x,U(:,1),car.r,car.rT);
X_pre(:,1) = x + dt*dx + Z(:,1);
for i = 2:cgmres.N
dx = func(X_pre(:,i-1),U(:,i),car.r,car.rT);
X_pre(:,i) = X_pre(:,i-1) + dt*dx + Z(:,2*i-1);
end
end
%Kでの随伴変数の計算
function Lambda = BackwardK(X_pre, U, Z, dt, car, cgmres)
Lambda = zeros(car.len_x, cgmres.N);
Lambda(:,cgmres.N) = cgmres.S*(X_pre(:,cgmres.N)-cgmres.x_ob)...
+ Z(:,2*cgmres.N);
for i = cgmres.N-1:-1:1
Lambda(:,i) = Lambda(:,i+1)...
+ CalcHx(X_pre(:,i),cgmres.x_ob,U(:,i+1),...
Lambda(:,i+1),cgmres.Q,car.R)*dt...
+Z(:,2*i);
end
end
%dH/du(の転置)
function dHdu = CalcHu(u,x,lambda,R,car)
cos_ = cos(x(3));
sin_ = sin(x(3));
B = [car.r*cos_ car.r*cos_; car.r*sin_ car.r*sin_; car.rT -car.rT];
dHdu = R*u + B'*lambda;
end
%dH/dx(の転置)
function dHdx = CalcHx(x,x_ob,u,lambda,Q,car_R)
dfdx = Calcfx(x,u,car_R);
dHdx = Q*(x-x_ob) + dfdx'*lambda;
end
%df/dx(状態方程式の状態変数微分)
function dfdx = Calcfx(x,u,car_R)
dfdx = [0 0 -(car_R/2)*sin(x(3))*(u(1)+u(2)); 0 0 (car_R/2)*cos(x(3))*(u(1)+u(2)); 0 0 0];
end
function [UTMat,Omega] = ToUTMat(H)
%UNTITLED HをGivens回転でもって上三角行列に変換する
m = length(H)-1; %Givens回転する回数
for i=1:m
nu = sqrt(H(i,i)^2 + H(i+1,i)^2);
c_i = H(i,i)/nu;
s_i = H(i+1,i)/nu;
Omega{i} = diag(ones(m+1,1));
Omega{i}(i:i+1,i:i+1) = [c_i,s_i
-s_i,c_i];
H = Omega{i} * H;
end
UTMat = H;
end
%差分駆動型二輪車のモデル(ode用)
function dxi = two_wheel_car(t,xi,u,car_R,T)
dxi = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dxi(1) = r*cos_*u(1) + r*cos_*u(2);
dxi(2) = r*sin_*u(1) + r*sin_*u(2);
dxi(3) = rT*u(1) - rT*u(2);
end
%二輪車の状態方程式
function dx = func(xi,u,car_R,car_T)
dx = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/car_T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dx(1) = r * cos_ * (u(1) + u(2));
dx(2) = r * sin_ * (u(1) + u(2));
dx(3) = rT * (u(1) - u(2));
end
ニュートン・GMRES法
close all; clear all; format compact; beep off;%#ok<CLALL>
Ts = 0.1; %サンプリング周期[sec]
t_sim_end = 20; %シミュレーション時間
tsim = 0:Ts:t_sim_end; %シミュレーションの基準となる時刻
%コントローラーのハイパーパラメーター
Ngmres.ht = Ts; %前進差分近似の時間幅[sec]
Ngmres.zeta = 1/Ts; %操作量の安定化ゲインζ
Ngmres.tf = 1.0; %予測時間の最終長さ[sec]
Ngmres.alpha = 0.5; %予測時間の上昇ゲイン
Ngmres.N = 10; %予測区間の分割数
% 評価関数中の重み
Ngmres.Q = 50 * [1 0 0; 0 1 0; 0 0 0];
Ngmres.R = 1*eye(2);
Ngmres.S = 50 * [1 0 0; 0 1 0; 0 0 0];
%車のパラメーター
car.R = 0.05; %車輪半径[m]
car.T= 0.2; %車輪と車輪の間の幅[m]
car.r = car.R/2; %よく使う値を先に計算
car.rT = car.R/car.T;
% 初期条件
car.x = [0;0;0];
Ngmres.u = [0;0];
car.len_x = length(car.x);
Ngmres.len_u = length(Ngmres.u);
%car.X = zeros(car.len_x*Ngmres.N,1); %コントローラに与える初期状態(今回はプラントの初期状態も同値とした)
Ngmres.U = zeros(Ngmres.len_u*Ngmres.N,1); %コントローラに与える初期操作量
%目標地点
Ngmres.x_ob = [2,2,0]';
% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
%シミュレーションループ
for i = 1:length(tsim)
log.u(:,i) = Ngmres.u;
log.x(:,i) = car.x;
car.x
%シミュレーション計算（最後だけは計算しない）
if i ~= length(tsim)
[t,xi] = ode45( @(t,xi) two_wheel_car(t,xi,Ngmres.u,car.R,car.T),...
[tsim(i) tsim(i+1)], car.x, opts);
else
break
end
Tl = Ngmres.tf*(1-exp(-Ngmres.alpha*Ts*i));
%ニュートン・GMRES法コントローラーを関数として実装
U = Newton_GMRES_control(car,Ngmres,Tl);
Ngmres.u = U(1:2);
Ngmres.U = U;
car.x = xi(end,:)';
end
%グラフ化
figure();
subplot(5,1,1);
plot(tsim,log.u(1,:)); ylabel('u1'); xlabel('time[s]');
subplot(5,1,2);
plot(tsim,log.u(2,:)); ylabel('u2'); xlabel('time[s]');
subplot(5,1,3);
plot(tsim,log.x(1,:)); ylabel('x1'); xlabel('time[s]');
subplot(5,1,4);
plot(tsim,log.x(2,:)); ylabel('x2'); xlabel('time[s]');
subplot(5,1,5);
plot(tsim,log.x(3,:)); ylabel('x3'); xlabel('time[s]');
%ニュートン・GMRES法コントローラー
function U = Newton_GMRES_control(car,Ngmres,Tl)
dt = Tl/Ngmres.N; %制御ホライズン内での1ステップを計算
car.dx = func(car.x,Ngmres.u,car.R,car.T);
temp.U = Ngmres.U;
%ニュートン・GMRES法のニュートンステップループ
for New = 1:1:10
F = CalcF(temp.U, car.x, Ngmres, car, dt);
%初期残差
r0 = -F;
if norm(r0) == 0
break
end
%クリロフ部分空間の最初の基底を求める
Vm(:,1) = r0/norm(r0);
% Arnoldi process(Hm_を作成する)
Hm = zeros(3,3);
% GMRESの繰り返し回数(基底ベクトルの数)
%m = Ngmres.len_u * Ngmres.N;
m = 10;
%Arnoldi法
for j=1:m
Fv = CalcF(temp.U + ( Vm(:,j) * Ngmres.ht ) , ...
car.x,Ngmres,car,dt);
Av = ( ( Fv - F ) / Ngmres.ht ); %Fxtは関数Fの状態微分
%Line 3
for k=1:j
Hm(k,j)=Av'*Vm(:,k);
end
%Line 4
temp_vec = 0;
for k = 1:j
temp_vec = temp_vec + Hm(k,j).*Vm(:,k);
end
v_hat = Av - temp_vec;
%Line 5
Hm(j+1,j) = norm(v_hat);
Vm(:,j+1) = v_hat ./ Hm(j+1,j);
end
[UTMat,Omega] = ToUTMat(Hm);
e = zeros( k + 1, 1 );
e(1) = 1;
gm_ = norm(r0)*e;
for k=1:length(Omega)
gm_ = Omega{k}*gm_;
end
min_y = zeros(length(UTMat)-1,1); %解の保存先
for k=length(UTMat)-1 :-1:1
min_y(k) = (gm_(k) - UTMat(k,:) * min_y)/UTMat(k,k);
end
temp.U = temp.U + Vm(:,1:m)*min_y;
end
U = temp.U;
end
%Fの計算
function F = CalcF(U,x,Ngmres,car,dt)
%Uを成形
U_temp = reshape(U,[Ngmres.len_u,Ngmres.N]);
%xの予測計算＆Bの値の再利用
[X, B_all] = Forward(x, U_temp, dt, car, Ngmres);
%随伴変数の計算
Lambda = Backward(X, U_temp, dt, car, Ngmres);
%Fのサイズを定める
F = zeros(Ngmres.len_u*Ngmres.N,1);
%Fの算出
for i=1:Ngmres.N
F(Ngmres.len_u*(i-1)+1:Ngmres.len_u*i) = CalcHu(U_temp(:,i), Lambda(:,i), Ngmres.R, B_all(:,Ngmres.len_u*(i-1)+1:Ngmres.len_u*i));
end
end
%xの予測計算
function [X, B_all] = Forward(x,U,dt,car,Ngmres)
X = zeros(car.len_x,Ngmres.N+1); %Xのサイズを定義
B_all = zeros(car.len_x,Ngmres.len_u*(Ngmres.N)); %B_allのサイズを定義
X(:,1) = x;
for i = 2:Ngmres.N+1
[dx, B] = funcB(X(:,i-1),U(:,i-1),car.r,car.rT);
X(:,i) = X(:,i-1) + dt*dx;
B_all(:,Ngmres.len_u*(i-2)+1:Ngmres.len_u*(i-1)) = B;
end
end
%随伴変数の計算
function Lambda = Backward(X, U, dt, car, Ngmres)
Lambda = zeros(car.len_x, Ngmres.N);
Lambda(:,Ngmres.N) = Ngmres.S*(X(:,Ngmres.N+1)-Ngmres.x_ob);
for i = Ngmres.N-1:-1:1
Lambda(:,i) = Lambda(:,i+1) + CalcHx(X(:,i+1),Ngmres.x_ob,U(:,i+1),Lambda(:,i+1),Ngmres.Q,car.R)*dt;
end
end
%dH/du(の転置)
function dHdu = CalcHu(u,lambda,R,B)
dHdu = R*u + B'*lambda;
end
%dH/dx(の転置)
function dHdx = CalcHx(x,x_ob,u,lambda,Q,car_R)
dfdx = Calcfx(x,u,car_R);
dHdx = Q*(x-x_ob) + dfdx'*lambda;
end
%df/dx(状態方程式の状態変数微分)
function dfdx = Calcfx(x,u,car_R)
dfdx = [0 0 -(car_R/2)*sin(x(3))*(u(1)+u(2)); 0 0 (car_R/2)*cos(x(3))*(u(1)+u(2)); 0 0 0];
end
function [UTMat,Omega] = ToUTMat(H)
%UNTITLED HをGivens回転でもって上三角行列に変換する
m = length(H)-1; %Givens回転する回数
for i=1:m
nu = sqrt(H(i,i)^2 + H(i+1,i)^2);
c_i = H(i,i)/nu;
s_i = H(i+1,i)/nu;
Omega{i} = diag(ones(m+1,1));
Omega{i}(i:i+1,i:i+1) = [c_i,s_i
-s_i,c_i];
H = Omega{i} * H;
end
UTMat = H;
end
%差分駆動型二輪車のモデル(ode用)
function dxi = two_wheel_car(t,xi,u,car_R,T)
dxi = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dxi(1) = r*cos_*u(1) + r*cos_*u(2);
dxi(2) = r*sin_*u(1) + r*sin_*u(2);
dxi(3) = rT*u(1) - rT*u(2);
end
%二輪車の状態方程式
function dx = func(xi,u,car_R,T)
dx = zeros(3,1); %dxiの型を定義
r = car_R/2;
rT = car_R/T;
cos_ = cos(xi(3));
sin_ = sin(xi(3));
dx(1) = r * cos_ * (u(1) + u(2));
dx(2) = r * sin_ * (u(1) + u(2));
dx(3) = rT * (u(1) - u(2));
end
%二輪車の状態方程式(CalcFで使う用)
function [dx,B] = funcB(xi,u,car_r,car_rT)
cos_ = cos(xi(3));
sin_ = sin(xi(3));
B = [car_r*cos_ car_r*cos_; car_r*sin_ car_r*sin_; car_rT -car_rT];
dx = B*u;
end
所感
自分もこれらのコードを回してみたので、いくつかわかったことを書きます。
オリジナルC/GMRES最強？
参考文献にはMultiple-Shootingを用いたC/GMRES法は解の安定性が増し、ニュートン・GMRES法は最適解の追従能力が上がる事が期待される的な事が書いてあったのですが、両方ともオリジナルのC/GMRES法に比べてすぐに暴走します。暴走するのが正しい最適解なのであればシステム上は計算の精密さが上がったことにはなりますが、それで実システムに使えるわけが無いので、結局使うとするとC/GMRES法という事に？
↓
ニュートン・GMRES法についてはコードを修正したところ安定性、目標値の追従性共々C/GMRESと同じくらいの性能を出せました。しかし、計算負荷の側面ではC/GMRES法より重くなってしまうため、最適解の急激な変化を伴う場合以外はC/GMRES法の方が良いのではないかと思われます。自分のコードではMultiple-Shooting C/GMRES法は不安定さが増してしまっており、参考にした論文に書かれていたような性能を実現できていないままです。
遅い！
評価関数を調整すればC/GMRES法とMultiple-Shooting C/GMRES法については10秒ほどでほぼ目標地点に到達出来るくらいにはなります（それでも遅いですが）。より高速移動する事を目指してパラメーターをいじくってみたのですが、上手く行きませんでした。ニュートン・GMRES法については20秒でも目標地点に近づききれないです。
（追記）
コードを修正したところ、ニュートン・GMRES法でも10秒程度で目標値付近に到達できるようになりました。
回転暴走
C/GMRES法ではほぼ問題にならなかったのですが、Multiple-Shooting C/GMRES法とニュートン・GMRES法は評価関数のステージコストにおける座標の重み、及び終端コストの重みを上げるとすぐに暴走します。具体的には、二輪車が何故か回転しまくります。その為、後者二つのコードでは回転運動を抑制する項を評価関数に追加して対応しています。それでもまだ暴走しやすいです。パラメーターさえ揃えれば理論的にはどの手法も同じ最適解を追跡するはずなのですが、ここまで差が出る理由が不明です。
評価関数の重みについて
Multiple-Shooting C/GMRESにおいて、cgmres.Qを-10にすると何故か上手く行きました...。評価関数の重みがマイナスで上手く行くのが何故なのかさっぱりわかりませんが、そうなったという事だけ記しておきます。（コード修正前も後もそうなったので、その部分のミスによるものでは無いと思われすが、自分のコードがそもそもどこかおかしい可能性は否定できません）
最後に
今後も何か新しい物が実装出来たら載せてみようかと思います。個人的にはTube-based MPCあたりが面白そうかなと思っています。