三、代碼實(shí)現(xiàn)

3.1、模型

這是一個(gè)QP問題，所以我們直接建模

這其實(shí)還是之前的那張圖，我們把這個(gè)大的框架帶入到之前的那個(gè)小車追擊的問題中去，得到以下的一些具體的約束條件

• CLF約束

$$L_{g}V(x)u?\delta \le ?L_{f}V(x)?\lambda V(x)$$

• CBF約束

$$?L_{g}B(x)u\le L_{f}B(x)+\gamma B(x)$$

• 輸入約束

$$\begin{array}{rl}u& \le u_{max}\\ ?u& \le ?u_{min}\end{array}$$

我們?nèi)繉懗闪诉@種小于等于的形式，便于下面的二次規(guī)劃

3.2、實(shí)現(xiàn)

clc; clear; close all;dt = 0.02;                                                                 
T = 30;
length = ceil(T ./ dt);sys.p = zeros(length,1);                                                   % 位置
sys.v = zeros(length,1);                                                   % 速度
sys.z = zeros(length,1);                                                   % 與前車距離
sys.u = zeros(length,1);                                                   % 控制量
sys.m = 1650;                                                              % 與系統(tǒng)相關(guān)參數(shù)
sys.g = 9.81;
sys.v0 = 14;
sys.vd = 24;
sys.f0 = 0.1;
sys.f1 = 5;
sys.f2 = 0.25;
sys.ca = 0.3;
sys.cd = 0.3;
sys.T = 1.8;
sys.u_max = sys.ca .* sys.m .* sys.g;                                      % 控制量最大值
sys.u_min = - sys.cd .* sys.m .* sys.g;                                    % 控制量最小值
sys.clf.rate = 5;                                                          % lambda
sys.cbf.rate = 5;                                                          % gamma
sys.wight.input = 2 ./ sys.m .^ 2;                                         % 二次型矩陣H
sys.wight.slack = 2e-2;                                                    % 松弛變量系數(shù) p% 狀態(tài)初始化
sys.p(1,1) = 0;
sys.v(1,1) = 10;
sys.z(1,1) = 100;for i = 1:(length)t = i .* dt;p = sys.p(i,1);v = sys.v(i,1);z = sys.z(i,1);x = [p; v; z];F_r = sys.f0 + sys.f1.*v + sys.f2 .* v .* v;f = [v; - F_r ./ sys.m; sys.v0 - v];g = [0; 1./sys.m; 0];V = (v - sys.vd) .^ 2;                                                 % 李雅普諾夫函數(shù)dV = [0, 2 .* (v - sys.vd), 0];                                        % 李雅普諾夫函數(shù)的導(dǎo)LfV = dV * f;                                                          % 李導(dǎo)數(shù)LgV = dV * g;B = z - sys.T .* v - 0.5 .* (v - sys.v0) .^ 2 ./ (sys.cd .* sys.g);    % 障礙函數(shù)dB = [0, - sys.T - (v - sys.v0) ./ sys.cd ./ sys.g, 0];                % 障礙函數(shù)的導(dǎo)LfB = dB * f;                                                          % 李導(dǎo)數(shù)LgB = dB * g;% 解控制量uA_ = [LgV, -1; -LgB, 0;1,0;-1,0];b_ = [-LfV - sys.clf.rate .* V; LfB + sys.cbf.rate .* B;sys.u_max;-sys.u_min;];H_ = [sys.wight.input, 0;0,sys.wight.slack];f_ = [- sys.wight.input * F_r; 0];u = quadprog(H_,f_,A_,b_);u = u(1); % 第二項(xiàng)是松弛變量，松弛變量這里也是一個(gè)待優(yōu)化的值dx = f + g .* u;x_n = x + dx .* dt;% 保存數(shù)據(jù)sys.u(i,1) = u;sys.p(i+1,1) = x_n(1);sys.v(i+1,1) = x_n(2);sys.z(i+1,1) = x_n(3);
end% 繪圖
figure(1);
subplot(4,1,1);
plot(dt:dt:T,sys.p(1:length,:));
ylabel('p')subplot(4,1,2);
plot(dt:dt:T,sys.v(1:length,:));
ylabel('v')subplot(4,1,3);
plot(dt:dt:T,sys.z(1:length,:));
ylabel('z')subplot(4,1,4);
plot(dt:dt:T,sys.u);
ylabel('u')

這里我們的二次規(guī)劃求解器用到了Matlab中的函數(shù)quadprog，其文檔地址為 https://ww2.mathworks.cn/help/optim/ug/quadprog.html

本文的結(jié)果為

相較于作者給出的代碼，本文的代碼更加簡單，適合初學(xué)者使用