MPC模型预测控制(四)-MATLAB跟踪圆

2024-04-10 14:58

本文主要是介绍MPC模型预测控制(四)-MATLAB跟踪圆,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

参考https://github.com/Janani-Mohan

%% YALMIP : Circular Trajectory Tracking using MPC 
clc;
clear;
close all;
yalmip('clear')%% MPC Parameters definition
% Model Parameters
params.Ts = 0.01;   % Sampling time 
params.nstates = 3; % No of States in the system [x, y,theta]
params.ninputs = 1; % No of Inputs in the system [delta_f], steering angle
params.l_f = 0.17; % Distance from the CG to the front wheel
params.l_r = 0.16; % Distance from the CG to the rear wheel
params.l_q = params.l_r / (params.l_r + params.l_f);% Control Parameters
params.Np = 10;   % The horizon 30
params.Q = [1 0 0; 0 1 0; 0 0 1]; % The Q matrix for running cost for error in trajectory tracking
params.Q_N = [1 0 0; 0 1 0; 0 0 1];% The Q matrix for terminal cost for error in trajectory tracking
params.R = 1; %  The R matrix for running cost for input% Initial conditions for the state of the vehicle       
params.x0 = 1.5;  %1.5
params.y0 = 0;  
params.v0 = 1; 
params.psi0 = pi/2; % Number of iterations
params.N = 500;%1000% Every point on the circle is a reference to be followed by the vehicle
% x0 and y0 are the coordinates of the circle's center,r is its radius.
trajectory_params.x0 = 0;
trajectory_params.y0 = 0;
trajectory_params.r = 1.5;%% Simulation environment
% Initialization
z(1,:)  = [params.x0, params.y0, params.psi0]; % State representation
u(1) = 0; % Control Input% Generate the trajectory to be followed
traj = trajectory_circular(trajectory_params);% Obtain the symbolic form of the matrices A, B
syms xkn ykn psikn xk yk psik Ts lr lf delta
[symbolic_linear_A, symbolic_linear_B] = Symbolic_Kinematic_Model(params);k = 0;
while (k < params.N)k = k+1;  
%   fprintf("Iteration number = %d ",k);% The distances of all the points in the trajectory to the vehicled = sqrt((traj(:,1)-z(k,1)).^2 + (traj(:,2)-z(k,2)).^2);% The index of the point that is the closest to the vehicle[~, idx] = min(d);% The first reference point for x, y, thetaz_ref(k,:) = traj(idx, :);   refs = zeros(params.Np+1, 3);for i = 1:params.Np+1theta = z_ref(k,3) +(params.v0 / trajectory_params.r) * params.Ts * i;x = trajectory_params.x0 + trajectory_params.r * cos(theta - pi/2);y = trajectory_params.y0 + trajectory_params.r * sin(theta - pi/2);refs(i,:) = [x, y, theta];end
%   plot(refs(:,1),refs(:,2));
% The first case
if (k == 1)params.A = subs(symbolic_linear_A, [Ts lr lf psik delta], [params.Ts params.l_r params.l_f z(k,3) 0]);params.B = subs(symbolic_linear_B, [Ts lr lf psik delta], [params.Ts params.l_r params.l_f z(k,3) 0]);params.A = double(params.A);params.B = double(params.B);
end
% The remaining cases
if (k > 1)% Completing the circleif (z_ref(k,3) < z_ref(k-1,3))    refs(:,3) = refs(:,3) + 2*pi;z_ref(k,3) = z_ref(k,3) + 2*pi;traj(idx,3) = traj(idx,3) + 2*pi;endparams.A = subs(symbolic_linear_A, [Ts lr lf psik delta], [params.Ts params.l_r params.l_f z(k,3) u(k-1)]);params.B = subs(symbolic_linear_B, [Ts lr lf psik delta], [params.Ts params.l_r params.l_f z(k,3) u(k-1)]);params.A = double(params.A);params.B = double(params.B);
end% Ensure stability : Ricatti Equation Solver is used to ensure stability 
% finding stable Q matrix[params.Qf,~,~,err] = dare(params.A, params.B, params.Q, params.R);if (err == -1 || err == -2)params.Qf = params.Q;endyalmip('clear')% The predicted state and control variablesz_mpc = sdpvar(params.Np+1, params.nstates);u_mpc = sdpvar(params.Np, params.ninputs);% Reset constraints and objective functionconstraints = [];J = 0;for i = 1:params.Npif i < params.NpJ = J + (z_mpc(i,:)-refs(i,:)) * params.Qf * (z_mpc(i,:)-refs(i,:))' +  ...u_mpc(i,:) * params.R * u_mpc(i,:)';% Model constraintsconstraints = [constraints, ...z_mpc(i+1,:)' == params.A * z_mpc(i,:)' + params.B * u_mpc(i,:)'];% Input constraintsconstraints = [constraints, ...-pi/4 <= u_mpc(i) <= pi/4];elseJ = J + ...(z_mpc(i,:)-refs(i,:)) * params.Q_N * (z_mpc(i,:)-refs(i,:))' +  ...u_mpc(i,:) * params.R * u_mpc(i,:)';% Model constraintsconstraints = [constraints, ...z_mpc(i+1,:)' == params.A * z_mpc(i,:)' + params.B * u_mpc(i,:)'];% Input constraintsconstraints = [constraints, ...-pi/4 <= u_mpc(i) <= pi/4];endend%  terminal costJ = J + (z_mpc(params.Np+1, :) - refs(params.Np+1,:)) * params.Qf * (z_mpc(params.Np+1, :)-refs(params.Np+1,:))';% terminal constraintsconstraints = [constraints, ...z_mpc(params.Np+1, 3) - refs(params.Np+1, 3) == 0];assign(z_mpc(1,:), z(k,:));% Optionsops = sdpsettings('solver', 'quadprog');% Optimizeoptimize([constraints, z_mpc(1,:) == z(k,:)], J, ops);% Take the first predicted input u(k) = value(u_mpc(1));% simulate the vehiclez(k+1,:) = vehicle_model(z(k,:), u(k), params);plot(traj(:,1), traj(:,2))hold onplot(z(2:end,1), z(2:end,2));plot(refs(:,1), refs(:,2), '*', 'Color','b')plot(z(k,1), z(k,2), '*', 'Color', 'r')axis equalaxis([-2 2 -2 2])drawnowtitle('Trajectory Tracking')hold offend
% Plot state deviations and inputs
figure
subplot(2,2,1)
plot(traj(:,1), traj(:,2),'-*r')
hold on
plot(z(:,1), z(:,2))
title('Trajectory Tracking')
legend ('Given Trajectory','My Trajectory','location','southeast');
axis equal
hold offsubplot(2,2,2)
plot((z(1:end-1, 1) - z_ref(:,1)).^2 + (z(1:end-1, 2) - z_ref(:,2)).^2)
xlabel('No of iterations');
ylabel('Deviation from the reference trajectory');
title('Distance deviation')subplot(2,2,3)
plot((z_ref(:,3) - z(1:end-1,3))*180/pi)
xlabel('No of iterations');
ylabel('Deviation from the reference trajectory');
title('Orientation deviation')figure
plot(u * 180 / pi)
xlabel('No of iterations');
ylabel('Variation in steering angle');
title('Input Steering Angle')
% 
% %% Trajectory Generator
% 
% %Creates the Trajectory to Follow
% function traj = trajectory_circular(trajectory_params)
% % Center of the circular trajectory 
%   x0 = trajectory_params.x0;
%   y0 = trajectory_params.y0;
%   r = trajectory_params.r;
% 
% % Creates the circular trajectory and corresponding angle
%   for i = 0:1:360
%       x = x0 + r*cos(pi * i / 180);
%       y = y0 + r*sin(pi * i / 180);
%       theta = pi - atan2(x - x0, y - y0);
% 
%       traj(i+1, 1) = x;
%       traj(i+1, 2) = y;
%       traj(i+1, 3) = theta;
%   end
%   
% end
% 
% %% Kinematic Bicycle Model
% 
% function [A, B] = Symbolic_Kinematic_Model(params)
% 
%   syms xkn ykn psikn xk yk psik Ts lr lf delta
%   
%   xkn = xk + Ts*params.v0*cos(psik + atan((lr/(lr+lf))*tan(delta)));
%   ykn = yk + Ts*params.v0*sin(psik + atan((lr/(lr+lf))*tan(delta)));
%   psikn = psik + Ts*(params.v0/lr)*sin(atan((lr/(lr+lf))*tan(delta)));
% 
%   A(1,1) = diff(xkn, xk);
%   A(1,2) = diff(xkn, yk);
%   A(1,3) = diff(xkn, psik);
% 
%   A(2,1) = diff(ykn, xk);
%   A(2,2) = diff(ykn, yk);
%   A(2,3) = diff(ykn, psik);
% 
%   A(3,1) = diff(psikn, xk);
%   A(3,2) = diff(psikn, yk);
%   A(3,3) = diff(psikn, psik);
% 
%   B(1,1) = diff(xkn, delta);
%   B(2,1) = diff(ykn, delta);
%   B(3,1) = diff(psikn, delta);
% 
% end
% 
% %% Vehicle Model
% function z_new = vehicle_model(z_curr, u_curr, params)
%     
%   delta = u_curr;
%   
%   % vehicle dynamics equations
%   z_new(1) = z_curr(1) + (params.v0*cos(z_curr(3)+atan((params.l_r/(params.l_r+params.l_f))*tan(delta))))*params.Ts;
%   z_new(2) = z_curr(2) + (params.v0*sin(z_curr(3)+atan((params.l_r/(params.l_r+params.l_f))*tan(delta))))*params.Ts;
%   z_new(3) = z_curr(3) + (params.v0 / params.l_r * sin(atan((params.l_r/(params.l_r+params.l_f))*tan(delta))))*params.Ts;
% 
% end

注释掉的有三个函数:轨迹函数,运动自行车模型函数,实际的运动结果函数。需要放到同文件夹里生成.m的函数。这里没有一一复制。这个需要安装yawmip,免费的。参考链接:https://blog.csdn.net/XIXI_0921/article/details/47981869

希望有志之士能把这个函数改为跟踪8字形。我试了下没成功,用NMPC的程序倒是能跟踪上。大家有问题就交流。对程序里有不理解的也可以评论问我,我会做相应解答。

这篇关于MPC模型预测控制(四)-MATLAB跟踪圆的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/891387

相关文章

Spring Security 基于表达式的权限控制

前言 spring security 3.0已经可以使用spring el表达式来控制授权,允许在表达式中使用复杂的布尔逻辑来控制访问的权限。 常见的表达式 Spring Security可用表达式对象的基类是SecurityExpressionRoot。 表达式描述hasRole([role])用户拥有制定的角色时返回true (Spring security默认会带有ROLE_前缀),去

大模型研发全揭秘:客服工单数据标注的完整攻略

在人工智能(AI)领域,数据标注是模型训练过程中至关重要的一步。无论你是新手还是有经验的从业者,掌握数据标注的技术细节和常见问题的解决方案都能为你的AI项目增添不少价值。在电信运营商的客服系统中,工单数据是客户问题和解决方案的重要记录。通过对这些工单数据进行有效标注,不仅能够帮助提升客服自动化系统的智能化水平,还能优化客户服务流程,提高客户满意度。本文将详细介绍如何在电信运营商客服工单的背景下进行

Andrej Karpathy最新采访:认知核心模型10亿参数就够了,AI会打破教育不公的僵局

夕小瑶科技说 原创  作者 | 海野 AI圈子的红人,AI大神Andrej Karpathy,曾是OpenAI联合创始人之一,特斯拉AI总监。上一次的动态是官宣创办一家名为 Eureka Labs 的人工智能+教育公司 ,宣布将长期致力于AI原生教育。 近日,Andrej Karpathy接受了No Priors(投资博客)的采访,与硅谷知名投资人 Sara Guo 和 Elad G

Retrieval-based-Voice-Conversion-WebUI模型构建指南

一、模型介绍 Retrieval-based-Voice-Conversion-WebUI(简称 RVC)模型是一个基于 VITS(Variational Inference with adversarial learning for end-to-end Text-to-Speech)的简单易用的语音转换框架。 具有以下特点 简单易用:RVC 模型通过简单易用的网页界面,使得用户无需深入了

透彻!驯服大型语言模型(LLMs)的五种方法,及具体方法选择思路

引言 随着时间的发展,大型语言模型不再停留在演示阶段而是逐步面向生产系统的应用,随着人们期望的不断增加,目标也发生了巨大的变化。在短短的几个月的时间里,人们对大模型的认识已经从对其zero-shot能力感到惊讶,转变为考虑改进模型质量、提高模型可用性。 「大语言模型(LLMs)其实就是利用高容量的模型架构(例如Transformer)对海量的、多种多样的数据分布进行建模得到,它包含了大量的先验

图神经网络模型介绍(1)

我们将图神经网络分为基于谱域的模型和基于空域的模型,并按照发展顺序详解每个类别中的重要模型。 1.1基于谱域的图神经网络         谱域上的图卷积在图学习迈向深度学习的发展历程中起到了关键的作用。本节主要介绍三个具有代表性的谱域图神经网络:谱图卷积网络、切比雪夫网络和图卷积网络。 (1)谱图卷积网络 卷积定理:函数卷积的傅里叶变换是函数傅里叶变换的乘积,即F{f*g}

秋招最新大模型算法面试,熬夜都要肝完它

💥大家在面试大模型LLM这个板块的时候,不知道面试完会不会复盘、总结,做笔记的习惯,这份大模型算法岗面试八股笔记也帮助不少人拿到过offer ✨对于面试大模型算法工程师会有一定的帮助,都附有完整答案,熬夜也要看完,祝大家一臂之力 这份《大模型算法工程师面试题》已经上传CSDN,还有完整版的大模型 AI 学习资料,朋友们如果需要可以微信扫描下方CSDN官方认证二维码免费领取【保证100%免费

【生成模型系列(初级)】嵌入(Embedding)方程——自然语言处理的数学灵魂【通俗理解】

【通俗理解】嵌入(Embedding)方程——自然语言处理的数学灵魂 关键词提炼 #嵌入方程 #自然语言处理 #词向量 #机器学习 #神经网络 #向量空间模型 #Siri #Google翻译 #AlexNet 第一节:嵌入方程的类比与核心概念【尽可能通俗】 嵌入方程可以被看作是自然语言处理中的“翻译机”,它将文本中的单词或短语转换成计算机能够理解的数学形式,即向量。 正如翻译机将一种语言

AI Toolkit + H100 GPU,一小时内微调最新热门文生图模型 FLUX

上个月,FLUX 席卷了互联网,这并非没有原因。他们声称优于 DALLE 3、Ideogram 和 Stable Diffusion 3 等模型,而这一点已被证明是有依据的。随着越来越多的流行图像生成工具(如 Stable Diffusion Web UI Forge 和 ComyUI)开始支持这些模型,FLUX 在 Stable Diffusion 领域的扩展将会持续下去。 自 FLU

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型,它综合考虑了土壤-水分-大气以及植被间的相互作用;是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程,使其能够精确的模拟土壤中水分的运动,而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者