LQR算法低速轨迹跟踪代码

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;%控制变量参数的个数
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初始状态
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,vx,T,L);

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,vx,T,L);
    u_feedForward = RefTraj_delta(G_Test);%前馈控制量
    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   -vx*sin(RefTraj_theta(j-1))*T; 0    1   vx*cos(RefTraj_theta(j-1))*T; 0    0   1;];
        COS2 = cos(RefTraj_delta(j-1))^2;
        B=[ 0 0  vx*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),vx,FWA(i,1),FWA(i+1,1),T,L);    
end
 
%%   绘图
figure(1) %绘制参考路径
plot(RefTraj_x,RefTraj_y,'k')
xlabel('x','fontsize',14)
ylabel('y','fontsize',14)
title('跟踪结果对比');
legend('参考轨迹');
axis equal 
grid on
hold on
plot(x_real(1,:),x_real(2,:),'r*');
hold on;
title('跟踪结果对比');
xlabel('横向位置X');
ylabel('纵向位置Y');  

 
end

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); %对DFWA进行定义
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,vx,FWA,DFWA,T,L)
%车辆运动学模型，状态量，x,y,heading;控制量：vx=constant，FWA
%固定的步数，来求得数值解
 
%%
%initial the status of the vehicle
num=100;  % 假设一个数字为100
Xmc=zeros(1,num); %将X放在1行100列的矩阵中
Ymc=zeros(1,num); %将Y放在1行100列的矩阵中
Headingmc=zeros(1,num); %将车的航向角也放在1行100列的矩阵中
Xmc(1)=x;  %设定初始坐标为x,y1
Ymc(1)=y;%x,y初始坐标
Headingmc(1)=heading;%航向,
Headingrate=zeros(1,num); %车辆的横摆角速度放在1行100列的矩阵中
FrontWheelAngle=zeros(1,num); %前轮偏角放在1行100列的矩阵中
t=T/num; %将采样周期均分为100等分
%%
FrontWheelAngle=linspace(FWA,DFWA,num); %前轮偏角  % 从FWA到DFWA均分为100等分
Headingrate=vx*tan(FrontWheelAngle)/L; %角速度计算
for i=2:num
    Headingmc(i)=Headingmc(i-1)+Headingrate(i)*t;
    Xmc(i)=Xmc(i-1)+vx*t*cos(Headingmc(i-1));
    Ymc(i)=Ymc(i-1)+vx*t*sin(Headingmc(i-1));
end
%%
    X=Xmc(num);
    Y=Ymc(num);
    H=Headingmc(num);
end