网上学习资料一大堆,但如果学到的知识不成体系,遇到问题时只是浅尝辄止,不再深入研究,那么很难做到真正的技术提升。
一个人可以走的很快,但一群人才能走的更远!不论你是正从事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
网上学习资料一大堆,但如果学到的知识不成体系,遇到问题时只是浅尝辄止,不再深入研究,那么很难做到真正的技术提升。
一个人可以走的很快,但一群人才能走的更远!不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人,都欢迎加入我们的的圈子(技术交流、学习资源、职场吐槽、大厂内推、面试辅导),让我们一起学习成长!
[外链图片转存中…(img-gjIVcAak-1715838258158)]
[外链图片转存中…(img-29sIsmVd-1715838258158)]
网上学习资料一大堆,但如果学到的知识不成体系,遇到问题时只是浅尝辄止,不再深入研究,那么很难做到真正的技术提升。
一个人可以走的很快,但一群人才能走的更远!不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人,都欢迎加入我们的的圈子(技术交流、学习资源、职场吐槽、大厂内推、面试辅导),让我们一起学习成长!