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

2401_84975883

于 2024-05-16 13:44:06 发布

阅读量284

点赞数 5

文章标签： c语言 c++ 学习

本文链接：https://blog.csdn.net/2401_84975883/article/details/138958438

版权

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

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

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

MPC solves a constrained optimization

In practice, optimization could lead to over-voltage, ovre-current, excessive force etc. You want a motor starts very quickly? The optimizer tells you give it an infinite electric current. So you use a saturation which destroys the optimality. MPC solves an optimization without excessing the limits.

In addition, LQR can be solved offline for an LTI system. However, MPC is not a linear controller. Typically, it must be solved online at each sample time. It requires higher computational load. MPC has toolbox in MATLAB. You can use it before you learn its theory in deep.

参考链接*https://www.quora.com/Whats-the-difference-between-constrained-LQR-and-MPC*

function LQR_1()
%这里先从简单开始，给定一个直线车道和车辆位置偏差。
%参考轨迹的生成方法有两种：
%1.车辆在Path上投影，然后在PATH上选取一系列的点作参考点
%*现在遇到的问题是Q R的参数怎么设置。而且通用性怎么办？*%

clear all;
close all;
clc;
%% 给定参数：

vel = 6; % 纵向车速,单位：m/s
L=2.85;%轴距
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;%控制变量参数的个数

FWA=zeros(N_l,1);%前轮偏角
FWA(1,1)= 0; %初始状态的前轮偏角

x_real=zeros(Nx,N_l);%实际状态
x_real(:,1)= [22 0 pi/2]; %x0=车辆初始状态X_init初始状态
% x_piao=zeros(N_l,Nx);%实际状态与参考轨迹的误差
% 
% u_real=zeros(N_l,Nu);%实际的控制量
% u_piao=zeros(N_l,Nu);%实际控制量与参考控制量的误差

% X_PIAO=zeros(N_l,3*Hp);%通过DR估计的状态
% 
% XXX=zeros(N_l,3*Hp);%用于保持每个时刻预测的所有状态值

RefTraj=zeros(3,1);
Delta_x = zeros(3,1);

Q=[10 0 0; 0 10 0; 0 0 100];
R=[10];%r是对控制量误差的weighting matrice

Pk=[1 0 0; 0 1 0; 0 0 1]; %人为给定,相当于QN
Vk=[0 0 0]'; %人为给定,相当于QN

%%  算法实现
 u_feedBackward=0;
 u_feedForward=0;
 
 %*首先生成参考轨迹，画出图来作参考*%
 [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)-2*pi;
    else if Delta_x(3,1) < -1*pi
            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            vel*T/L;]; 
        COS2 = cos(RefTraj_delta(j-1))^2;
        B=[ 0 0  vel*T/(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 Xb==Xn
    if Yn>Yb
        K=pi/2;
    else
        K=3*pi/2;
    end
else
    if Yb==Yn
        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 >= 3*pi/2
    Theta = Alpha-3*pi/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+Omega*Ts*(k-1);
    RefTraj(k,1)=Radius*cos(Alpha(k))+CEN_x;%x
    RefTraj(k,2)=Radius*sin(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=vel*tan(FrontWheelAngle)/L;
for i=2:num
    Headingmc(i)=Headingmc(i-1)+Headingrate(i)*t;
    Xmc(i)=Xmc(i-1)+vel*t*cos(Headingmc(i-1));
    Ymc(i)=Ymc(i-1)+vel*t*sin(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(2*N,3);
% Tout=zeros(2*N,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，100*3
% 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 = Rr*w; % For circular trajectory，参考系统的纵向速度
vd2 = 0;%参考系统的前轮偏角

%根据控制系统的维度信息，提前定义好相关矩阵并赋值
x_real=zeros(Nr,Nc);%X的真实状态
x_piao=zeros(Nr,Nc);%X的误差状态
u_real=zeros(Nr,2);%真实控制量


![img](https://img-blog.csdnimg.cn/img_convert/b581a4d156fa2dfd9188bbf3f5925e2e.png)
![img](https://img-blog.csdnimg.cn/img_convert/b7b9a5d59e9efb376c9ebfb46f71ec13.png)

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

**[需要这份系统化的资料的朋友，可以添加戳这里获取](https://bbs.csdn.net/topics/618668825)**


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



[外链图片转存中...(img-vqdhDOui-1715838224949)]
[外链图片转存中...(img-JLueAftZ-1715838224949)]

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

**[需要这份系统化的资料的朋友，可以添加戳这里获取](https://bbs.csdn.net/topics/618668825)**


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