MPC追踪点matlab仿真出现误差
利用MPC原理控制差速两轮小车进行目标点追踪，仿真后发现规划出的“最优”输入走出来的路径很奇怪，有一个奇怪的尖角。我把R置零，代价函数应该只考虑了输出误差，此时为什么没有直接靠近目标点，而是拐了一个尖角？
clc;
clear all;
%% 生成参考轨迹 计算所有期望值
T=0.1; %采样时间
L=0.245; %轮间距
t=0:1:10*pi;
xr=5*sin(0.2*t);
yr=5-5*cos(0.2*t); %逆时针的圆
thetar=0.2*t;
N=length(xr);
Xr=[xr' yr' thetar'];
%参考控制量装载
vcr=kron(ones(N,1),0.25);
w=kron(ones(N,1),0.05);
Ur=[vcr w];
%% 控制系统的基本信息
Nx=3;
Nu=2;
Np=30; %预测步长
Nc=10; %控制步长
Row=10; %松弛因子
X0=[0 0 0]; %车辆初始状态
U0=[0 0];
%% 控制系统维度信息
qe0={(X0-Xr(2,:))'}; % 加了转置，状态量按列看
%!!!!单点跟踪时需要修改
qe0=cell2mat(qe0);
ue(:,1)=[0;0];
ue_collect(:,1)=ue(:,1); %用来记录所有产生的ue
qe(:,1)=qe0;
qe_collect(:,1)=qe;
delta_ue=[0;0];
delta_ue_collect(:,1)=[0;0];
X_real(:,1)=X0';
j=0; %用来记录到达参考点误差限内的误差值
k=0; %填充实际状态量
which_dot_flag=0;
Q=[1 0 0;0 1 0;0 0 1]; %theta位的修改会影响轨迹点处的震荡
q=eye(Np);
Q_bar=1*kron(q,Q);
R=1*eye(Nu*Nc);
%%单点跟踪************************
i=2 ;%针对第i个轨迹参考点
theta=Xr(i,3); %theta为期望角度，或期望朝向
A_star=[1 0 -0.25*T*sin(theta);
0 1 0.25*T*cos(theta);
0 0 1;];
B_star=[T*cos(theta) 0;
T*sin(theta) 0;
0 T]; %？？？？？？？？？？？？
A_tilde_cell={A_star B_star;
zeros(2,3) eye(2);};
B_tilde_cell={B_star;eye(2);};
C_tilde_cell={eye(3) zeros(3,2);};
A_tilde=cell2mat(A_tilde_cell);
B_tilde=cell2mat(B_tilde_cell);
C_tilde=cell2mat(C_tilde_cell); %A_tilde*kesi(k)+B_tilde*Δue（k）
PHI_cell=cell(Np,1); %构建并填充PHI和TAHETA
THETA_cell=cell(Np,Nc);
for r=1:1:Np
PHI_cell{r,1}=C_tilde*A_tilde^r;
for s=1:1:Nc
if s<=r
THETA_cell{r,s}=C_tilde*(A_tilde^(r-s))*B_tilde;
else
THETA_cell{r,s}=zeros(Nx,Nu);
end
end
end
PHI=cell2mat(PHI_cell);
THETA=cell2mat(THETA_cell);
H_cell=cell(2,2); %构建H
H_cell{1,1}=2*((THETA')*Q_bar*THETA+R);
H_cell{1,2}=2*zeros(Nu*Nc,1);
H_cell{2,1}=2*zeros(1,Nu*Nc);
H_cell{2,2}=2*Row;
H=cell2mat(H_cell);
H=(H+H')/2;
for m=1:1:200 %控制周期的个数
kesi(:,1)=[qe(:,1);ue(:,1)]; % qe与ue相差一个时刻
E=PHI*kesi(:,1); %%%此处可能存在问题，需改正
%事实上，E=PHI*kesi-Yref,而Yref=0
f_cell=cell(1,2);
f_cell{1,1}=2*E'*Q_bar*THETA;
f_cell{1,2}=0;
f=cell2mat(f_cell);
%生成约束
%不等式约束
A_t=zeros(Nc,Nc);
for p=1:1:Nc
for q=1:1:Nc
if q<=p
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
end
A_I=kron(A_t,eye(Nu));%对应于falcone论文约束处理的矩阵A,求克罗内克积
Ut=kron(ones(Nc,1),ue(:,1));%此处感觉论文里的克罗内科积有问题,暂时交换顺序
umin=[-0.75;-0.2;];%维数与控制变量的个数相同
umax=[0.75;0.2]; %控制量的边界
delta_umin=[-0.5;-0.1;]; %控制量增量的边界
delta_umax=[0.5;0.1];
% umin=[-1;-1;];%维数与控制变量的个数相同
% umax=[1;1]; %控制量的边界
% delta_umin=[-0.5;-0.5;]; %控制量增量的边界
% delta_umax=[0.5;0.5];
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);
A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1)};
b_cons_cell={Umax-Ut;-Umin+Ut};
A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值
% 状态量约束
M=10; %取值为10意义不明 改改看看
delta_Umin=kron(ones(Nc,1),delta_umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
lb=[delta_Umin;0];%（求解方程）状态量下界，包含控制时域内控制增量和松弛因子
ub=[delta_Umax;M];%（求解方程）状态量上界，包含控制时域内控制增量和松弛因子
%options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');
[X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
%计算发现X是单列向量，但X不是[ΔU 伊普西隆]吗，ΔU包含的控制量呢
%控制量也是按列排的，所以两两一组为一个时刻的ue，最后一位是伊普西隆
delta_ue(:,1)=[X(1,1);X(2,1);]; % 仅有一列，会被覆写
ue(:,1)=delta_ue(:,1)+ue(:,1); %为靠近轨迹点计算出的ue
qe(:,1)=A_star*qe(:,1)+B_star*ue(:,1); %计算写入最优输入后与轨迹点的误差，并且此处开始进入下一时刻
k=k+1; %每计算一次qe，即更新一次状态信息，就准备向X_real填充新的数据
X_real(:,k+1)=qe(:,1)+Xr(i,:)';
M_collect(:,k)=X(Nu*Nc+1,1); %记录松弛因子
delta_ue_collect(:,k)=delta_ue;
ue_collect(:,k)=ue;
qe_collect(:,k+1)=qe; %记录写入最优输入后与参考点的误差
fval_collect(:,k)=fval;
end
%%单点绘图**************************
figure(1)
n=1:1:201;
yyaxis left
plot(n,qe_collect(1,:),'r-',n,qe_collect(2,:),'b-')
ylabel('误差（m）')
yyaxis right
plot(n,qe_collect(3,:),'c-')
xlabel('T')
ylabel('误差（rad）')
title('状态误差')
legend('x误差','y误差','方向误差')
figure(2)
n=1:1:200;
u=ue_collect+kron(ones(1,200),[0.25;0.05]);
subplot(1,2,1)
plot(n,kron(ones(1,200),0.25),'r-',n,u(1,:),'b-')
xlabel('T')
ylabel('误差（m/s）')
title('速度跟踪')
legend('期望速度','实际速度')
subplot(1,2,2)
plot(n,kron(ones(1,200),0.05),'r-',n,u(2,:),'b-')
xlabel('T')
ylabel('误差（m/s）')
title('角速度跟踪')
legend('期望角速度','实际角速度')
figure(3)
plot(Xr(i,1),Xr(i,2),'r*',X_real(1,:),X_real(2,:),'b-')
xlabel('x')
ylabel('y')
title('跟踪轨迹')
legend('目标点','跟踪轨迹')
- 帮你找了个相似的问题, 你可以看下: https://ask.csdn.net/questions/1084112
- 这篇博客你也可以参考下：8,MPC的简单matlab实现
如果你已经解决了该问题, 非常希望你能够分享一下解决方案, 写成博客, 将相关链接放在评论区, 以帮助更多的人 ^-^