% 基于车辆二自由度动力学误差模型的连续系统无限时间LQR控制(假设V恒定)
% Author: LiangWang
% Date: 2020/8/29
clc;
clear all;
%% 参考路径生成,包含离散路径点坐标以及曲率
[X_xout,Y_xout]=S_turn();
road_point = path_matching(X_xout,Y_xout);
% figure(1);
% plot(road_point(:,1),road_point(:,2),'r','Linewidth',2);
% xlabel('横坐标X/m');
% ylabel('纵坐标Y/m');
% legend('期望路径');
% hold off;
% figure(2);
% plot(road_point(:,5),road_point(:,4),'r','Linewidth',2);
% xlabel('路程s/m');
% ylabel('曲率/(1/m)');
% legend('参考曲率');
% hold off;
% figure(3);
% plot(road_point(:,5),road_point(:,3),'r','Linewidth',2);
% xlabel('路程s/m');
% ylabel('参考横摆角/rad');
% legend('参考横摆角');
% hold off;
%% 参数设置
dt = 0.02; %采样周期
Q = [5 0 0 0; %误差及其变化率权重矩阵
0 1 0 0;
0 0 1 0;
0 0 0 1;];
R = 2*eye(1); %控制量权重矩阵
steer_max = 35/180*pi; %前轮转角范围约束
vehicle_state_now = [0 0 0 15 0 0]; %车辆当前初始状态[x,y,yaw,Vx,Vy,yaw_rate];
%% LQR控制主体
i = 1;
vehicle_state_all(i,:) = vehicle_state_now;
ind = 0;
ind_end = length(road_point(:,1));
while (ind<ind_end)
[ind,error_y] = find_nearest_point(vehicle_state_now,road_point);
nearest_point = road_point(ind,:);
delta_f = lqr_control(vehicle_state_now,nearest_point,Q,R,error_y,steer_max);
vehicle_state_now = vehicle_update(vehicle_state_now,delta_f,dt,steer_max);
error_y_all(i) = error_y;
delta_f_all(i) = delta_f;
time_all(i) = dt*i;
vehicle_state_all(i+1,:) = vehicle_state_now;
i = i+1;
figure(1);
plot(road_point(:,1),road_point(:,2),'k','Linewidth',1);
hold on;
plot(vehicle_state_all(i-1,1),vehicle_state_all(i-1,2),'r*');
hold on;
end
figure(2);
plot(road_point(:,1),road_point(:,2),'r','Linewidth',1);
hold on;
plot(vehicle_state_all(:,1),vehicle_state_all(:,2),'k','Linewidth',1);
xlabel('横坐标X/m');
ylabel('纵坐标Y/m');
legend('参考轨迹','跟踪结果');
hold off;
figure(3);
plot(time_all,error_y_all,'r','Linewidth',1);
hold on;
xlabel('时间/t');
ylabel('横向误差/m');
legend('跟踪误差');
hold off;
figure(4);
plot(time_all,delta_f_all,'r','Linewidth',1);
hold on;
xlabel('时间/t');
ylabel('前轮转角/rad');
legend('前轮转角变化曲线');
hold off;
- 1
- 2
- 3
- 4
- 5
- 6
前往页