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

三、代码实现

3.1、模型

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

请添加图片描述

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

  • CLF约束

L g V ( x ) u − δ ≤ − L f V ( x ) − λ V ( x ) L_g V(x) u - \delta \le - L_fV(x) - \lambda V(x) LgV(x)uδLfV(x)λV(x)

  • CBF约束

− L g B ( x ) u ≤ L f B ( x ) + γ B ( x ) -L_g B(x) u \le L_fB(x) + \gamma B(x) LgB(x)uLfB(x)+γB(x)

  • 输入约束

u ≤ u m a x − u ≤ − u m i n \begin{aligned} u &\le u_{max} \\ -u &\le - u_{min} \end{aligned} uuumaxumin

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

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

这里我们的二次规划求解器用到了Matlab中的函数quadprog,其文档地址为 https://ww2.mathworks.cn/help/optim/ug/quadprog.html

本文的结果为

请添加图片描述

相较于作者给出的代码,本文的代码更加简单,适合初学者使用

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/610369.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

LeetCode刷题记录:(4)重复的子字符串

leetcode传送通道 /** 重复字符串的next数组是有规律的 abcabc:-1 0 0 0 1 2 abcabcabc:-1 0 0 0 1 2 3 4 5 abcabcabcabc:-1 0 0 0 1 2 3 4 5 6 7 8 abcabcabcabg:-1 0 0 0 1 2 3 4 5 6 7 8 欸嘿,和上面一样&#xff…

Linux——firewalld防火墙(一)

一、Linux防火墙基础 Linux 的防火墙体系主要工作在网络层.针对TCP/P数据包实时过滤和限制.属于典型的包过滤防火墙(或称为网络层防火墙)。Linux系统的防火墙体系基于内核编码实现.具有非常稳定的性能和高效率,也因此获得广泛的应用.在CentOS 7系统中几种…

FlinkAPI开发之自定义函数UDF

案例用到的测试数据请参考文章: Flink自定义Source模拟数据流 原文链接:https://blog.csdn.net/m0_52606060/article/details/135436048 概述 用户自定义函数(user-defined function,UDF),即用户可以根据…

基于SpringBoot的毕业生实习与就业管理系统(系统+数据库+文档)

🍅点赞收藏关注 → 私信领取本源代码、数据库🍅 本人在Java毕业设计领域有多年的经验,陆续会更新更多优质的Java实战项目 希望你能有所收获,少走一些弯路。🍅关注我不迷路🍅一、绪论 1. 研究背景 现在大家…

私域流量转化差,这些问题你都解决了吗?

一、流量不精准 这是一个常见而又经常被忽视的问题。许多企业在私域运营中面临转化率低下的问题,但有没有想过,这可能只是因为你吸引的流量与你的产品不匹配? 从公域引流到私域,数量并非唯一关键,精准度更是重中之重…

【计算机组成原理】计算机系统概论 常用计算机术语英文缩写汇总

集成电路 小规模集成电路 (SSIC):Small Scale Integrated Circuits中规模集成电路 (MSIC):Medium Scale Integrated Circuits大规模集成电路 (LSIC):Large Scale Integr…

【Vue3+Ts项目】硅谷甄选 — 品牌管理+平台属性管理+SPU管理+SKU管理

一、品牌管理模块 1.1 静态模块搭建 使用到element-plus的card、button、table、pagination等组件&#xff1a;src/views/product/trademark/index.vue <template><el-card><!-- 卡片顶部添加品牌按钮 --><el-button type"primary" size&quo…

第十九章 调用Callout Library函数 - 将 $ZF(-5) 与多个库和许多函数调用一起使用

文章目录 第十九章 调用Callout Library函数 - 将 $ZF(-5) 与多个库和许多函数调用一起使用将 $ZF(-5) 与多个库和许多函数调用一起使用 第十九章 调用Callout Library函数 - 将 $ZF(-5) 与多个库和许多函数调用一起使用 将 $ZF(-5) 与多个库和许多函数调用一起使用 Method G…

数字IC后端实现之Innovus TA-152错误解析(分频generated clock定义错误)

**ERROR: (TA-152): A latency path from the ‘Fall’ edge of the master clock at source pin… Error Code TA-152 在数字IC后端实现innovus中我们经常会看到这类Error&#xff0c;具体信息如下所示。 Error Message **ERROR: (TA-152): A latency path from the ‘Fa…

Goby高级食用指南

Goby高级食用指南 1.Goby POC2.自定义字典3.Goby插件生态 - 一些好用的插件分享FOFASubDomainsBruteExportCsvAWVSRedis-cliGoby4waf初级篇参考 - Goby基本使用 1.Goby POC Goby的漏洞模块包含官方自定义的一些初始POC: 红队版的POC会实时更新,普通版则不会 Goby的POC编写…

沈阳数字孪生赋能工业智能制造,助力制造业企业数字化转型

沈阳数字孪生赋能工业智能制造&#xff0c;助力制造业企业数字化转型。在数字经济时代&#xff0c;数字孪生作为实现各行各业智能化、数字化的重要手段之一&#xff0c;受到了各方的广泛重视。随着各项关键使能技术的不断发展&#xff0c;数字孪生的应用价值有望得到进一步释放…

GitLab clone 地址不对的解决办法

1丶问题描述 2丶解决方案 解决方案&#xff1a; 找到挂载到宿主机配置文件&#xff1a;gitlab.rb vi gitlab.rb 改成自己的ip 重启容器 docker restart gitlab 如果发现容器一直重启&#xff0c;可采用粗暴的方法&#xff0c;直接干掉当前容器&#xff0c;重新运行一个 …

一键与图片对话!LLM实现图片关键信息提取与交互

本期文心开发者说邀请到飞桨开发者技术专家徐嘉祁&#xff0c;主要介绍了如何通过小模型与大模型的结合&#xff0c;解决数据分析中的问题。 项目背景 在智能涌现的大模型时代&#xff0c;越来越多的企业和研究机构开始探索如何利用大模型来提升工作效率&#xff0c;助力业务智…

企业数字化转型指南,12步实现企业转型之路

引言 在这个数字化时代&#xff0c;企业面临着前所未有的机遇和挑战。随着科技的飞速发展和市场竞争的加剧&#xff0c;传统商业模式正在经历翻天覆地的变革。数字化转型&#xff0c;已经不再只是一种选择&#xff0c;而是企业生存和发展的必然路径。它不仅仅是技术的升级&…

Open CASCADE学习|基于visual studio 2022编译源码

目录 1、简介 2、下载 2.1下载visual studio 2022 community 2.2下载下载cmake工具 2.3下载源码 2.4下载第三方插件 3、安装 3.1安装visual studio 2022 community 3.2安装cmake 4、编译源码 5、测试 1、简介 Open CASCADE&#xff08;简称…

C //练习 4-8 假定最多只压回一个字符。请相应地修改getch与ungetch这两个函数。

C程序设计语言 &#xff08;第二版&#xff09; 练习 4-8 练习 4-8 假定最多只压回一个字符。请相应地修改getch与ungetch这两个函数。 注意&#xff1a;代码在win32控制台运行&#xff0c;在不同的IDE环境下&#xff0c;有部分可能需要变更。 IDE工具&#xff1a;Visual St…

【OCR】 - Tesseract OCR在Windows系统中安装

Tesseract OCR 在Windows环境下安装Tesseract OCR&#xff08;Optical Character Recognition&#xff09;通常包括以下几个步骤&#xff1a; 下载Tesseract 访问Tesseract的GitHub发布页面&#xff1a;https://github.com/tesseract-ocr/tesseract/releases找到适合你操作系…

羊奶制作工艺揭秘,如何打造丰富多样的口味品种?

羊奶制作工艺揭秘&#xff0c;如何打造丰富多样的口味品种&#xff1f; 羊奶一直以来都是人们健康饮食的选择之一&#xff0c;它不仅营养丰富&#xff0c;而且口感独特。但是&#xff0c;你是否好奇羊奶是如何做到各种口味的呢&#xff1f;下面就跟随小编羊大师一起揭秘羊奶制…

SpringBoot集成Minio

pom文件导入依赖 <?xml version"1.0" encoding"UTF-8"?> <project xmlns"http://maven.apache.org/POM/4.0.0" xmlns:xsi"http://www.w3.org/2001/XMLSchema-instance"xsi:schemaLocation"http://maven.apache.org/P…

Hadoop之mapreduce参数大全-3

51.指定Shuffle传输过程中可以同时连接的节点数 mapreduce.shuffle.max.connections是Hadoop MapReduce中的一个配置参数&#xff0c;用于指定Shuffle传输过程中可以同时连接的节点数。该参数用于控制Shuffle传输的并发度&#xff0c;以保障任务的稳定性和性能。 可以通过以下…