はじめに
今までにいくつかモデル予測制御のコードを載せてきたのですが、バリア関数の部分に我流で若干無理やりな部分が残っていました。今回はRelaxed-log Barrier functionと言う対数バリア関数の改良版のコードを作り、モデル予測制御のコードに追加したいと思います。
環境
MATLAB R2023a
参考文献
最初に参考文献を載せておきます。
- Relaxed Logarithmic Barrier Function Based Model Predictive Control of Linear Systems (C.Feller and C. Ebenbauer, 2015, arxiv)
- 非線形最適制御入門（大塚敏之，2011）
モデル予測制御のコードについては以下の自分の記事から引っ張ってきています。
内点法
最適化問題において不等式制約を考慮する方法はいくつかあるのですが、その内の一つに内点法と言うのがあります。最小化問題においては、制約条件を満たさなくなろうとすると値が大きく、制約条件を余裕をもって満たすほど値が小さくなるような関数を評価関数に追加してしまう手法です。例えば、次の様な最適化問題を考えます。
\begin{align}
\min_{x} \ f(x) \\
\text{subject to} \ \ x\geqq d
\end{align}
つまり、$x$が$d$に近づくほど大きく、$x$から正の方向に$d$から遠ざかる程小さくなる関数を考えます。良く使われるのは次のような対数関数です。
-\log(x-d)
これを問題の関数に加える事で、次のような最適化問題に変形します。
\min_{x} \ f(x)-\log(x-d)
$x$が$d$以下になろうとすると関数の値が大きくなってしまうので、最小化問題を解くと（解析的には）必然的に制約条件を満たすわけです。ただし、制約条件を満たしていても限界値付近でペナルティーが発生してしまうため、元の最適化問題の解とは若干のズレが出ます。
最適化問題におけるこの様な対数関数を対数バリア関数と言います。
対数バリア関数の問題点
最適化問題を数値的に解く際、勾配を使った手法を用いる事が多いと思います。ただし、そうした数値計算法は離散的な解の更新計算を含みます。簡単な例で言うと次の様な感じだと思います。
x \leftarrow x - lr \frac{\partial f}{\partial x}
ここで学習率の値が適切に調整されていないと、更新の際に解が制約条件の外に飛び出てしまう事があります。ただし、対数は真数が$0$以下、つまり制約条件外では定義出来ませんから、ここで計算が止まってしまう事になります。
Relaxed 対数バリア関数
これを解決するために、対数バリア関数の特徴は保ったまま、制約条件外でも定義できるように改良する必要があります。その一つが元論文で挙げられているRelaxed Log Barrier Functionです。簡単に言うと制約条件ギリギリ及び制約条件を満たさない範囲は二次関数や三次関数、制約条件を満たす範囲は対数関数となる様に二次関数（や三次関数）と対数関数をくっつけた関数です。元論文のFigure１を見て頂くのが一番わかりやすいと思います。この記事では二次関数と対数関数の組み合わせを使います。
Relaxed 対数バリア関数の実装
例えば、入力ベクトル$u=[u_{1},u_{2},u_{3}]^{T}$に対する制約条件として、次のようなものを考えます。
c_{1}u_{1}+c_{2}u_{2}+c_{3}u_{3} \leqq d
ここで、のちの見やすさのため、次のように変数$z$を置いて制約条件を書き換えます。
\begin{align}
z &= d - (c_{1}u_{1}+c_{2}u_{2}+c_{3}u_{3})\\
z &\geqq 0
\end{align}
これに対して、次のようなバリア関数$B$を設定します。元論文の式(10)に$k=2$を代入したものを使用しています。
B=
\begin{cases}
-\log z & (z\geqq\delta) \\
\frac{1}{2}\left[\left(\frac{z-2\delta}{\delta}\right)^{2}- 1\right] - \log\delta & (z\leqq \delta)
\end{cases}
$\delta$は任意の微小値です。つまり、$z$の値が$0$に近づくとバリア関数が二次関数にスイッチするような仕組みです。計算してみればわかるのですが、バリア関数の値と勾配がきちんと連続になるように設定されています。
ここまでをコードにすると次の様になります。
control.bar_d が $d$
control.bar_C が ベクトル$c=[c_{1},c_{2},c_{3}]$
control.del_bar が $\delta$
に対応します。
%バリア関数（controlはパラメータの構造体）
function Bar = barrier(u,control)
z = control.bar_d - control.bar_C*u; %zに直す
Bar = barrier_z(z,control.del_bar); %zに直してから計算
end
%バリア関数（-logか二次関数かを勝手に切り替えてくれる）
function value = barrier_z(z,delta)
if z > delta
value = -log(z);
else
value = (1/2)*( ((z-2*delta)/delta)^2 -1) - log(delta);
end
end
では、次の様に制約条件が複数ある場合に拡張します。
\begin{align}
c_{11}u_{1}+c_{12}u_{2}+c_{13}u_{3} \leqq d_{1}\\
c_{21}u_{1}+c_{22}u_{2}+c_{23}u_{3} \leqq d_{2}\\
c_{31}u_{1}+c_{32}u_{2}+c_{33}u_{3} \leqq d_{3}
\end{align}
行列を用いてこれらの制約条件を次のように表します。
\begin{align}
Cu &\leqq d\\
C &=
\begin{pmatrix}
c_{11} & c_{12} & c_{13} \\
c_{21} & c_{22} & c_{23} \\
c_{31} & c_{32} & c_{33}
\end{pmatrix} \\
d &=
\begin{pmatrix}
d_{1}\\
d_{2}\\
d_{3}
\end{pmatrix}
\end{align}
$z$も次の様にベクトルで表します。
\begin{align}
z &= d - Cu\\
z &= [z_{1},z_{2},z_{3}]^{T}\\
z_{1} &= d_{1} - (c_{11}u_{1}+c_{12}u_{2}+c_{13}u_{3})\\
z_{2} &= d_{2} - (c_{21}u_{1}+c_{22}u_{2}+c_{23}u_{3})\\
z_{3} &= d_{3} - (c_{31}u_{1}+c_{32}u_{2}+c_{33}u_{3})
\end{align}
確認ですが、制約条件の数だけバリア関数も必要になります。つまりバリア関数全体では次のようになります。
B = B_{1}(z_{1})+B_{2}(z_{2})+B_{3}(z_{3})
ではコードを書き換えます。ベクトル$z$の値を上から一つずつ取り出して計算するようにします。
zs がベクトルにした$z$に対応します。
control.con_dim は拘束条件の数です。
%バリア関数全体（controlはパラメータの構造体）
function Bar = barrier(u,control)
zs = control.bar_d - control.bar_C*u;
Bar = 0;
%拘束条件の数だけ繰り返す
for i = 1:control.con_dim
%zの値を上からとってbarrier_zに渡す&バリア関数合計値を更新
Bar = Bar + barrier_z(zs(i),control.del_bar);
end
end
%バリア関数（-logか二次関数かを勝手に切り替えてくれる）
function value = barrier_z(z,delta)
if z > delta
value = -log(z);
else
value = (1/2)*( ((z-2*delta)/delta)^2 -1) - log(delta);
end
end
バリア関数の微分計算の実装
モデル予測制御にバリア関数を実装する際、バリア関数の微分値やヘッシアンが必要になります。それらを作っていきます。
まず、数式で確認します。一つのバリア関数$B_{i}$を入力$u$で微分すると例えば次のようになります。
\begin{align}
\frac{\partial B_{i}}{\partial u} &= \frac{\partial B_{i}}{\partial z_{i}} \frac{\partial z_{i}}{\partial u}\\
\\
\frac{\partial B_{i}}{\partial z_{i}} &=
\begin{cases}
-\frac{1}{z_{i}} & (z_{i}\geqq \delta) \\
\frac{z_{i}-2\delta}{\delta} & (z_{i}\leqq \delta)
\end{cases}\\
\\
\frac{\partial z_{i}}{\partial u} &= -C_{i} = -[c_{i1},c_{i2},c_{i3}]
\end{align}
これを全てのバリア関数について計算し、合計します。
ここまでをコードで作ります。
%バリア関数のu微分（答えは行ベクトルになる）
function dBdu = barrier_du(u,control)
zs = control.bar_d - control.bar_C*u;
dBdu = zeros(1,control.len_u); %サイズを決定しておく（無くても良い）
for i = 1:control.con_dim %拘束条件の数（＝バリア関数の数）だけ繰り返す
dBdz = barrier_dz(zs(i),control.del_bar);
dBdu = dBdu + dBdz*(-control.bar_C(i,:)); %dBdzにdzduをかけて足していく
end
end
%バリア関数の微分値（B(z)のz微分）
function value = barrier_dz(z,delta)
if z > delta
value = -(1/z);
else
value = (z-2*delta)/delta;
end
end
では最後にヘッシアンを作ります。数式では次のようになります。
\begin{align}
\frac{\partial^{2} B_{i}}{\partial u^{2}} &= \left(\frac{\partial z_{i}}{\partial u}\right)^{T} \frac{\partial^{2} B_{i}}{\partial z_{i}^{2}} \frac{\partial z_{i}}{\partial u}\\
\\
\frac{\partial^{2} B_{i}}{\partial z_{i}^{2}} &=
\begin{cases}
\frac{1}{z_{i}^{2}} & (z_{i}\geqq \delta) \\
\frac{1}{\delta} & (z_{i}\leqq \delta)
\end{cases}
\end{align}
コードを作ります。
%バリア関数のuヘッシアン（答えは行列になる）
function dBduu = barrier_hes_u(u,control)
zs = control.bar_d - control.bar_C*u;
dBduu = zeros(control.len_u,control.len_u);
for i = 1:control.con_dim
dBdzz = barrier_hes_z(zs(i),control.del_bar);
dBduu = dBduu + control.bar_C(i,:)'*dBdzz*control.bar_C(i,:);
end
end
%バリア関数のz二階微分（B(z)のz二階微分）
function value = barrier_hes_z(z,delta)
if z > delta
value = 1/(z^2);
else
value = 1/delta;
end
end
モデル予測制御にRelaxed 対数バリア関数を導入する
参考文献にも書きましたが、モデル予測制御のコードについては以前の自分の記事を参照してください（この記事に影響ないレベルで若干のマイナーチェンジはありますが）。
変更点部分だけを見て行きます。
まずC/GMRESにおいては、$\partial H/\partial u$の計算でバリア関数の微分を使います。該当箇所を次の様に修正します。cgmres.b はバリア関数の重みです。
%dH/du(の転置)
function dHdu = CalcHu(u,x,lambda,cgmres,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];
dBdu = barrier_du(u,cgmres);
dHdu = cgmres.R*u + B'*lambda + cgmres.b*dBdu';
end
次に、iLQRのコードを修正します。$Quu$と$Qu$の計算にバリア関数の項を加えます。iLQR.bはバリア関数の重みです。
%Backward Pass
function [K,d,dV1,dV2] = Backward(X,U,iLQR,car)
sk = (X(:,end)-iLQR.x_ob)'*iLQR.S; %Vxの初期値
Sk = iLQR.S; %Vxxの初期値
Qk = iLQR.Q;
Rk = iLQR.R;
K = zeros(iLQR.len_u,iLQR.len_x,iLQR.N);
d = zeros(iLQR.len_u,iLQR.N);
dV1 = 0; %dが1次の項を入れる変数
dV2 = 0; %dが2次の項を入れる変数
for i = iLQR.N:-1:1
x = X(:,i);
u = U(:,i);
Ak = CalcA(x,u,iLQR.dt,car);
Bk = CalcB(x,iLQR.dt,car);
Qx = ((x-iLQR.x_ob)'*Qk)*iLQR.dt + sk*Ak;
Qxx = Qk*iLQR.dt + Ak'*Sk*Ak;
dBdu = barrier_du(u,iLQR);
dBduu = barrier_hes_u(u,iLQR);
Qu = (u'*Rk)*iLQR.dt + sk*Bk + iLQR.b*dBdu*iLQR.dt;
Quu = (Rk)*iLQR.dt + Bk'*Sk*Bk + iLQR.b*dBduu*iLQR.dt;
Qux = Bk'*Sk*Ak;
K_= -inv(Quu)*Qux; %閉ループゲインの計算
K(:,:,i) = K_;
d_ = -inv(Quu)*Qu'; %開ループフィードバックの計算
d(:,i) = d_;
sk = Qx + d_'*Quu*K_ + Qu*K_ + d_'*Qux; %Vxの更新
Sk = Qxx + K_'*Quu*K_ + K_'*Qux + Qux'*K_; %Vxxの更新
dV1 = dV1 + Qu*d_;
dV2 = dV2 + (1/2)*d_'*Quu*d_;
end
end
全体コード
最後に全体コードを置いておきます。（細かい所に以前の記事とのマイナーチェンジがありますが、今回の記事の内容には影響しません。）
C/GMRES
CGMRES.m
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 = 1*eye(2);
cgmres.S = 100 * [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);
cgmres.U = zeros(cgmres.len_u*cgmres.N,1); %コントローラに与える初期操作量
cgmres.dU = 0;
%目標地点
cgmres.x_ob = [3,2,0]';
%拘束条件
cgmres.umax = [10;10];
cgmres.umin = [-10;-10];
%バリア関数用のパラメータ
cgmres.b = 0.3; %バリア関数のパラメータ
cgmres.del_bar = 0.5;
cgmres.bar_C = [eye(cgmres.len_u); -eye(cgmres.len_u)];
cgmres.bar_d = [cgmres.umax; -cgmres.umin];
cgmres.con_dim = length(cgmres.bar_d);
% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
%描画範囲
area=[-0.2 3 -0.2 3];
tic;
%シミュレーションループ
for i = 1:length(tsim)
log.u(:,i) = cgmres.u;
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);
%xi = car.x + func(car.x,cgmres.u,car.R,car.T)*Ts;
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_preb = car.x;
car.x = xi(end,:)';
%car.x = xi;
end
toc;
%グラフ化
f=figure();
subplot(5,1,1)
plot(tsim,log.x(1,:),'LineWidth',1); ylabel('x1'); xlabel('Time[s]');
ylim([0,3])
%yticks(0:1:3)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,2);
plot(tsim,log.x(2,:),'LineWidth',1); ylabel('x2'); xlabel('Time[s]');
ylim([0,2])
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,3);
plot(tsim,log.x(3,:),'LineWidth',1); ylabel('Φ'); xlabel('Time[s]');
%yticks(0:0.1:0.7)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,4);
plot(tsim,log.u(1,:),'LineWidth',1); ylabel('u1'); xlabel('Time[s]');
%yticks(0:1:11)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,5);
plot(tsim,log.u(2,:),'LineWidth',1); ylabel('u2'); xlabel('Time[s]');
%yticks(0:1:10)
grid on;
set(gca, 'FontSize', 9.5)
%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;
% GMRESの繰り返し回数(基底ベクトルの数)
m = 10; %cgmres.len_u * cgmres.N;
Vm = zeros(cgmres.len_u*cgmres.N,m+1);
%1つ目の基底ベクトル
Vm(:,1) = r0 ./ norm(r0);
% Arnoldi process(Hm_を作成する)
Hm = zeros(m+1,m);
%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の予測計算
X = 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), X(:,i), Lambda(:,i), cgmres, car);
end
end
%xの予測計算
function X = Forward(x,U,dt,car,cgmres)
X = zeros(car.len_x,cgmres.N+1); %Xのサイズを定義
X(:,1) = x;
for i = 2:cgmres.N+1
dx = func(X(:,i-1),U(:,i-1),car.r,car.rT);
X(:,i) = X(:,i-1) + dt*dx;
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,x,lambda,cgmres,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];
dBdu = barrier_du(u,cgmres);
dHdu = cgmres.R*u + B'*lambda + cgmres.b*dBdu';
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
%バリア関数全体
function Bar = barrier(u,control)
zs = control.bar_d - control.bar_C*u;
Bar = 0;
%拘束条件の数だけ
for i = 1:control.con_dim
Bar = Bar + barrier_z(zs(i),control.del_bar); %値を足していく
end
end
%バリア関数のu微分（答えは行ベクトルになる）
function dBdu = barrier_du(u,control)
zs = control.bar_d - control.bar_C*u;
dBdu = zeros(1,control.len_u);
for i = 1:control.con_dim
dBdz = barrier_dz(zs(i),control.del_bar);
dBdu = dBdu + dBdz*(-control.bar_C(i,:));
end
end
%バリア関数のuヘッシアン（答えは行列になる）
function dBduu = barrier_hes_u(u,control)
zs = control.bar_d - control.bar_C*u;
dBduu = zeros(control.len_u,control.len_u);
for i = 1:control.con_dim
dBdzz = barrier_hes_z(zs(i),control.del_bar);
dBduu = dBduu + control.bar_C(i,:)'*dBdzz*control.bar_C(i,:);
end
end
%バリア関数（-logか二次関数かを勝手に切り替えてくれる）
function value = barrier_z(z,delta)
if z > delta
value = -log(z);
else
value = (1/2)*( ((z-2*delta)/delta)^2 -1) - log(delta);
end
end
%バリア関数の微分値（B(z)のz微分）
function value = barrier_dz(z,delta)
if z > delta
value = -(1/z);
else
value = (z-2*delta)/delta;
end
end
%バリア関数のz二階微分（B(z)のz二階微分）
function value = barrier_hes_z(z,delta)
if z > delta
value = 1/(z^2);
else
value = 1/delta;
end
end
iLQR
close all; clear all; format compact; beep off;%#ok<CLALL>
Ts = 0.1; %サンプリング周期[sec]
t_sim_end = 20; %シミュレーション時間
tsim = 0:Ts:t_sim_end; %シミュレーションの基準となる時刻
%コントローラーのパラメーター
iLQR.Ts = Ts; %制御周期
iLQR.tf = 1.0; %予測ホライズンの長さ[sec]
iLQR.N = 10; %予測区間の分割数
iLQR.iter = 100; %繰り返し回数の上限値
iLQR.dt = iLQR.tf/iLQR.N; %予測ホライズンの分割幅
iLQR.torelance = 1; %評価関数のズレの許容範囲
% 評価関数中の重み
iLQR.Q = 100 * [1 0 0; 0 1 0; 0 0 0];
iLQR.R = 1*eye(2);
iLQR.S = 100 * [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];
iLQR.u = [0;0];
iLQR.len_x = length(car.x);
iLQR.len_u = length(iLQR.u);
iLQR.U = zeros(iLQR.len_u,iLQR.N); %コントローラに与える初期操作量
%目標地点
iLQR.x_ob = [3,2,0]';
%拘束条件
iLQR.umax = [15;15];
iLQR.umin = [-15;-15];
%バリア関数用のパラメータ
iLQR.b = 0.3; %バリア関数の重み
iLQR.del_bar = 0.5;
iLQR.bar_C = [eye(iLQR.len_u); -eye(iLQR.len_u)];
iLQR.bar_d = [iLQR.umax; -iLQR.umin];
iLQR.con_dim = length(iLQR.bar_d);
% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
%臨時追加
X = zeros(iLQR.len_x,iLQR.N+1);
log.X = zeros(iLQR.len_x,iLQR.N+1,401);
tic;
%シミュレーションループ
for i = 1:length(tsim)
log.u(:,i) = reshape(iLQR.U,[iLQR.len_u*iLQR.N,1]);
log.x(:,i) = car.x;
log.X(:,:,i) = X;
car.x
%シミュレーション計算（最後だけは計算しない）
if i ~= length(tsim)
[t,xi] = ode45( @(t,xi) two_wheel_car(t,xi,iLQR.u,car),...
[tsim(i) tsim(i+1)], car.x, opts);
%xi = func_risan(car.x,iLQR.u,iLQR.dt,car);
else
break
end
%iLQR法コントローラーを関数として実装
[U,X] = iLQR_control(iLQR,car);
iLQR.u = U(:,1);
iLQR.U = U;
car.x = xi(end,:)';
end
toc;
%
%グラフ化
f=figure();
subplot(5,1,1)
plot(tsim,log.x(1,:),'LineWidth',1); ylabel('x1'); xlabel('Time[s]');
ylim([0,3])
%yticks(0:1:3)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,2);
plot(tsim,log.x(2,:),'LineWidth',1); ylabel('x2'); xlabel('Time[s]');
ylim([0,2])
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,3);
plot(tsim,log.x(3,:),'LineWidth',1); ylabel('Φ'); xlabel('Time[s]');
%yticks(0:0.1:0.7)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,4);
plot(tsim,log.u(1,:),'LineWidth',1); ylabel('u1'); xlabel('Time[s]');
%yticks(0:1:11)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,5);
plot(tsim,log.u(2,:),'LineWidth',1); ylabel('u2'); xlabel('Time[s]');
%yticks(0:1:10)
grid on;
set(gca, 'FontSize', 9.5)
%}
%iLQRコントローラー
function [U,X] = iLQR_control(iLQR,car)
U = iLQR.U; %前ステップの入力を初期解として使う
X = Predict(car.x,U,iLQR,car); %状態変数の将来値を入力初期値から予測
J = CalcJ(X,U,iLQR); %評価関数の初期値を計算
loop = 0;
while true
[K,d,dV1,dV2] = Backward(X,U,iLQR,car); %Backward Passの計算
[X,U,J_new] = Forward(X,U,K,d,dV1,dV2,J,iLQR,car); %Forward Passの計算
loop = loop + 1;
if abs(J_new-J) <= iLQR.torelance %評価関数値が収束して来たら
break
end
if loop == iLQR.iter %繰り返し回数が限界に来たら
break
end
J = J_new;
end
end
%状態変数の初期予測関数
function X = Predict(x,U,iLQR,car)
xk = x;
xk = func_risan(xk,U(:,1),iLQR.dt,car);
X = zeros(iLQR.len_x,iLQR.N+1);
X(:,1) = xk;
for i = 1:1:iLQR.N
xk = func_risan(xk,U(:,i),iLQR.dt,car);
X(:,i+1) = xk;
end
end
%評価関数の計算
function J = CalcJ(X,U,iLQR)
%終端コストの計算
phi = (X(:,end) - iLQR.x_ob)' * iLQR.S * (X(:,end) - iLQR.x_ob);
%途中のコストの計算
L = 0;
for i = 1:1:iLQR.N
bar_val = barrier(U(:,i),iLQR);
L = L + (X(:,i) - iLQR.x_ob)' * iLQR.Q * (X(:,i) - iLQR.x_ob)...
+ U(:,i)' * iLQR.R * U(:,i)...
+ iLQR.b * bar_val;
end
L = L*iLQR.dt; %最後にまとめてdtをかける
J = phi + L;
end
%Backward Pass
function [K,d,dV1,dV2] = Backward(X,U,iLQR,car)
sk = (X(:,end)-iLQR.x_ob)'*iLQR.S; %Vxの初期値
Sk = iLQR.S; %Vxxの初期値
Qk = iLQR.Q;
Rk = iLQR.R;
K = zeros(iLQR.len_u,iLQR.len_x,iLQR.N);
d = zeros(iLQR.len_u,iLQR.N);
dV1 = 0; %dが1次の項を入れる変数
dV2 = 0; %dが2次の項を入れる変数
for i = iLQR.N:-1:1
x = X(:,i);
u = U(:,i);
Ak = CalcA(x,u,iLQR.dt,car);
Bk = CalcB(x,iLQR.dt,car);
Qx = ((x-iLQR.x_ob)'*Qk)*iLQR.dt + sk*Ak;
Qxx = Qk*iLQR.dt + Ak'*Sk*Ak;
dBdu = barrier_du(u,iLQR);
dBduu = barrier_hes_u(u,iLQR);
Qu = (u'*Rk)*iLQR.dt + sk*Bk + iLQR.b*dBdu*iLQR.dt;
Quu = (Rk)*iLQR.dt + Bk'*Sk*Bk + iLQR.b*dBduu*iLQR.dt;
Qux = Bk'*Sk*Ak;
K_= -inv(Quu)*Qux; %閉ループゲインの計算
K(:,:,i) = K_;
d_ = -inv(Quu)*Qu'; %開ループフィードバックの計算
d(:,i) = d_;
sk = Qx + d_'*Quu*K_ + Qu*K_ + d_'*Qux; %Vxの更新
Sk = Qxx + K_'*Quu*K_ + K_'*Qux + Qux'*K_; %Vxxの更新
dV1 = dV1 + Qu*d_;
dV2 = dV2 + (1/2)*d_'*Quu*d_;
end
end
%Forward
function [X,U,J] = Forward(X,U,K,d,dV1,dV2,J,iLQR,car)
alpha = 1; %直線探索の係数を初期化
X_ = zeros(iLQR.len_x,iLQR.N+1); %新しいxの値を入れていく変数
X_(:,1) = X(:,1); %xの初期値は変化しない
U_ = zeros(iLQR.len_u,iLQR.N); %新しいuの値を入れていく変数
%直線探索を終わらせるためのカウント
count = 0;
%評価関数の最少記録とその周辺
J_min = J;
U_min = U;
X_min = X;
while true
for i = 1:1:iLQR.N
U_(:,i) = U(:,i) + K(:,:,i)*(X_(:,i)-X(:,i)) + alpha*d(:,i);
X_(:,i+1) = func_risan(X_(:,i),U_(:,i),iLQR.dt,car);
end
J_new = CalcJ(X_,U_,iLQR);
dV1_ = alpha*dV1;
dV2_ = (alpha^2)*dV2;
z = (J_new-J)/(dV1_+dV2_);
if 1e-4 <= z && z <= 10 %直線探索が条件を満たしていれば
J = J_new;
U = U_;
X = X_;
break
end
if J_min > J_new %評価関数の最少記録を更新したら
J_min = J_new;
U_min = U_;
X_min = X_;
end
if count == 10 %10回やっても直線探索が上手く行かなければ
J = J_min;
U = U_min;
X = X_min;
break
end
alpha = (1/2)*alpha;
count = count + 1;
end
end
%Akの計算
function Ak = CalcA(x,u,dt,car)
Ak = eye(3) + ...
[0 0 -car.r*sin(x(3))*(u(1)+u(2))
0 0 car.r*cos(x(3))*(u(1)+u(2))
0 0 0]*dt;
end
%Bkの計算
function Bk = CalcB(x,dt,car)
cos_ = cos(x(3));
sin_ = sin(x(3));
Bk = [car.r*cos_ car.r*cos_
car.r*sin_ car.r*sin_
car.rT -car.rT]*dt;
end
%差分駆動型二輪車のモデル(ode用)
function dxi = two_wheel_car(t,xi,u,car)
dxi = zeros(3,1); %dxiの型を定義
r = car.r;
rT = car.rT;
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 xk1 = func_risan(xk,u,dt,car)
xk1 = zeros(3,1); %xk1の型を定義
r = car.r;
rT = car.rT;
cos_ = cos(xk(3));
sin_ = sin(xk(3));
xk1(1) = xk(1) + (r * cos_ * (u(1) + u(2)))*dt;
xk1(2) = xk(2) + (r * sin_ * (u(1) + u(2)))*dt;
xk1(3) = xk(3) + (rT * (u(1) - u(2)))*dt;
end
%バリア関数全体
function Bar = barrier(u,control)
zs = control.bar_d - control.bar_C*u;
Bar = 0;
%拘束条件の数だけ
for i = 1:control.con_dim
Bar = Bar + barrier_z(zs(i),control.del_bar); %値を足していく
end
end
%バリア関数のu微分（答えは行ベクトルになる）
function dBdu = barrier_du(u,control)
zs = control.bar_d - control.bar_C*u;
dBdu = zeros(1,control.len_u);
for i = 1:control.con_dim
dBdz = barrier_dz(zs(i),control.del_bar);
dBdu = dBdu + dBdz*(-control.bar_C(i,:));
end
end
%バリア関数のuヘッシアン（答えは行列になる）
function dBduu = barrier_hes_u(u,control)
zs = control.bar_d - control.bar_C*u;
dBduu = zeros(control.len_u,control.len_u);
for i = 1:control.con_dim
dBdzz = barrier_hes_z(zs(i),control.del_bar);
dBduu = dBduu + control.bar_C(i,:)'*dBdzz*control.bar_C(i,:);
end
end
%バリア関数（-logか二次関数かを勝手に切り替えてくれる）
function value = barrier_z(z,delta)
if z > delta
value = -log(z);
else
value = (1/2)*( ((z-2*delta)/delta)^2 -1) - log(delta);
end
end
%バリア関数の微分値（B(z)のz微分）
function value = barrier_dz(z,delta)
if z > delta
value = -(1/z);
else
value = (z-2*delta)/delta;
end
end
%バリア関数のz二階微分（B(z)のz二階微分）
function value = barrier_hes_z(z,delta)
if z > delta
value = 1/(z^2);
else
value = 1/delta;
end
end
補足参考文献
この記事とは直接関係ないのですが、モデル予測制御のコードは以下の記事、著作物を参考にしております。