
---
**两轮差速小车的路径跟踪实战**
今天用 MATLAB 搞了个路径跟踪的仿真,核心是想对比 PID 和 LQR 两种方法对轨迹控制
的性能差异。直接上干货,从模型到代码,边写边分析。
---
**先说模型:两轮差速的底层逻辑**
小车的运动学模型简化成两轮差速驱动,左右轮速度分别为 v_left 和 v_right。核心公式就
两个:
```matlab
% 状态更新方程
theta_dot = (v_right - v_left) / wheelbase; % 航向角变化率
v = (v_right + v_left) / 2; % 线速度
x_dot = v * cos(theta);
y_dot = v * sin(theta);
```
这里`wheelbase`是轮间距,直接决定转向灵敏度。仿真时用欧拉法更新位置和角度,简单
粗暴但有效。
---
**经典 PID:先调航向,再怼距离**
先试单 PID 控制航向角的方案。目标路径设为直线 y=0,小车从(0,2)出发,初始航向角
45 度(故意给个偏差)。PID 控制器只负责把航向角拉到 0 度,让车头对准目标线。
关键代码片段:
```matlab
% PID 航向控制
error_theta = target_theta - current_theta;
integral_theta = integral_theta + error_theta * dt;
derivative_theta = (error_theta - prev_error_theta) / dt;
output_theta = Kp*error_theta + Ki*integral_theta + Kd*derivative_theta;
% 计算差速(假设线速度固定)
v_left = v_set - output_theta;
v_right = v_set + output_theta;
```