【路径规划】线性二次调节器(LQR)法机器人路径规划【含Matlab源码 2810期】

⛄一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【路径规划】基于matlab线性二次调节器(LQR)法机器人路径规划【含Matlab源码 2810期】

获取代码方式2：
付费专栏Matlab路径规划（初级版）

备注：
点击上面蓝色字体付费专栏Matlab路径规划（初级版），扫描上面二维码，付费29.9元订阅海神之光博客付费专栏Matlab路径规划（初级版），凭支付凭证，私信博主，可免费获得1份本博客上传CSDN资源代码（有效期为订阅日起，三天内有效）；
点击CSDN资源下载链接：1份本博客上传CSDN资源代码

⛄二、线性二次调节器(LQR)法机器人路径规划

线性二次调节器（Linear Quadratic Regulator，LQR）是一种经典的控制方法，可以用于机器人路径规划。LQR方法基于状态空间模型，通过优化控制器的权重矩阵来实现最优的轨迹跟踪或稳定控制。

机器人路径规划的目标是使机器人在给定的环境中找到一条最优的路径以达到目标点。LQR方法可以用于在路径规划过程中为机器人提供最优的控制策略。

具体步骤如下：

系统建模：将机器人的动力学或运动学建模为状态空间模型，其中状态变量描述机器人的位置、速度等状态信息，控制输入变量描述施加在机器人上的控制力或扭矩。
状态反馈设计：利用LQR方法设计状态反馈控制器。LQR方法通过优化权重矩阵来最小化系统的性能指标，如跟踪误差或控制输入能量。
路径规划：根据机器人的起始点和目标点，利用路径规划算法（如A*算法、Dijkstra算法等）生成一条最优路径。
路径跟踪：将生成的路径分解为一系列目标点，并在每个时间步计算控制输入。利用LQR控制器计算每个目标点上的最优控制输入。
实施控制：将计算得到的控制输入施加到机器人上，实现路径跟踪或稳定控制。
需要注意的是，LQR方法在路径规划中通常用于局部轨迹跟踪，即机器人在已知目标点的情况下进行控制。对于全局路径规划，还需要结合其他算法和方法来生成一条整体最优的路径。此外，LQR方法也可以与其他路径规划方法相结合，如模型预测控制（Model Predictive Control，MPC），以实现更复杂的路径规划和控制目标。

⛄三、部分源代码

clc
clear
close all
load path.mat

%% 相关参数定义
dt = 0.1;
L = 2.9 ;
Q = 100eye(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

⛄四、运行结果

⛄五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]陈玉昆,高玉洁,邓瑞明,杨乾坤,李鹏飞,郭东升.现代控制理论与设计（第三版）[D]

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除