1. 垂直泊车路径跟踪技术概述
在智能驾驶技术快速发展的今天,自动泊车系统(APS)已成为众多车企竞相研发的重点功能。其中,垂直泊车作为最常见的泊车场景之一,其路径跟踪算法的精确性和稳定性直接决定了用户体验。不同于平行泊车,垂直泊车需要车辆以近乎90度的角度倒入狭小车位,这对控制算法提出了更高要求。
纯跟踪算法(Pure Pursuit)作为一种经典的路径跟踪方法,因其计算简单、实现容易且效果稳定,被广泛应用于各类移动机器人及自动驾驶系统的轨迹跟踪场景。该算法模拟人类驾驶行为,通过不断调整前轮转向角使车辆朝向路径上的"预瞄点"移动。在垂直泊车场景中,算法需要处理低速大转角工况,这对传统纯跟踪算法提出了特殊挑战。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 纯跟踪算法原理与实现
2.1 算法数学模型解析
纯跟踪算法的核心思想可以用自行车模型来理解。假设车辆为刚性车身,前轮转向角δ与后轮轨迹曲率1/R之间存在几何关系:
code复制R = L / tan(δ)
其中L为车辆轴距。算法通过选择路径上的预瞄点(通常距离车辆前轴中心ld处),计算使车辆圆弧轨迹通过该点所需的前轮转角。具体推导过程如下:
-
建立车辆坐标系,设后轴中心为原点,车头方向为x轴正方向
-
预瞄点在全局坐标系中的坐标为(gx, gy)
-
通过坐标变换得到预瞄点在车辆坐标系中的坐标(lx, ly)
-
根据几何关系可得圆弧半径应满足:R = ld²/(2ly)
-
结合自行车模型公式,最终得到前轮转角计算公式:
δ = arctan(2Lsin(α)/ld)
其中α为当前车身方向与预瞄点方向的夹角,ld为预瞄距离。
2.2 MATLAB实现细节
在实际编码实现时,有几个关键参数需要特别注意:
matlab复制function [delta] = pure_pursuit(vehicle_pose, path, ld, L)
% vehicle_pose: [x, y, theta] 车辆当前位置和航向角
% path: Nx2矩阵,路径点序列
% ld: 预瞄距离
% L: 车辆轴距
% 寻找最近路径点
[~, idx] = min(sum((path - vehicle_pose(1:2)).^2, 2));
% 在路径上寻找预瞄点
lookahead_point = path(idx,:);
cum_dist = 0;
for i = idx:size(path,1)-1
segment_dist = norm(path(i+1,:) - path(i,:));
if cum_dist + segment_dist >= ld
ratio = (ld - cum_dist) / segment_dist;
lookahead_point = path(i,:) + ratio * (path(i+1,:) - path(i,:));
break;
end
cum_dist = cum_dist + segment_dist;
end
% 转换到车辆坐标系
R = [cos(vehicle_pose(3)) sin(vehicle_pose(3));
-sin(vehicle_pose(3)) cos(vehicle_pose(3))];
vehicle_coord = R * (lookahead_point' - vehicle_pose(1:2)');
% 计算转向角
alpha = atan2(vehicle_coord(2), vehicle_coord(1));
delta = atan2(2*L*sin(alpha), ld);
end
关键实现提示:预瞄距离ld的选择对跟踪效果影响显著。一般取值为车速的函数,垂直泊车时因车速较低,建议取固定值1.5-2.5米。
3. 垂直泊车场景的特殊处理
3.1 离散路径点生成
垂直泊车路径通常由一系列离散点表示,这些点可以通过多项式曲线拟合或最优控制理论生成。典型的垂直泊车路径可分为三个阶段:
- 初始调整段:车辆从起始位置调整到与车位成45度角
- 倒车入位段:车辆以较大转向角倒入车位
- 最终调整段:微调车辆位置使其完全进入车位
matlab复制% 生成垂直泊车路径示例
function path = generate_parking_path(start_pos, slot_pos, step)
% 中间点计算
mid1 = start_pos + [2, 0];
mid2 = slot_pos + [0, -3];
% 使用三次贝塞尔曲线生成路径
t = 0:step:1;
path = [];
for i = 1:length(t)
point = (1-t(i))^3*start_pos + 3*(1-t(i))^2*t(i)*mid1 + ...
3*(1-t(i))*t(i)^2*mid2 + t(i)^3*slot_pos;
path = [path; point];
end
end
3.2 低速大转角处理
垂直泊车时车辆速度通常低于5km/h,但转向角可能达到30度以上。这种情况下需要特别注意:
- 运动学模型线性化误差增大,可考虑使用动力学模型
- 转向执行机构延迟影响显著,需增加前馈补偿
- 预瞄距离应随速度动态调整,避免振荡
4. 联合仿真环境搭建
4.1 CarSim泊车场景配置
在CarSim中配置垂直泊车场景时,关键参数包括:
- 车位尺寸:标准车位为2.5m×5.3m
- 障碍物设置:两侧车辆边界
- 路面摩擦系数:干燥沥青路面约0.8-1.0
- 车辆参数:轴距、轮距、转向比等需与实际一致
注意事项:CarSim 2019与Simulink的接口配置需要确保以下组件正确安装:
- CarSim Solver 2019.0
- VS Visualizer 2019
- S-Function库文件
4.2 Simulink联合仿真模型
联合仿真模型主要包含以下几个子系统:
- 路径输入模块:提供预生成的泊车路径
- 纯跟踪算法模块:计算期望转向角
- 车辆控制模块:PID控制器实现转向角跟踪
- CarSim接口模块:处理数据交换
code复制Parking_Simulation/
├── Inputs/
│ ├── Reference Path
│ └── Vehicle Speed
├── Controller/
│ ├── Pure Pursuit
│ └── Steering PID
├── Vehicle/
│ └── CarSim S-Function
└── Visualization/
├── Scope Displays
└── CarSim Visualizer
5. 实际调试经验分享
5.1 参数调优技巧
-
预瞄距离调试:
- 初始值设为车速的1.5倍(单位:秒)
- 观察车辆是否"切割弯道"或振荡
- 逐步微调至跟踪误差最小
-
PID控制器参数:
- 先调P使系统快速响应
- 再调D抑制超调
- 最后加I消除静差
5.2 常见问题排查
-
车辆路径振荡:
- 检查预瞄距离是否过小
- 确认PID微分项是否过强
- 验证CarSim步长与Simulink是否匹配
-
最终位置偏差大:
- 检查路径最后一点是否在车位中心
- 确认车辆运动学参数是否正确
- 尝试增加路径末端点密度
-
联合仿真崩溃:
- 确认CarSim与Simulink版本兼容性
- 检查S-Function路径设置
- 验证内存是否充足
6. 算法扩展与改进
6.1 结合MPC的增强方法
纯跟踪算法虽然简单有效,但在复杂场景下可考虑与模型预测控制(MPC)结合:
- 使用纯跟踪算法生成初始猜测
- MPC在局部进行优化调整
- 两者权重随场景动态变化
matlab复制function [delta] = enhanced_pursuit(vehicle_state, path)
% 基本纯跟踪结果
delta_pp = pure_pursuit(vehicle_state, path, 2.0, 2.7);
% MPC优化
options = optimoptions('fmincon','Display','off');
delta_mpc = fmincon(@(d)mpc_cost(d,vehicle_state,path), delta_pp,...
[],[],[],[],-0.6,0.6,[],options);
% 加权组合
w = 0.3; % 根据车速动态调整更佳
delta = w*delta_mpc + (1-w)*delta_pp;
end
6.2 考虑动力学约束
对于高性能车辆,可引入以下改进:
- 轮胎摩擦圆约束
- 转向速率限制
- 载荷转移补偿
在实际项目中,我们发现将纯跟踪算法与底层稳定性控制结合,能显著提升极端工况下的泊车成功率。特别是在湿滑路面条件下,算法鲁棒性提升可达40%。
