MPC模型预测控制（二）-MATLAB代码实现_function [reftraj_x,reftraj_y,reftraj_theta,reftra

于 2024-05-16 13:44:39 发布
阅读量230 收藏 6
点赞数 3
文章标签： c语言 c++ 学习
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/2401_84975883/article/details/138958457
版权

img
img

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

%首先生成参考轨迹，画出图来作参考%
[RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(x_real(1,1),x_real(1,2),CEN(1),CEN(2),Radius,250,vel,T,L);

figure(1) %绘制参考路径
plot(RefTraj_x,RefTraj_y,‘k’)
xlabel(‘x’,‘fontsize’,14)
ylabel(‘y’,‘fontsize’,14)
title(‘Plot of x vs y - Ref. Trajectory’);
legend(‘reference traj’);
axis equal
grid on
hold on

for i=1:1:N_l

G_Test = 3;
%先确定参考点和确定矩阵A,B.这里姑且认为A和B是不变的
[RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(x_real(1,i),x_real(2,i),CEN(1),CEN(2),Radius,Hp,vel,T,L);
u_feedForward = RefTraj_delta(G_Test);%前馈控制量

% u_feedForward=0;
RefTraj_x(G_Test)
RefTraj_y(G_Test)
RefTraj_theta(G_Test)
Delta_x(1,1) = x_real(1,i) - RefTraj_x(G_Test);
Delta_x(2,1) = x_real(2,i) - RefTraj_y(G_Test);
Delta_x(3,1) = x_real(3,i) - RefTraj_theta(G_Test);
if Delta_x(3,1) > pi
Delta_x(3,1) = Delta_x(3,1)-2pi;
else if Delta_x(3,1) < -1pi
Delta_x(3,1) = Delta_x(3,1) +2*pi;
else
Delta_x(3,1) = Delta_x(3,1);
end
end

 % 通过Backward recursion 求K    
for  j=Hp:-1:2   
    Pk_1 = Pk;
    Vk_1 = Vk;     
    A=[1    0   -vel*sin(RefTraj_theta(j-1))*T; 0    1   vel*cos(RefTraj_theta(j-1))*T; 0    0   1;];

% B=[cos(RefTraj_theta(j-1))T 0; sin(RefTraj_theta(j-1))T 0; 0 velT/L;];
COS2 = cos(RefTraj_delta(j-1))^2;
B=[ 0 0 velT/(L*COS2)]';

    K = (B'*Pk_1*A)/(B'*Pk_1*B+R);
    Ku = R/(B'*Pk_1*B+R);
    Kv = B'/(B'*Pk_1*B+R);

    Pk=A'*Pk_1*(A-B*K)+Q;   
    Vk=(A-B*K)'*Vk_1 - K'*R*RefTraj_delta(j-1); 
end

 u_feedBackward = -K*(Delta_x)-Ku*u_feedForward-Kv*Vk_1;  

FWA(i+1,1)=u_feedForward+u_feedBackward;

 [x_real(1,i+1),x_real(2,i+1),x_real(3,i+1)]=Func_VehicleKineticModule_Euler(x_real(1,i),x_real(2,i),x_real(3,i),vel,FWA(i,1),FWA(i+1,1),T,L);

end

%% 绘图
% figure(1);
% plot(RefTraj_x,RefTraj_y,‘b’)
% hold on;
plot(x_real(1,:),x_real(2,:),‘r*’);
title(‘跟踪结果对比’);
xlabel(‘横向位置X’);
% axis([-1 5 -1 3]);
ylabel(‘纵向位置Y’);

end


还有4个子函数

function K=Func_Alpha_Pos(Xb,Yb,Xn,Yn)
AngleY=Yn-Yb;
AngleX=Xn-Xb;
%求Angle****%
if XbXn
if Yn>Yb
K=pi/2;
else
K=3*pi/2;
end
else
if YbYn
if Xn>Xb
K=0;
else
K=pi;
end
else
K=atan(AngleY/AngleX);
end
end
%修正K,使之在0~360°之间*%
if (AngleY>0&&AngleX>0)%第一象限
K=K;
elseif (AngleY>0&&AngleX<0)||(AngleY<0&&AngleX<0)%第二、三象限
K=K+pi;
else if (AngleY<0&&AngleX>0)%第四象限
K=K+2*pi;
else
K=K;
end
end
end

function Theta=Func_Theta_Pos(Alpha)

if Alpha >= 3pi/2
Theta = Alpha-3pi/2;
else
Theta = Alpha+pi/2;
end

end

function [RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(Pos_x,Pos_y,CEN_x,CEN_y,Radius,N,Velo,Ts,L)
%RefTraj为要生成的参考路径
%Pos_x,Pos_y为车辆坐标
%CEN_x,CEN_y,Radius圆心与半径
%N要生成几个参考点，即预测空间。
%Velo,Ts车速与采样时间
%L汽车的轴距
RefTraj=zeros(N,4);%生成的参考路径
Alpha_init=Func_Alpha_Pos(CEN_x,CEN_y,Pos_x,Pos_y);%首先根据车辆位置和圆心确定alpha

Omega=Velo/Radius%已知车速和半径，可以求得角速度。

DFWA=atan(L/Radius);

for k=1:1:N
Alpha(k)=Alpha_init+OmegaTs(k-1);
RefTraj(k,1)=Radiuscos(Alpha(k))+CEN_x;%x
RefTraj(k,2)=Radiussin(Alpha(k))+CEN_y;%y
RefTraj(k,3)=Func_Theta_Pos(Alpha(k));%theta

RefTraj(k,4)=DFWA;%前轮偏角，可以当做前馈量

end
RefTraj_x= RefTraj(:,1);
RefTraj_y= RefTraj(:,2);
RefTraj_theta= RefTraj(:,3);
RefTraj_delta= RefTraj(:,4);

end

function [X,Y,H]=Func_VehicleKineticModule_Euler(x,y,heading,vel,FWA,DFWA,T,L)
%车辆运动学模型，状态量，x,y,heading;控制量：vel=constant，FWA
%固定的步数，来求得数值解

%%
%initial the status of the vehicle
num=100;
Xmc=zeros(1,num);
Ymc=zeros(1,num);
Headingmc=zeros(1,num);
Xmc(1)=x;
Ymc(1)=y;%x,y初始坐标
Headingmc(1)=heading;%航向,

Headingrate=zeros(1,num);
FrontWheelAngle=zeros(1,num);

t=T/num;
%%
FrontWheelAngle=linspace(FWA,DFWA,num);%前轮偏角
Headingrate=veltan(FrontWheelAngle)/L;
for i=2:num
Headingmc(i)=Headingmc(i-1)+Headingrate(i)t;
Xmc(i)=Xmc(i-1)+veltcos(Headingmc(i-1));
Ymc(i)=Ymc(i-1)+veltsin(Headingmc(i-1));
end
%%
X=Xmc(num);
Y=Ymc(num);
H=Headingmc(num);
end

%% test
% [X,Y,H]=VehicleKineticModule_Euler(0,0,0,10,0,3,0.1,2.85)
%plot(X,Y,‘b’);


现在再看看MPC的代码实现

clc;
close all;
clear all;
%% 参考轨迹生成
N=100;%参考轨迹点数量
T=0.05;%采样时间，控制周期
% Xout=zeros(2N,3);
% Tout=zeros(2N,1);
Xout=zeros(N,3);
Tout=zeros(N,1);
for k=1:1:N
Xout(k,1)=k*T;
Xout(k,2)=2;
Xout(k,3)=0;
Tout(k,1)=(k-1)*T;
end

%% Tracking a constant reference trajectory
Nx=3;%状态量个数
Nu =2;%控制量个数
Tsim =20;%仿真时间
X0 = [0 0 pi/3];%初始状态
[Nr,Nc] = size(Xout); % Nr is the number of rows of Xout，1003
% Mobile Robot Parameters
c = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
L = 1;%车辆轴距
Rr = 1;
w = 1;
% Mobile Robot variable Model
vd1 = Rrw; % For circular trajectory，参考系统的纵向速度
vd2 = 0;%参考系统的前轮偏角

%根据控制系统的维度信息，提前定义好相关矩阵并赋值
x_real=zeros(Nr,Nc);%X的真实状态
x_piao=zeros(Nr,Nc);%X的误差状态
u_real=zeros(Nr,2);%真实控制量
u_piao=zeros(Nr,2);%误差控制量
x_real(1,:)=X0;%初始状态
x_piao(1,:)=x_real(1,:)-Xout(1,:);%与预期的误差值
X_PIAO=zeros(Nr,NxTsim);
XXX=zeros(Nr,NxTsim);%用于保持每个时刻预测的所有状态值
q=[1 0 0;0 1 0;0 0 0.5];
Q_cell=cell(Tsim,Tsim);
for i=1:1:Tsim
for j=1:1:Tsim
if i==j
Q_cell{i,j}=q;
else
Q_cell{i,j}=zeros(Nx,Nx);
end
end
end
Q=cell2mat(Q_cell);%权重矩阵
R=0.1eye(NuTsim,Nu*Tsim);%权重矩阵

%模型预测控制主体
for i=1:1:Nr
t_d =Xout(i,3);
a=[1 0 -vd1*sin(t_d)T;
0 1 vd1cos(t_d)*T;
0 0 1;];
b=[cos(t_d)*T 0;
sin(t_d)*T 0;
0 T;];
A_cell=cell(Tsim,1);
B_cell=cell(Tsim,Tsim);
for j=1:1:Tsim
A_cell{j,1}=a^j;
for k=1:1:Tsim
if k<=j
B_cell{j,k}=(a^(j-k))*b;
else
B_cell{j,k}=zeros(Nx,Nu);
end
end
end
A=cell2mat(A_cell);
B=cell2mat(B_cell);

H=2*(B'*Q*B+R);
f=2*B'*Q*A*x_piao(i,:)';
A_cons=[];
b_cons=[];
lb=[-1;-1];
ub=[1;1];
tic
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);%二次规划求解
toc
X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';
if i+j<Nr
     for j=1:1:Tsim
         XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
         XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
         XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
     end
else
     for j=1:1:Tsim
         XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
         XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
         XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
     end

img
img

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

[外链图片转存中…(img-gjIVcAak-1715838258158)]
[外链图片转存中…(img-29sIsmVd-1715838258158)]

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

2401_84975883
关注
  • 3
    点赞
  • 踩
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
自动驾驶-最优控制方法LQR的C语言工程实现【附Github源码链接】
qq_40464599的博客
07-19 2592
最近一直在致力于LQR的c语言工程实现，从LQR工程实现的调研到使用QR求矩阵逆，再到今天的LQR的C语言的实现，从底层一步一步去写算法，这种感觉让我回忆起在哈工大读硕期间，和算法能力非常强的泽哥合作时，他将复杂的空间三维立体路径规划问题，一步一步拆分成可用计算机语言去处理的逻辑，我在他的引导下顺利完成了代码的编写，对他的算法能力甚是佩服。是每天在重复做调参？要想矩阵逆，首先要QR分解，然后再求上三角矩阵的逆，这些具体的算法如果不懂没关系，但是要能看懂所给的数学结论，然后用这些数学结论的公式去编写代码。..
LQR设计简单matlab代码
06-06
LQR设计简单的例子，matlab代码和说明文档
LQR算法低速轨迹跟踪代码
更适合青年研究者的资源库！公众号：杰哥的无人驾驶便利店
03-30 2881
function LQR() clc; %% 给定参数： vx = 5; % 纵向车速,单位：m/s L=3;%轴距 T=0.05;% sample time， control period % 给定圆形参考轨迹 CEN=[0,0]; % 圆心 Radius=20; % 半径 %% 设置参数 Hp =10;%predictive horizion, control horizon N_l=200;% 设置迭代次数 Nx=3;%状态变量参数的个数 Nu=1;%控制变量参数的.
LQR算法原理和代码实现
weixin_44924725的博客
03-22 8194
本文讲解线性二次优化器LQR的原理和C++的代码实现，同时在CARLA-ROS联合仿真环境验证算法效果。
【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现
热门推荐
weixin_42301220的博客
05-29 1万+
文章目录参考资料1. 基本概念1.1 运动学模型的离散状态方程1.2 LQR求解步骤2. python实现2.1 车辆模型2.2 相关参数设置2.3 生成轨迹曲线2.4 角度归一化2.5 解代数里卡提方程2.6 LQR控制算法2.7 主函数 参考资料 基于运动模型的LQR优化 强化学习与最优控制：用于轨迹跟踪的 LQR Linear–quadratic regulator (LQR) steering control 路径规划与轨迹跟踪系列算法 1. 基本概念 从控制理论角度出发的讲解可以翻看博客。
AIEM.rar_AIEM MATLAB_AIEM模型_AIEM模型代码_散射 积分方程
07-14
AIEM模型的代码实现对于科研人员和工程师来说具有很高的价值，因为它可以直接用于模拟实验，调整参数以适应不同场景，甚至可以与其他模型比较，以验证其准确性。因此，理解和掌握AIEM模型及其MATLAB实现对于深入理解...
ARMA.rar_ARMA matlab_ARMA模型预测_MATLAB ARMA预测_预测 ARMA
09-19
在MATLAB中实现ARMA模型预测，首先需要理解模型的基本结构。ARMA模型由自回归（AR）部分和移动平均（MA）部分组成。AR部分描述了当前值与过去若干期的滞后值之间的线性关系，而MA部分则表示当前值是过去误差项的线性...
krigingmodel_Matlab_代理模型_代理模型matlab_Kriging代理模型_krigingmodel_Mat
09-30
krigingmodel_Matlab 是一个基于Matlab实现的Kriging代理模型工具包，它在工程优化、仿真分析和预测等领域中有着广泛的应用。Kriging代理模型是一种统计插值方法，通过构建高斯过程回归模型来逼近复杂函数，以进行...
MATLAB教学视频：傅里叶变换的频域滤波详解.zip_matlab 分析谱半径 结构工程_wilson-theta_傅里叶_动
07-15
这个可以用来做结构动力学的反应,求解wilson-theta的
matlab-clinder-drawing.rar_drawing_matlab 圆柱体_matlab倾斜圆柱_matlab画
07-15
"matlab-clinder-drawing.rar_drawing_matlab 圆柱体_matlab倾斜圆柱_matlab画"这个标题和描述暗示了如何在MATLAB环境中创建并倾斜圆柱体的教程。下面将详细介绍这些知识点。 首先，MATLAB中的三维图形主要依赖于`...
【控制】《现代控制理论》谢克明老师-第6章-最优控制
赵继超的笔记
12-31 1642
第5章 回到目录 第7章 第6章-最优控制6.1 最优控制的基本概念6.2 最优控制中的变分法6.3 极大值原理6.4 线性二次型最优控制问题6.5 利用 MATLAB 求解线性二次型最优控制问题 6.1 最优控制的基本概念 6.2 最优控制中的变分法 6.3 极大值原理 6.4 线性二次型最优控制问题 6.5 利用 MATLAB 求解线性二次型最优控制问题 ...
路径跟踪之LQR控制算法
Ronnie_Hu的博客
04-29 1万+
（一）车辆建模 两自由度车辆模型为： 考虑恒定不变，则上述模型可以看作一个输入为、状态变量为的控制系统，可以表示为： 对于参考轨迹，可以表示为： 将（2）式在处泰勒展开，并忽略高次项，有： 记、，式（4）-（3）可得： 对上式进行离散化处理，可得： 所以， 记： 则（7）式可以写为： 由于系统矩阵A和输入矩阵B的元素随时间变化，所以上述系统是一个线性时变系统。 （二） LQR控制算法仿真 根据上一篇博文的分析，LQR设计步骤如下： 1）根据工程经验和
路径跟踪—基于车辆运动学方程的离散时间LQR控制仿真
weixin_44020886的博客
02-16 1万+
提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档 文章目录前言一、运动学模型二、建模仿真总结 前言 路径跟踪，轨迹跟踪的算法有基于运动学和基于动力学的，其中LQR算法基于车辆动力学网上有很多，毕竟百度开源了apollo，该算法通过简单改进后的实车效果还是非常棒的，之前通过dSPACE MicroAutoBox做了试验，横向误差都是厘米级。由于动力学模型需要估计车辆重量，轮胎侧偏刚度，还有绕Z轴转动惯量，需要估计的参数非常多，相对而言，运动学则简单了很多，不需要估计很多参数，且听某FSAC
lqr算法 c语言,【CS229 lecture18】linear quadratic regulation(LQR) 線性二次型調節控制
weixin_28784019的博客
05-23 777
lecture18今天來討論我最喜歡的算法之一，用以控制MDP，我覺得是一個優雅、高效、強大的算法。先討論MDP的幾個常見變種(與現有的MDP定義會有不同)。Agendastate-action rewardsfinite horizon MDPslinear dynamical systems 線性動力系統-models-linear quadratic regulation(LQR) 線性二次...
需求跟踪矩阵实例_笔记|线性二次型LQR在路径跟踪中的应用
weixin_39846089的博客
11-22 748
这里是学习《无人驾驶车辆模型预测控制》一书的一点笔记评价函数评价函数第一项突出了对终端误差的要求，为终端误差函数，x表示误差矢量，表达了跟踪过程中误差大小的代价函数第二项表示动态过程中对控制的约束或要求，衡量控制功率大小的代价函数目的：通过控制评价函数J的最小化，寻求最优的控制率u(t)，将系统从初始状态转移到最终状态，实现以不大的控制保持较小的跟踪误差，达到能量和跟踪误差最优的目的。变式：引入拉...
【LQR】离散代数黎卡提方程的求解，附Matlab/python代码（笔记）
RickyWasYoung的博客
09-13 2304
离散系统：在matlab里有现成的函数dlqr()，但为了搞清楚其内核，编写matlab代码展示其求解过程matlab帮助文件里的dlqr()说明其中等号左边的S0认为是S(k+1)，右边的S认为是S(k)此公式迭代即可，采用的迭代思想（仅仅参考迭代法的。
基于线性二次调节器(LQR)法实现机器人路径规划附matlab代码
Matlab_dashi的博客
07-09 269
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。????个人主页：Matlab科研工作室????个人信条：格物致知。⛄ 内容介绍随着移动机器人在农业领域内广泛的应用,由于其复杂不规则的地形以及多变的环境对移动机器人的运动控制和稳定性提出了更加严格的要求,因此如何提高移动机器人对于复杂多变的...
基于LQR算法跟踪圆形轨迹，MATLAB 代码
ilovewold的博客
05-15 2872
希望做一个分享者 轨迹跟踪效果图 位姿误差图 参考博文 LQR 的直观推导及简单应用_白巧克力亦唯心的博客-CSDN博客_lqr算法 关键词：泰勒展开；黎卡提方程求解；状态反馈 clear all; close all; clc; %% 参数 ts=0.1; t=30; d=1.45; %% 初始状态 x(1)=0; y(1)=0; xita(1)=0; v=3; w=0.2; u=[v;w]; Z(:,1)=[-3;-3;0.2]; for k=1:1:t/ts .
Visual studio C++：LQR轨迹跟踪仿真
superem_的博客
07-26 5301
Visual studio C++：LQR轨迹跟踪仿真
function [reftraj_x,reftraj_y,reftraj_theta,reftraj_delta]=funccircularrefer
最新发布
02-04
最后，将计算得到的参考值分别存储在reftraj_x、reftraj_y、reftraj_theta和reftraj_delta中，并返回给调用者。 函数funccircularrefer的输入参数包括圆心位置、半径和起始角度，输出参数包括参考轨迹的x坐标、y...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值