車の速度を一定に保つ最適制御及びそのソルバをMATLABで実装する

概要

最近最適制御を勉強しており、勉強がてらちょっとした制御システム&シミュレーターを作って試してみました。

坂がある中、車の速度をなるべく保つようにする制御を、最適化制御問題として解きました。ソルバーとして、(1)Adam Optimizerを内包した勾配法と(2)MATLABに最初から用意されているfminunc の両者を使用して比較したところ、(1)も(2)も優れた制御を実現していることと、(1)の方が少しだけ優れた制御をしていることが分かりました(!!)。

図. 制御シミュレーションの結果

問題設定

プラント

• 経路程$z$、速度$v$を状態$x=[z,v]$として持つ車があります。この車がプラントです。
• この車は最初$x_0=[z_0,v_0]=[0,v^{ref}]$の状態にあります。ただし、$v^{ref}=10$です。
• この車はこの先走っていくと、坂に出会います。坂があると、車には強制的に加速度が生じます。坂による車への加速度を、車の現在の経路程$z$を用いて$s(z)$とします。
• また、この車にはアクセルとブレーキ$u(t)$があり、$u(t)$を車への加速度とすることができます。

これらのことから、この車の状態方程式は

\dot{x}(t) = f(x(t)) = [v(t),u(t)+s(z(t))]

となります。

坂

坂による加速度の情報は関数形式で表す必要があるので、以下のようなクラスを定義して、Slopedata#s(z)で$s(z)$を調べられるようにします✨

Slopedata2.m

classdef Slopedata2<Slopedata
    properties
        
        pattern = [...
            40 0
            50 -2
            70 0
            80 1
            130 0
            135 -1
            145 0
            155 -1
            180 0
            200 1
            250 0
            300 -1
            320 0 ...
            ]';
    end
    methods
        function obj = Slopedata2()
        end
        function s = s(obj, z)
            for x = obj.pattern
                if x(1)>z
                    s = x(2);
                    return
                end
            end
            s = 0;
        end
        function sonz = sonz(obj,z)
            sonz = 0;
        end
    end
end

ここではコードを示しませんが、上記のSlopedata2は基底クラスSlopedataを継承・実装しています。

今回の坂の形状はパキパキしてて、緩和曲線がありません。つまり、s(z) の z 微分は常に 0。グラフにすると以下みたいな感じです。

図.　坂の様子

なぜパキパキの適当な坂にしたのかは、後述します。あと、この坂データはコントローラーから全て観測可能とします。カーナビの中に地理情報が全部入っているような感じです。

目標

今回の目標は、以下のような評価関数$J$を最小にするような入力$u(k)$を求めることです。なお、全て離散時間系で処理していきます。

J = \phi(x(N)) + \sum_{k=1}^N L(x(k), u(k))

ただし、ステージコスト$L$は

L(x(k), u(k)) = \frac{1}{2} (v(k)-v^{ref})^2 + \frac{10^3}{2} u(k)^2

終端コスト$\phi(x(N))$は

\phi(x(N)) = \frac{1}{2} (v(N)-v^{ref})^2

評価区間の終端ステップ$N$は

N = 100

です。

この評価関数では以下のような目標を意図しています。

• 速度は$v=v^{ref}$の一定で走ってほしい。
• アクセル/ブレーキ操作$u$は大きさが0.3を超えてはいけないという不等式制約。

両者はトレードオフの関係にあります。一定の速度で走るためには、坂に入る/出る瞬間にアクセル/ブレーキを頑張ってかける必要があります。が、そうなるとアクセル/ブレーキ操作が強くなりすぎるかもしれません。逆に、アクセル/ブレーキを穏やかにしすぎると、坂に入る/出る瞬間に速度が大きく変化してしまいます。

ということで、2つの項目をうまい感じに色々調整してくれるコントローラーを組みましょう。

車のシミュレーター

車の状態$x$を更新する関数を作っておきます。

car_step.m

function nextx = car_step(x,u,delta_t,slopedata)
    z = x(1);
    v = x(2);
    s = slopedata.s(z);

    dotx = [v u+s];
    nextx = x + dotx*delta_t;
end

car_steps.m

function x_s = car_steps(x0,u_s,delta_t,slopedata)
    N = length(u_s);
    x_s = zeros([N 2]);
    x = x0;
    for k=1:N
        u = u_s(k);
        x = car_step(x,u,delta_t,slopedata);
        x_s(k,:) = x;
    end
end

シミュレーションや求解でのルール

以下からのシミュレーションや制御解の求解では、

• 時間の刻み幅 $\Delta t=0.5$
• 評価区間の終端時刻 $t_{final} = 50$
• 評価区間の終端ステップ $N = 100$

としています。

delta_t = 0.5;
t_final = 50;
max_k = t_final/delta_t;
t_s = (1:max_k)*delta_t;

零入力応答 : 何も操作しないと車はどうなるのか見てみる

とりあえずは、何もアクセル/ブレーキ操作をしない、つまり、$u(k)=0\ for\ all\ k$の時に車の状態$x(k)$がどうなるか見ていきます。

free_u_s = zeros(size(t_s));%操作しない
free_x_s = car_steps(x0,free_u_s,delta_t,slopedata);%車の状態のシミュレーション

figure
plot(free_x_s(:,1),free_x_s(:,2))
xlabel("z [m]"); ylabel("v [m/s]")

図. システムの零入力応答
途中で坂を登りきれずにバックしてて草

最適制御の解を求めるソルバー

今回の目標は最適な$u(k)\ (k =1,\dots ,N)$を求めることです。これは(今回は離散ですけど) 凡関数の最適化問題に帰着します。
関数の最適化とは、関数が最小値を取るような「点」を見つけることでしたが、凡関数の最適化とは、とある関数が最小値を取るような「線」を見つけることに相当します。

勾配法

凡関数の最適化であっても、関数の最適化と同様、「勾配法」のようなものを行うことができます。
具体的には以下のような方法です。



大塚(2018). 非線形最適制御入門. コロナ社

なお、ハミルトン関数は$H(x,u,λ,t) = L(x,u,t) + λ^T f(x,u,t)$と定義されます。

で、ちょっとここで気になったんですが、$u(t)$の更新の部分、丸々SGDですよね。今回は解の発散や収束の遅れを防止するため (学習率を調整するのが面倒臭かったから) 、この$u(k)$の更新を、SGDではなくて、人工知能界隈で有名なあの Adam Optimizer にやらせることにします！！

という訳でこれまでのことをまとめると以下のようになりました。まずはAdam Optimizerの定義です。

Adam.m

classdef Adam
    properties
        m = [];
        v = [];
        beta1 = 0.95;
        beta2 = 0.99;
        eta = 0.01;

    end
    
    methods
        function obj = Adam(ndim)
            obj.m = zeros([1 ndim]);
            obj.v = zeros([1 ndim]);
        end
        function [obj,param] = update(obj, param, grad)
            obj.m = obj.beta1*obj.m+(1-obj.beta1)*grad;
            obj.v = obj.beta2*obj.v+(1-obj.beta2)*(grad.^2);
            mbar = obj.m./(1-obj.beta1);
            vbar = obj.v./(1-obj.beta2);
            param = param - mbar.*obj.eta./(sqrt(vbar)+1e-5);
        end
    end
end

これを以下の中で使います。

GdOptimizer.m

classdef GdOptimizer

    properties
        slopedata;
        x0;
        delta_t;
        max_k;
        vref;

        k_s;
        u_s;
        x_s;
        lambda_s;
        Honu_s;
        Honu_integral;

        penalty_for_u;

        adam;
    end
    
    methods
        function obj = GdOptimizer(x0,delta_t,max_k,vref,slopedata)
            obj.x0 = x0;
            obj.delta_t = delta_t;
            obj.max_k = max_k;
            obj.k_s = 1:max_k;
            obj.vref = vref;
            obj.slopedata = slopedata;

            obj.penalty_for_u = 1e3;

            obj.adam = Adam(max_k);

            obj = obj.step1;
            obj = obj.step2;
        end

        function obj = step1(obj)
            % 適当な u を決める
            obj.u_s = zeros([1, obj.max_k]);
            
        end

        function obj = step2(obj)
            % dotx = f(x) より、 x を k=1:max_k について解く
            obj.x_s = car_steps(obj.x0,obj.u_s,obj.delta_t,obj.slopedata);
        end

        function obj = step3(obj)
            % lambda(k_final) = phi_on_x (x(k_final))
            % dotlambda = - H_on_x より、
            % lambda を k=max_k:1 について解く

            obj.lambda_s = zeros([obj.max_k 2]);

            v_final = obj.x_s(obj.max_k,2);
            lambda_final = [0 v_final-obj.vref];
            
            obj.lambda_s(obj.max_k,:) =lambda_final;

            for k = obj.max_k-1:-1:1
                lambda = obj.lambda_s(k+1,:);
                v = obj.x_s(k,2);
                dotlambda = - [0 v-obj.vref+lambda(1)];
                prevlambda = lambda - obj.delta_t*dotlambda;
                obj.lambda_s(k,:) = prevlambda;
            end
        end

        function obj = step4(obj)
            % H_on_u を求める
            obj.Honu_s = ...
                obj.lambda_s(:,2)'+ ...
                2*obj.penalty_for_u*obj.u_s.*(obj.u_s.^2>0.09);
            obj.Honu_integral = sum(obj.Honu_s.^2);
        end

        function obj = step5(obj)
            % u -= alpha * H_on_u
            [obj.adam, obj.u_s] = obj.adam.update(obj.u_s,obj.Honu_s);
        end

        function obj = epoch(obj)
            obj = obj.step3;
            obj = obj.step4;
            obj = obj.step5;
            obj = obj.step2;
        end

        function obj = epochs(obj,num_epochs)
            for i=1:num_epochs
                obj = obj.epoch;
            end
        end

    end
end

GdOptimizer#epochsによってエポックを何回か回し、$u(k)$を訓練していきます。今回は150回回しましたが、うちのMacBook Proちゃんは0.2秒くらいで終わりました。

gdo = GdOptimizer(x0,delta_t,max_k,vref);
gdo_s = [gdo];
for epoch = 1:150
    gdo = gdo.epoch;
    gdo_s = [gdo_s gdo];
end

fminunc

MATLABには凸関数の制約なし局所最適解を解くfminuncとかいう優秀な子がいるので、これを使うことにします。

この関数を使うためには、$u(k)$から 評価関数$J$を出す関数が必要なので、作っておきます。

get_J_from_u.m

function J = get_J_from_u(u_s,slopedata,delta_t,x0,vref)
    x_s = car_steps(x0,u_s,delta_t,slopedata);
    v_s = x_s(:,2)';
    L_s = 0.5*((v_s-vref).^2 + 1e3*max(0,u_s.^2-0.09));
    J = sum(L_s);
end

objective = @(u_s) get_J_from_u(u_s,slopedata,delta_t,x0,vref);

僕のコード作成の特徴として、関数名と変数名の付け方を明確に分けることと、やたらクラス化/関数化したくなるという傾向があります。昔、C++やJavaでゲームなどを作ったり、仕事でアプリを作ったりしていたので、その名残です。

最適化は以下のコードで完了です。勾配法を実装した苦労はなんだったんだこのやろうが！！

u_s = zeros([1, max_k]);
[fmiinunc_u_s,fval] = fminunc(objective, u_s);

両者の求解結果を見る

図. シミュレーション結果

これは、それぞれの方法で求まった入力に対して車の状態をシミュレートし、結果をプロットしたものです。勾配法(GD Optimize)もfminuncも、速度が 10 で一定になるように頑張ってることが分かります。

面白いのが、$z=230$くらいのところ。$z=250$から速度が減る坂 = 上り坂が始まる訳ですが、それを見越してもうすでにアクセルを踏んでいます。上り坂でアクセルが制約条件$|u|\le 3$を破らずに済むように、前もってちょっとだけ加速しておくのですね。計画性が素晴らしい✨

うちの勾配法、なんか入力がギザギザしています。それに対しfminuncは終始穏やかなアクセル/ブレーキ使いをしています。最適化法にも個性みたいなものがあるんでしょうかね。

%シミュレーション
gdo_final_u_s = gdo_s(length(gdo_s)).u_s;
gdo_final_x_s = car_steps(x0,gdo_final_u_s,delta_t,slopedata);
fminunc_x_s = car_steps(x0,fmiinunc_u_s,delta_t,slopedata);
free_u_s = zeros(size(t_s));
free_x_s = car_steps(x0,free_u_s,delta_t,slopedata);

%結果の表示
figure
xlim_setting = [0 500];

subplot(3,1,1); hold on; title("Slope")
plot(z_s,s_s)
xlabel("z [m]"); ylabel("s [m/s^2]")
xlim(xlim_setting)
hold off

subplot(3,1,2); hold on; title("v")
plot(gdo_final_x_s(:,1),gdo_final_x_s(:,2))
plot(fminunc_x_s(:,1),fminunc_x_s(:,2))
plot(free_x_s(:,1),free_x_s(:,2))
xlabel("z [m]"); ylabel("v [m/s]")
yline(vref,":")
legend("GD Optimize","fminunc","Free","vref")
xlim(xlim_setting); ylim([0 20])
hold off

subplot(3,1,3); hold on; title("u")
plot(gdo_final_x_s(:,1),gdo_final_u_s)
plot(fminunc_x_s(:,1),fmiinunc_u_s)
plot(free_x_s(:,1),free_u_s)
xlabel("z [m]"); ylabel("v [m/s]")
legend("GD Optimize","fminunc","Free")
xlim(xlim_setting)
hold off

制御の質を定量的に考察する

次は制御がどのくらい目標を満たしているか見てみます。これは、先ほど作った評価関数 J を計算する関数にやってもらいます。

objective(gdo_final_u_s)
objective(fminunc_u_s)
objective(free_u_s)

ドキドキ….

図. それぞれの制御での評価関数

おおおぉおおおおおうちが作った勾配法が一番小さい！！！fminuncに勝った！!MATLABに勝ったぞおお！！
というのは偶然かもしれません。というのも、Adam Optimizerのハイパーパラメータをちょっといじったら評価関数がfminuncに負けました。。。

まとめ

最適制御を2つの方法で解いてみたところ、両者ともいい感じの制御ができていることが分かりました。Adam Optimizerがこんなところで役に立つというのも驚きでした。今後の課題ややってみたいこととしては、

• 坂を色々変えてやってみる。
• fminconを使ってみる。
• 今回、車の状態$x$を横ベクトルにして処理してしまいましたが、制御工学的にはここは縦ベクトルにするのが一般的です。そうしないと、今回みたいに行と列の関係が全部逆転してしまう。あちゃー
• 車のシミュレーションや随伴関数$\lambda$を求めるのに、今回はオイラー法を使いましたが、ルンゲ・クッタ法や2次のオイラー法を使ってもやってみたいですね。
• 車のシミュレーターやステージコスト$L$などもクラス化して、勾配とかの算出処理にもOOPをバンバン導入したいですね。

参考

• 大塚(2018). 非線形最適制御入門. コロナ社

パキパキ坂にした理由

課題設定にて定義した坂による加速度$s(z)$は、その$z$微分を常に0としていました。これにはちょっと理由があります。

ハミルトン関数を展開してみると、

H(x,u,λ,t) = L(x,u,t) + λ^T f(x,u,t) = L(x,u,t) + λ_1 * v + λ_2 * (u(t) + s(z))

となりますが、勾配法ではハミルトン関数の$x=[z, v]$微分をそれぞれ行う必要があります。$z$微分は

\frac{\partial H}{\partial z}
=\frac{\partial H}{\partial s}\frac{\partial s}{\partial z}
=\lambda_2\frac{\partial s}{\partial z}

ってなって、坂に$z$微分が入ってると数値が発生しちゃって面倒臭いというか、今のSlopedata2で実装するのが困難だったんで、もうパキパキでいいやって思った訳です。