控制障碍函数(Control Barrier Function,CBF) 三、代码

news2024/11/20 8:25:39

三、代码实现

3.1、模型

这是一个QP问题，所以我们直接建模

请添加图片描述

这其实还是之前的那张图，我们把这个大的框架带入到之前的那个小车追击的问题中去，得到以下的一些具体的约束条件

CLF约束

$L_g V(x) u - \delta \le - L_fV(x) - \lambda V(x)$

CBF约束

$-L_g B(x) u \le L_fB(x) + \gamma B(x)$

输入约束

$\begin{aligned} u &\le u_{max} \\ -u &\le - u_{min} \end{aligned}$

我们全部写成了这种小于等于的形式，便于下面的二次规划

3.2、实现

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;                                                              % 与系统相关参数
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;                                                    % 松弛变量系数 p

% 状态初始化
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;                                                 % 李雅普诺夫函数
    dV = [0, 2 .* (v - sys.vd), 0];                                        % 李雅普诺夫函数的导
    LfV = dV * f;                                                          % 李导数
    LgV = dV * g;
    B = z - sys.T .* v - 0.5 .* (v - sys.v0) .^ 2 ./ (sys.cd .* sys.g);    % 障碍函数
    dB = [0, - sys.T - (v - sys.v0) ./ sys.cd ./ sys.g, 0];                % 障碍函数的导
    LfB = dB * f;                                                          % 李导数
    LgB = dB * g;
    

    % 解控制量u
    A_ = [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); % 第二项是松弛变量，松弛变量这里也是一个待优化的值

    dx = f + g .* u;
    x_n = x + dx .* dt;

    % 保存数据
    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')