✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
随着移动机器人在农业领域内广泛的应用,由于其复杂不规则的地形以及多变的环境对移动机器人的运动控制和稳定性提出了更加严格的要求,因此如何提高移动机器人对于复杂多变的环境的自主优化能力成为了众多学者研究的热点.
⛄ 部分代码
% LQR法clcclearclose allload 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% 画图figureplot(refPos_x,refPos_y,'r')hold onfor 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_LQRsave 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.
