MATLAB路径规划仿真 轨迹规划，船舶轨迹跟踪控制，数学模型基于两轮差速的小车模型，用PID环节对航向角进行控制，迫使小车走向目标，或用PID环节对航向角和距离进行控制，迫使小车走向目标 LQR 算法 可自行小车起点坐标

在智能车控制以及诸多移动机器人领域，路径规划与轨迹跟踪控制一直是热门研究方向。今天咱们就来唠唠基于 MATLAB 的相关仿真实现，以两轮差速小车模型为基础，探索 PID 控制和 LQR 算法在轨迹控制中的应用。

两轮差速小车数学模型

两轮差速小车通过左右轮不同的转速来实现转向和前进后退。假设小车的轴距为 \(L\)，左右轮半径为 \(r\)，左轮速度为 \(vl\)，右轮速度为 \(vr\)。

小车的线速度 \(v\) 和角速度 \(\omega\) 可以表示为：

\[v = \frac{r(vl + vr)}{2}\]

\[\omega = \frac{r(vr - vl)}{L}\]

在 MATLAB 中，我们可以简单定义这些参数，例如：

L = 0.5; % 轴距
r = 0.1; % 轮半径

PID 控制航向角

PID 控制是一种经典且应用广泛的控制算法。在小车轨迹跟踪中，我们可以利用 PID 控制航向角，使小车朝着目标方向前进。

MATLAB路径规划仿真 轨迹规划，船舶轨迹跟踪控制，数学模型基于两轮差速的小车模型，用PID环节对航向角进行控制，迫使小车走向目标，或用PID环节对航向角和距离进行控制，迫使小车走向目标 LQR 算法 可自行小车起点坐标

假设我们已经获取了当前小车的航向角 \(\theta\) 和目标航向角 \(\thetad\)，航向角误差 \(e{\theta}=\theta_d - \theta\)。PID 控制器的输出 \(u\) 可以表示为：

\[u = Kp e{\theta}+Ki \int{0}^{t} e{\theta} dt + Kd \frac{de_{\theta}}{dt}\]

在 MATLAB 中实现简单的 PID 控制航向角的代码片段如下：

% PID 参数
Kp = 1;
Ki = 0.1;
Kd = 0.01;
dt = 0.01; % 时间步长

% 初始化误差和积分项
e_theta_prev = 0;
integral_theta = 0;

for k = 1:1000 % 仿真步数
    % 获取当前和目标航向角，这里假设已有获取函数
    theta = get_current_theta(); 
    theta_d = get_desired_theta(); 
    e_theta = theta_d - theta;
    
    integral_theta = integral_theta + e_theta * dt;
    derivative_theta = (e_theta - e_theta_prev) / dt;
    
    u = Kp * e_theta + Ki * integral_theta + Kd * derivative_theta;
    
    % 根据u来调整小车左右轮速度，这里假设有调整函数
    adjust_wheel_speeds(u); 
    
    e_theta_prev = e_theta;
    % 小车运动更新位置和航向角等，假设有更新函数
    update_cart_state(); 
end

这里，Kp、Ki、Kd 分别是比例、积分、微分系数。我们在每个时间步计算误差，并根据 PID 公式计算控制量 u，进而调整小车左右轮速度。integraltheta 用于累计误差，derivativetheta 计算误差的变化率。

PID 控制航向角和距离

除了单纯控制航向角，我们还可以结合距离信息。假设小车当前位置 \((x,y)\) 与目标位置 \((xd,yd)\)，距离误差 \(ed=\sqrt{(xd - x)^2+(y_d - y)^2}\)。同样可以设计一个结合距离和航向角误差的 PID 控制器。

在 MATLAB 中的实现代码框架类似，不过误差计算部分会更复杂些：

% 新的PID参数，针对距离和航向角
Kp_d = 0.5;
Ki_d = 0.05;
Kd_d = 0.005;

% 初始化距离和航向角相关误差和积分项
e_theta_prev = 0;
e_d_prev = 0;
integral_theta = 0;
integral_d = 0;

for k = 1:1000 % 仿真步数
    % 获取当前位置、航向角和目标位置、航向角
    [x,y] = get_current_position(); 
    [x_d,y_d] = get_desired_position(); 
    theta = get_current_theta(); 
    theta_d = get_desired_theta(); 
    
    e_theta = theta_d - theta;
    e_d = sqrt((x_d - x)^2+(y_d - y)^2);
    
    integral_theta = integral_theta + e_theta * dt;
    integral_d = integral_d + e_d * dt;
    
    derivative_theta = (e_theta - e_theta_prev) / dt;
    derivative_d = (e_d - e_d_prev) / dt;
    
    u_theta = Kp * e_theta + Ki * integral_theta + Kd * derivative_theta;
    u_d = Kp_d * e_d + Ki_d * integral_d + Kd_d * derivative_d;
    
    % 综合两个控制量来调整小车左右轮速度，假设有融合函数
    u = combine_control_commands(u_theta, u_d); 
    adjust_wheel_speeds(u); 
    
    e_theta_prev = e_theta;
    e_d_prev = e_d;
    % 小车运动更新位置和航向角等，假设有更新函数
    update_cart_state(); 
end

这里分别针对距离和航向角误差设计了 PID 控制，最后将两个控制量融合来调整小车运动。

LQR 算法

LQR（线性二次型调节器）是一种基于状态空间的最优控制算法。首先我们要将两轮差速小车模型转化为状态空间形式。假设小车的状态 \(x = [x, y, \theta]^T\)，输入 \(u=[vl, vr]^T\)。经过一系列推导（这里省略详细过程），可以得到状态空间方程 \(\dot{x}=Ax + Bu\)。

在 MATLAB 中，我们可以利用 lqr 函数来设计 LQR 控制器。例如：

% 假设已经得到A和B矩阵
A = [0 0 0; 0 0 0; 0 0 0]; 
B = [0 0; 0 0; 0 0]; 

% 定义Q和R矩阵，用于权衡状态和输入
Q = [1 0 0; 0 1 0; 0 0 1]; 
R = [0.1 0; 0 0.1]; 

[K, S, E] = lqr(A, B, Q, R);

这里 Q 矩阵用于对状态进行加权，R 矩阵对输入进行加权。lqr 函数返回反馈增益矩阵 K，我们可以利用这个矩阵根据当前状态来计算最优控制输入。

仿真与总结

通过上述方法，我们可以在 MATLAB 中对两轮差速小车的轨迹规划与控制进行仿真。PID 控制简单直观，易于实现和调试；而 LQR 算法则从最优控制的角度出发，能得到性能更优的控制策略。在实际应用中，可以根据具体需求和场景选择合适的方法或结合使用，以实现小车高效准确的轨迹跟踪。希望大家通过这些分享，能在自己的项目中更好地运用这些方法。