基于线性二次调节器(LQR)法实现机器人路径规划附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

⛄ 内容介绍

随着移动机器人在农业领域内广泛的应用,由于其复杂不规则的地形以及多变的环境对移动机器人的运动控制和稳定性提出了更加严格的要求,因此如何提高移动机器人对于复杂多变的环境的自主优化能力成为了众多学者研究的热点.

⛄ 部分代码

% LQR法

clc
clear
close all
load  path.mat

%% 相关参数定义
dt = 0.1;
L = 2.9 ;
Q = 100*eye(3);
R = eye(2)* 2;

%% 轨迹处理
% 定义参考轨迹
refPos_x = path(:,1);
refPos_y = path(:,2);
refPos = [refPos_x, refPos_y];

% 计算航向角和曲率
diff_x = diff(refPos_x) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(refPos_y) ;
diff_y(end+1) = diff_y(end);
derivative1 = gradient(refPos_y) ./ abs(diff_x);              % 一阶导数
derivative2 = del2(refPos_y) ./ abs(diff_x);                  % 二阶导数
refHeading = atan2(diff_y , diff_x);                   % 航向角
refK = abs(derivative2) ./ (1+derivative1.^2).^(3/2);  % 计算曲率

% 根据阿克曼转向原理，计算参考前轮转角
refPos_Delta = atan(L*refK);

% 参考速度
refSpeed = 40/3.6;

%% 主程序
% 赋初值
x = refPos_x(1)+0.5; 
y = refPos_y(1)+0.5; 
yaw = refHeading(1)+0.02;
v = 10;
Delta = 0;
idx = 1;

% 轨迹跟踪实际量
pos_actual = [x,y];
v_actual  = v;
Delta_actual = Delta;
idx_actual = 1;
latError_LQR = [];

% 循环
while idx < length(refPos_x)-1
    % 寻找参考轨迹最近目标点
    idx = calc_target_index(x,y,refPos_x,refPos_y);  
    
    % LQR控制器
    [v_delta,delta,delta_r,latError] =  LQR_control(idx,x,y,v,yaw,refPos_x,refPos_y,refHeading,refPos_Delta,refK,L,Q,R,dt);    
    
    % 如果误差过大，退出循迹
    if abs(latError) > 3
        disp('误差过大，退出程序!\n')
        break
    end
    
    % 更新状态
    [x,y,yaw,v,Delta] = update(x,y,yaw,v, v_delta,delta, dt,L, refSpeed,delta_r);
    
    % 保存每一步的实际量
    pos_actual(end+1,:) = [x,y];
    v_actual(end+1,:)  = v;
    Delta_actual(end+1)  = Delta;
    idx_actual(end+1) = idx;
    latError_LQR(end+1,:) =  [idx,latError];
end

% 画图
figure
plot(refPos_x,refPos_y,'r')
hold on
for i = 1:size(pos_actual,1)
    scatter(pos_actual(i,1), pos_actual(i,2),150,'b.')
    pause(0.01);
end

% 保存
path_LQR = pos_actual;
save path_LQR.mat path_LQR
save latError_LQR.mat latError_LQR

⛄ 运行结果

⛄ 参考文献

[1] 强明辉,李娟.利用遗传算法优化线性二次型调节器(LQR)[J].甘肃工业大学学报, 1998.DOI:CNKI:SUN:GSGY.0.1998-04-011.

[2] 强明辉,周鹏,李娟,等.利用遗传算法优化线性二次型调节器(LQR)[J].兰州理工大学学报, 1998(4):51-55.

[3] 张羊阳.基于自适应动态规划的履带机器人路径跟随控制方法研究[D].安徽农业大学,2019.

[4] 徐鑫,刘彬,刘东洋.基于线性二次调节器算法的两轮自平衡机器人最优控制方法[J].科学技术与工程, 2021, 21(21):8.

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料

🍅 仿真咨询

1.卷积神经网络(CNN)、LSTM、支持向量机(SVM)、最小二乘支持向量机(LSSVM)、极限学习机(ELM)、核极限学习机(KELM)、BP、RBF、宽度学习、DBN、RF、RBF、DELM实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

2.图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

3.旅行商问题(TSP)、车辆路径问题(VRP、MVRP、CVRP、VRPTW等)、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划

4.无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

5.传感器部署优化、通信协议优化、路由优化、目标定位

6.信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号

7.生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化

8.微电网优化、无功优化、配电网重构、储能配置

9.元胞自动机交通流 人群疏散 病毒扩散 晶体生长