1. 项目概述：基于纯跟踪的自动泊车系统开发
去年参与某园区无人驾驶接驳车项目时，我们遇到了一个棘手问题：传统PID控制在低速泊车场景下频繁出现转向振荡。经过多轮方案对比，最终采用纯跟踪（Pure Pursuit）算法作为横向控制核心，配合纵向PID控制，在CarSim仿真环境中实现了误差小于5cm的侧方停车效果。这套方案后来成功应用于实际项目，今天就把其中的技术细节和实战经验完整分享给大家。
纯跟踪算法的本质是让车辆像人眼观察一样，始终朝着前方某个"预瞄点"行驶。与模型预测控制（MPC）等复杂算法相比，其计算量小、参数直观的特点特别适合嵌入式设备部署。在自动泊车这类对实时性要求高、但速度较低的场景中，往往能获得意想不到的稳定表现。
2. 系统架构设计
2.1 整体控制框架
我们的系统采用经典的分层控制架构：
code复制[轨迹规划层] → [运动控制层] → [车辆执行层]
↑ ↑
[环境感知] [状态反馈]
运动控制层又细分为：
- 纵向控制：通过PID调节车速
- 横向控制：通过Pure Pursuit计算转向角
- 协调器：处理纵向/横向控制的耦合关系
关键设计原则：纵向控制优先级高于横向控制。当系统检测到可能碰撞时，会立即切断转向控制指令，优先保证制动安全。
2.2 仿真环境搭建
使用CarSim 2021.1搭建的侧方停车场景包含：
- 标准B级车参数（轴距2.7m，最小转弯半径5.2m）
- 6m×2.5m标准车位尺寸
- 包含路面摩擦系数变化区域（μ=0.3→0.7突变）
- 10%坡度变化测试段
仿真参数设置要点：
python复制# Carsim接口采样时间
sim_step = 0.01 # 100Hz更新率
# 车辆初始状态
init_pose = [0, 1.5, 0] # x,y,heading(rad)
# 目标车位坐标
parking_spot = [[5,0], [5,2.5], [11,2.5], [11,0]]
3. 核心算法实现
3.1 纵向PID控制优化
传统PID在低速工况下容易产生积分饱和问题。我们的改进方案：
python复制class ParkingPID:
def __init__(self, Kp=0.8, Ki=0.05, Kd=0.1):
# 抗积分饱和参数
self.integral_limit = 0.5
self.dead_zone = 0.02 # 2cm内不调节
def update(self, target_v, current_v):
error = target_v - current_v
if abs(error) < self.dead_zone:
return 0
# 积分项限幅
self.integral = np.clip(
self.integral + error,
-self.integral_limit,
self.integral_limit
)
# 微分项平滑处理
derivative = 0.6*self.prev_error + 0.4*error
output = self.Kp*error + self.Ki*self.integral + self.Kd*derivative
self.prev_error = error
return np.clip(output, -1, 1) # 归一化输出
参数整定经验：
- 先调Kp至系统开始振荡，然后取60%作为基准值
- Ki初始设为Kp/10，观察稳态误差
- Kd最后加入，用于抑制超调
3.2 纯跟踪算法深度解析
核心公式推导：
code复制前轮转角 δ = arctan(2L sin(α)/ld)
其中：
L - 车辆轴距
α - 当前航向与预瞄点方向夹角
ld - 预瞄距离（动态调节关键）
动态预瞄距离算法：
python复制def get_lookahead_distance(v, k=1.5, min_ld=1.0, max_ld=3.0):
"""
v: 当前车速(m/s)
k: 时间前瞻系数(典型值1.0-2.0)
"""
ld = k * v
return np.clip(ld, min_ld, max_ld)
轨迹匹配优化技巧：
- 使用三次样条插值平滑原始路径点
- 预瞄点搜索采用KD-Tree加速
- 增加曲率前馈补偿项：
python复制feedforward = math.atan2(L*kappa, 1) # kappa为路径曲率
4. Simulink模型详解
4.1 接口设计关键
模型包含三个核心子系统：
-
CarSim接口：处理车辆状态信息的输入输出
- 使用S-Function实现高速数据交换
- 注意单位统一（角度制/弧度制）
-
控制决策模块
- 有限状态机管理泊车流程
- 包含紧急制动逻辑
-
算法实现模块
- PID控制器采用抗饱和设计
- PurePursuit加入曲率补偿
4.2 参数调试技巧
-
先开环测试车辆动力学响应
- 给固定转向角观察轨迹
- 验证CarSim参数准确性
-
单独调试纵向PID
- 从阶跃响应观察超调量
- 注意低速时的积分累积
-
最后整定PurePursuit
- 预瞄距离与车速的比值系数k
- 曲率补偿权重因子
5. 典型问题排查指南
5.1 振荡问题
现象：车辆在接近目标时左右摇摆
- 检查预瞄距离是否过小
- 降低横向控制的响应速度
- 增加路径平滑处理
5.2 轨迹偏离
现象：车辆切弯或外抛
- 验证定位精度（特别是航向角）
- 检查轴距参数是否正确
- 尝试增加预瞄距离
5.3 响应延迟
现象：控制指令明显滞后
- 检查Simulink求解器步长
- 确认CarSim仿真步长匹配
- 优化算法计算耗时
实测发现当系统延迟超过200ms时，控制性能会急剧下降。建议通过CPU负载监控确保实时性。
6. 工程实践建议
-
传感器误差处理
- 对定位数据做卡尔曼滤波
- 设置合理的异常值剔除机制
-
控制模式切换
- 设计平滑的handover逻辑
- 建议采用渐变权重切换
-
实时性保障
- 算法循环周期≤50ms
- 使用RTOS或Xenomai实时补丁
-
安全冗余设计
- 增加看门狗定时器
- 实现心跳检测机制
这个项目让我深刻体会到，优秀的控制算法不在于复杂度，而在于对物理本质的把握。纯跟踪算法用简单的几何关系就解决了轨迹跟踪问题，这种"少即是多"的哲学值得每个控制工程师思考。最后分享一个调试心得：当算法表现异常时，不妨暂时抛开代码，用纸笔画出车辆运动几何关系，往往会有意想不到的发现。