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