LQR控制Matlab,lqr控制matlab仿真,matlab

% 基于车辆二自由度动力学误差模型的连续系统无限时间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;