1. 项目概述
在自动驾驶和车辆动力学控制领域，双移线测试是评估车辆操纵稳定性的经典工况。这个项目通过建立二自由度车辆模型，采用LQR（线性二次调节器）作为控制器，实现了双移线路径的跟踪仿真。作为一名从事车辆控制算法开发多年的工程师，我发现这种基础模型虽然结构简单，但能很好地揭示车辆横向控制的核心原理。
二自由度模型将车辆简化为横向和横摆两个运动方向，既保留了关键动力学特性，又大幅降低了计算复杂度。LQR控制器则通过状态反馈实现最优控制，在保证跟踪精度的同时兼顾控制效率。这种组合特别适合用于控制算法的快速验证和教学演示。
2. 模型建立与参数设定
2.1 二自由度车辆模型推导
二自由度车辆模型又称"自行车模型"，它做了以下合理假设：
- 忽略悬架运动，认为车辆只在平面内运动
- 将左右侧轮胎合并处理（故称自行车模型）
- 假设纵向速度恒定，仅考虑横向和横摆运动
模型的状态方程可以表示为：
code复制ẋ = Ax + Bu
y = Cx + Du
其中状态变量x通常包括：
- 横向位置偏差（e1）
- 横向速度偏差（ė1）
- 横摆角偏差（e2）
- 横摆角速度偏差（ė2）
提示：实际建模时，建议采用相对路径的偏差量作为状态变量，而不是绝对位置，这样更利于控制器设计。
2.2 关键参数确定
模型需要设置以下核心参数：
- 车辆质量（m）和转动惯量（Iz）
- 前后轴距（a,b）
- 前后轮侧偏刚度（Cf, Cr）
- 纵向速度（Vx）
这些参数可以通过实车数据或参考同类车型获得。例如对于紧凑型轿车，典型参数可能为：
| 参数 | 值 | 单位 |
|---|---|---|
| m | 1200 | kg |
| Iz | 1600 | kg·m² |
| a | 1.2 | m |
| b | 1.3 | m |
| Cf | 60000 | N/rad |
| Cr | 60000 | N/rad |
| Vx | 15 | m/s |
3. LQR控制器设计
3.1 LQR基本原理
LQR（Linear Quadratic Regulator）是一种基于状态反馈的最优控制器，它通过最小化以下代价函数来求解最优控制律：
code复制J = ∫(x'Qx + u'Ru)dt
其中：
- Q：状态权重矩阵（调节跟踪精度）
- R：控制权重矩阵（调节控制量大小）
控制输入u通过状态反馈得到：
code复制u = -Kx
其中K为反馈增益矩阵，通过求解Riccati方程获得。
3.2 权重矩阵设计
Q和R的选择直接影响控制性能。我的经验是：
- 初始设置可令Q为对角阵，对角线元素对应各状态的重视程度
- R通常设为较小的标量（如0.1）
- 通过试错法调整，观察以下指标：
- 路径跟踪误差
- 前轮转角变化率
- 控制量大小
注意：过大的Q会导致控制量剧烈变化，过小的Q则跟踪精度不足。建议从以下初始值开始调试：
code复制Q = diag([10, 0.1, 5, 0.1])
R = 0.1
3.3 控制量限制处理
实际车辆的前轮转角存在物理限制（通常±30度）。在仿真中需要加入饱和限制：
python复制delta_max = np.deg2rad(30) # 最大转向角
u = np.clip(u, -delta_max, delta_max)
否则在高速度或急弯工况下，仿真结果会失真。
4. 双移线路径生成
4.1 标准双移线定义
双移线是ISO标准中定义的测试工况，由两个连续的S形弯道组成。其数学表达式可表示为：
code复制y_ref =
A1*sin(2π*x/L1) for 0 ≤ x < L1
A2*sin(2π*(x-L1)/L2 + π/2) for L1 ≤ x < L1+L2
其中A1、A2为幅值，L1、L2为波长。典型参数为：
- 总长：100m
- 车道宽度：3.5m
- 过渡段长度：25m
4.2 路径平滑处理
直接使用三角函数生成的路径曲率不连续，会导致控制困难。我推荐采用五次多项式进行平滑过渡：
python复制def smooth_transition(x, x0, x1, y0, y1):
dx = x1 - x0
t = (x - x0)/dx
if t <= 0: return y0
if t >= 1: return y1
return y0 + (y1-y0)*(10*t**3 - 15*t**4 + 6*t**5)
这种方法保证路径在连接点处的位置、角度和曲率都连续。
5. 仿真实现与结果分析
5.1 仿真框架搭建
建议采用以下仿真流程：
- 初始化模型参数和初始状态
- 在每个时间步：
a. 计算当前参考路径点
b. 计算状态偏差
c. LQR控制器计算控制量
d. 施加饱和限制
e. 更新车辆状态 - 记录数据并可视化
典型的时间步长取0.01-0.05秒。过大的步长会导致数值不稳定。
5.2 结果评估指标
除了直观的轨迹对比，建议量化以下指标：
- 最大横向偏差（Max Lateral Error）
- 平均横向偏差（RMSE）
- 前轮转角变化率（Steer Rate）
- 计算耗时（针对实时性评估）
下表是不同速度下的典型仿真结果：
| 速度(m/s) | 最大误差(m) | 平均误差(m) | 最大转角(deg) |
|---|---|---|---|
| 10 | 0.12 | 0.05 | 8.7 |
| 15 | 0.21 | 0.09 | 12.3 |
| 20 | 0.35 | 0.15 | 18.6 |
5.3 典型问题与调试技巧
在实际调试中，我遇到过以下典型问题及解决方法：
-
系统发散振荡
- 原因：Q矩阵中对速度项的权重过大
- 解决：降低ė1和ė2的权重系数
-
响应迟缓
- 原因：R值过大或Q中对位置项的权重不足
- 解决：增大Q中e1和e2的权重，或减小R值
-
转向角突变
- 原因：路径曲率不连续或控制频率过低
- 解决：检查路径平滑性，提高控制频率
经验分享：调试时建议先固定速度（如15m/s），从简单的直线跟踪开始，逐步过渡到双移线。同时建议绘制状态变量的时间历程曲线，比单纯看轨迹更能发现问题。
6. 扩展与改进方向
虽然这个基础模型已经能说明核心原理，但在实际项目中还需要考虑以下扩展：
-
考虑轮胎非线性
- 在极限工况下，轮胎力会进入非线性区
- 可引入Pacejka轮胎模型改进精度
-
增加纵向控制
- 将速度Vx也作为控制量
- 实现速度规划与跟踪
-
道路曲率前馈
- 在LQR基础上增加前馈项
- 提前补偿已知的路径曲率
-
参数自适应
- 针对不同车速自动调整LQR参数
- 或采用LTV（线性时变）方法
在实现这些扩展时，建议保持模块化设计，便于不同控制策略的对比测试。我在实际项目中发现，基础LQR虽然简单，但在中等速度下（<20m/s）已经能提供不错的控制效果，计算效率也极高，非常适合作为更复杂控制器的性能基准。