1. 自动驾驶横纵向控制概述
在自动驾驶系统的开发中,车辆控制是最关键的环节之一。横纵向控制负责将规划模块生成的轨迹转化为实际的车辆运动,其性能直接影响自动驾驶的安全性和舒适性。我最近在Carsim+Simulink联合仿真环境中实现了一套横纵向控制器,采用纵向PID+横向MPC的经典架构,基于二自由度车辆动力学模型开发,取得了不错的控制效果。
这套控制器的核心思路是:纵向控制采用双环PID结构,负责油门和刹车的精确控制;横向控制采用模型预测控制(MPC),负责方向盘的转向控制。这种组合既发挥了PID在简单控制任务中的稳定性和易用性,又利用了MPC在多变量约束优化中的优势。实测在双移线工况下,横向位置误差能控制在0.2米以内,纵向速度误差在±3km/h范围内,基本满足L2级自动驾驶的控制精度要求。
2. 车辆动力学模型基础
2.1 二自由度车辆模型
控制算法的设计离不开车辆动力学模型的支持。二自由度车辆模型(又称自行车模型)是自动驾驶控制中最常用的简化模型,它保留了横向和横摆两个自由度的运动特性,同时计算复杂度适中,非常适合实时控制。
模型的基本假设包括:
- 忽略悬架运动和轮胎的垂直载荷变化
- 假设左右轮胎特性相同,简化为单轮表示
- 小角度假设,适用于常规驾驶工况
模型的运动方程可以表示为:
code复制m(v̇y + vxφ̇) = Fyf + Fyr
Izφ̈ = aFyf - bFyr
其中,m为车辆质量,Iz为横摆转动惯量,vx和vy分别为纵向和横向速度,φ为横摆角,a和b为前后轴到质心的距离,Fyf和Fyr为前后轮胎的侧向力。
2.2 轮胎模型线性化
在小角度假设下,轮胎侧向力可以线性化为:
code复制Fyf = Cfαf
Fyr = Crαr
其中,Cf和Cr为前后轮胎的侧偏刚度,αf和αr为前后轮胎的侧偏角,可以进一步表示为:
code复制αf = δ - (vy + aφ̇)/vx
αr = -(vy - bφ̇)/vx
将轮胎模型代入运动方程,可以得到状态空间形式的二自由度模型,这是MPC控制器设计的基础。
3. 纵向PID控制器实现
3.1 双环控制结构
纵向控制采用了类似百度Apollo的双环PID结构:
- 外环:速度控制环,根据速度误差计算目标加速度
- 内环:加速度控制环,根据加速度误差计算油门/刹车指令
这种分层结构将复杂的纵向控制问题分解为两个相对简单的子问题,每层控制器只需关注单一目标,提高了系统的可调试性和鲁棒性。
3.2 油门刹车一体化设计
一个反直觉但非常实用的设计是使用同一个PID控制器同时控制油门和刹车。这种设计的优势在于:
- 避免了油门和刹车分别控制时可能出现的指令冲突
- 简化了控制逻辑,减少了参数调节的工作量
- 天然实现了油门刹车切换的平滑过渡
实现代码如下:
matlab复制function [throttle, brake] = fcn(target_accel, current_speed)
persistent pid;
if isempty(pid)
pid = pidController('Kp',0.8,'Ki',0.05,'Kd',0.02);
end
accel_error = target_accel - getActualAccel();
control = step(pid, accel_error);
if control > 0
throttle = saturate(control, 0, 1);
brake = 0;
else
throttle = 0;
brake = saturate(-control, 0, 1);
end
end
3.3 积分项处理技巧
在速度控制中,积分项(I项)对于消除稳态误差至关重要。这里采用了一种巧妙的处理方式:故意不做积分抗饱和处理。这是因为:
- 在稳态巡航时,积分项会自动收敛到平衡点
- 避免了复杂的抗饱和逻辑,简化了实现
- 实测表明,这种方法在匀速工况下能保持稳定的油门开度
例如,在30km/h匀速行驶时,油门开度能稳定在18%左右,与实际车辆数据吻合良好。
4. 横向MPC控制器设计
4.1 预测模型建立
基于二自由度模型,我们可以推导出MPC使用的状态空间模型。将模型在工作点附近线性化,得到状态矩阵A:
cpp复制A << 0, 1, 0, 0,
0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, -(a*Cf - b*Cr)/(m*vx),
0, 0, 0, 1,
0, -(a*Cf - b*Cr)/(Iz*vx), (a*Cf - b*Cr)/Iz, -(a²*Cf + b²*Cr)/(Iz*vx);
这个模型清晰地反映了车辆横向动力学特性,但需要注意一个关键问题:当车速vx趋近于零时,部分矩阵元素会趋向无穷大。为此,我们在实现中增加了低速保护逻辑,当车速低于0.5m/s时冻结横向控制器,等待纵向速度恢复后再重新激活。
4.2 权重矩阵调参
MPC控制器的性能很大程度上取决于权重矩阵的选择。经过大量仿真测试,我们发现以下权重配置能取得较好的平衡:
- 前轮转角权重:10
- 横向误差权重:500
- 横摆角误差权重:200
这种配置既能保证良好的轨迹跟踪精度,又能避免方向盘过于敏感导致的"抽风"现象。调参过程中,我们特别注意了不同速度下的控制效果,确保在全速度范围内都能稳定工作。
4.3 实时优化求解
MPC的核心是每个控制周期求解一个优化问题。我们采用如下形式的代价函数:
code复制min J = Σ( x'Qx + u'Ru ) + x_N'Px_N
s.t. x_k+1 = Ax_k + Bu_k
u_min ≤ u_k ≤ u_max
其中Q、R、P分别为状态、输入和终端状态的权重矩阵。在实际实现中,我们使用了QP求解器来高效求解这个优化问题,确保满足实时性要求(计算时间<10ms)。
5. 轨迹规划与处理
5.1 五次多项式轨迹
规划模块生成的参考轨迹采用五次多项式表示:
code复制y(x) = a0 + a1x + a2x² + a3x³ + a4x⁴ + a5x⁵
这种表示方法可以保证轨迹在位置、方向、曲率等多个层次上的平滑性,非常适合车辆跟踪。然而,实际应用中发现直接使用这种轨迹会导致两个问题:
- 曲率突变:在连接点处可能出现曲率的突然变化
- 高频抖动:轨迹的高阶导数可能引入不必要的方向盘抖动
5.2 低通滤波处理
为了解决上述问题,我们在规划层和控制器之间增加了一个低通滤波器,将曲率变化率限制在0.1m⁻¹/s以内。这种处理带来了明显的改善:
- 方向盘抖动显著减少
- 驾驶舒适性提高
- 执行器磨损降低
但同时也引入了一个约0.5m的跟踪滞后,这是在控制性能和乘坐舒适性之间做出的合理折衷。
6. 联合仿真实现
6.1 Carsim-Simulink接口配置
联合仿真中最具挑战性的部分之一是Carsim和Simulink的接口配置。以下是一些关键经验:
- 必须确保VS_Init函数在Initialize子函数中首先被调用
- 需要仔细检查单位系统的一致性(Carsim默认使用英制单位)
- 信号采样时间必须匹配,避免数据不同步
一个常见的错误是在S-Function中错误地调用carsim2simulink.dll,这会导致车辆状态全为NaN。我们在模型中添加了多个示波器监控关键信号,帮助快速定位和解决这类问题。
6.2 仿真环境搭建
我们的仿真环境配置如下:
- Matlab/Simulink版本:2021a
- Carsim版本:2019.0
- 车辆模型:中型轿车参数
- 测试工况:双移线、蛇形绕桩、恒定半径转弯
在搭建过程中,特别注意了以下几点:
- 确保车辆参数准确反映实际特性
- 合理设置路面摩擦系数(通常设为0.8-1.0)
- 配置适当的传感器噪声模型
7. 性能评估与改进方向
7.1 控制精度分析
在标准双移线工况下的测试结果显示:
- 横向位置误差:<0.2m
- 纵向速度误差:±3km/h
- 最大横向加速度:<0.3g
- 方向盘转角速率:<100°/s
这些指标表明控制器在常规工况下表现良好,但在极端工况(如低附着路面、紧急避障)下仍有改进空间。
7.2 抗干扰能力测试
为了评估控制器的鲁棒性,我们进行了以下测试:
- 在参考轨迹中添加10cm随机噪声
- 模拟GPS信号丢失
- 引入执行器延迟
测试发现当前控制器对轨迹噪声较为敏感,MPC会放大高频噪声导致车辆路径振荡。一个可能的改进方向是引入动态预瞄距离,根据车速和曲率实时调整控制视界。
7.3 未来优化方向
基于当前结果,我们确定了以下几个优化方向:
- 动态调整预瞄距离,提高抗干扰能力
- 引入自适应MPC,自动调整权重参数
- 增加轮胎非线性特性补偿
- 优化低速控制策略,解决转向死区问题
控制系统的开发永远是一个迭代优化的过程,需要在理论分析和实际测试之间不断循环,逐步提升性能。