✅作者简介:热爱科研的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.