MATLAB路径规划仿真 轨迹规划，船舶轨迹跟踪控制，数学模型基于两轮差速的小车模型，用PID环节对航向角进行控制，迫使小车走向目标，或用PID环节对航向角和距离进行控制，迫使小车走向目标 LQR 算法 可自行小车起点坐标
在智能小车的研究领域，路径规划和轨迹跟踪控制是关键技术。今天咱们就来聊聊基于 MATLAB 的相关实现，这里涉及基于两轮差速的小车模型，以及 PID 控制和 LQR 算法。
两轮差速小车模型
两轮差速小车是常见的移动机器人模型。它通过左右轮的速度差来实现转向。假设小车的轴距为 \( L \)，左轮速度为 \( vl \)，右轮速度为 \( vr \)，那么小车的线速度 \( v \) 和角速度 \( \omega \) 可以表示为：
\[ v = \frac{vl + vr}{2} \]
\[ \omega = \frac{vr - vl}{L} \]
在 MATLAB 中，我们可以这样简单定义小车的模型参数：
L = 0.3; % 轴距，单位：米这里设定了小车的轴距为 0.3 米，后续计算小车运动状态时会用到这个参数。
PID 控制实现航向角控制
PID 控制是一种经典且应用广泛的控制算法。在小车路径规划中，我们可以用它来控制航向角，使小车走向目标。PID 控制器的输出 \( u \) 由比例（P）、积分（I）和微分（D）三部分组成，公式如下：
\[ u(t) = Kp e(t) + Ki \int{0}^{t} e(\tau) d\tau + Kd \frac{de(t)}{dt} \]
其中 \( e(t) \) 是误差，即目标值与当前值的差， \( Kp \)、 \( Ki \)、 \( K_d \) 分别是比例、积分、微分系数。
假设我们已经知道小车当前航向角 \( \theta \) 和目标航向角 \( \theta{target} \)，误差 \( e = \theta{target} - \theta \)。在 MATLAB 中实现一个简单的离散 PID 控制代码如下：
% PID 参数 Kp = 1; Ki = 0.1; Kd = 0.01; dt = 0.01; % 时间步长 e_prev = 0; I = 0; for k = 1:num_steps e = theta_target - theta(k); P_term = Kp * e; I = I + e * dt; I_term = Ki * I; D_term = Kd * (e - e_prev) / dt; u(k) = P_term + I_term + D_term; e_prev = e; % 根据控制量 u(k) 更新小车状态，这里省略具体更新代码 end上述代码初始化了 PID 的三个参数 \( Kp \)、 \( Ki \)、 \( Kd \) 以及时间步长 \( dt \)。在循环中，计算每一步的误差 \( e \)，然后分别算出比例项 \( Pterm \)、积分项 \( Iterm \) 和微分项 \( Dterm \)，最终得到控制量 \( u(k) \)。不过这里省略了根据控制量更新小车实际状态的具体代码，实际应用中需要根据小车运动学模型来实现。
PID 控制实现航向角和距离控制
除了单纯控制航向角，我们还可以同时对航向角和距离进行控制。假设小车当前位置 \( (x, y) \)，目标位置 \( (x{target}, y{target}) \)，首先计算距离误差 \( d{error} = \sqrt{(x{target} - x)^2 + (y_{target} - y)^2} \)。然后结合航向角误差，综合计算 PID 的误差项。
% 假设小车当前位置 (x, y) 和目标位置 (x_target, y_target) x = 0; y = 0; % 初始位置 x_target = 5; y_target = 5; % 目标位置 d_error = sqrt((x_target - x)^2 + (y_target - y)^2); theta_error = theta_target - theta; % 综合误差计算，这里简单加权相加，实际需调整权重 total_error = 0.5 * d_error + 0.5 * theta_error;在这段代码中，我们先计算了距离误差 \( d{error} \) 和航向角误差 \( thetaerror \)，然后通过简单加权得到综合误差 \( total_error \)，实际应用中需要根据具体情况调整权重以达到更好的控制效果。之后就可以像之前一样，基于这个综合误差进行 PID 控制计算。
LQR 算法
LQR（线性二次型调节器）是一种最优控制算法。它通过求解 Riccati 方程，找到使二次型性能指标最小化的反馈控制律。对于线性化后的两轮差速小车模型，状态方程可以表示为 \( \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \)，其中 \( \mathbf{x} \) 是状态向量，包含位置、速度、航向角等信息， \( \mathbf{u} \) 是控制输入向量， \( \mathbf{A} \) 和 \( \mathbf{B} \) 是系统矩阵。
在 MATLAB 中使用 LQR 函数非常方便，假设我们已经定义好 \( \mathbf{A} \)、 \( \mathbf{B} \) 矩阵，以及权重矩阵 \( \mathbf{Q} \) 和 \( \mathbf{R} \)：
% 假设已定义 A, B 矩阵 Q = [1 0 0; 0 1 0; 0 0 1]; % 状态权重矩阵 R = 0.1; % 控制输入权重矩阵 [K, S, E] = lqr(A, B, Q, R);上述代码调用lqr函数，传入系统矩阵 \( \mathbf{A} \)、 \( \mathbf{B} \) 以及权重矩阵 \( \mathbf{Q} \)、 \( \mathbf{R} \)，得到反馈增益矩阵 \( K \)，后续就可以根据这个增益矩阵来计算控制输入 \( \mathbf{u} = -K\mathbf{x} \)。
设定小车起点坐标并仿真
我们可以自行设定小车的起点坐标，假设起点为 \( (0, 0) \)，目标点为 \( (5, 5) \)。在 MATLAB 中可以结合上述控制算法进行路径规划的仿真。这里以简单的基于 PID 控制航向角和距离的情况为例：
% 设定起点和目标点 start_x = 0; start_y = 0; target_x = 5; target_y = 5; % 初始化小车状态 x = start_x; y = start_y; theta = 0; % 初始航向角 % 仿真循环 num_steps = 1000; for k = 1:num_steps % 计算距离和航向角误差 d_error = sqrt((target_x - x)^2 + (target_y - y)^2); theta_error = atan2(target_y - y, target_x - x) - theta; total_error = 0.5 * d_error + 0.5 * theta_error; % PID 控制计算 % 省略具体 PID 计算代码，参考前面 PID 控制部分 % 根据控制量更新小车状态 % 省略具体更新代码 % 记录小车位置，用于绘图 x_history(k) = x; y_history(k) = y; end % 绘图展示小车轨迹 figure; plot(x_history, y_history); xlabel('X 坐标'); ylabel('Y 坐标'); title('小车路径规划轨迹');这段代码设定了小车起点和目标点，在仿真循环中不断计算误差，通过 PID 控制得到控制量并更新小车状态，同时记录小车位置，最后绘图展示小车的轨迹。
通过以上方法，我们在 MATLAB 中实现了基于两轮差速小车模型的路径规划与轨迹跟踪控制，PID 控制和 LQR 算法都为小车智能行驶提供了有效的手段，希望这些内容能为你的研究或项目带来帮助。
以上代码仅为示意性代码，实际应用中需根据具体需求和模型进行完善与调试。
