小狐狸与小道士1. 平行泊车算法原理与MATLAB实现
平行泊车是自动驾驶技术中最具挑战性的场景之一。作为一名汽车电子工程师，我在开发ADAS系统时发现，理解泊车轨迹生成算法是掌握自动泊车技术的钥匙。今天就用MATLAB带大家拆解这个看似神奇的过程。
1.1 阿克曼转向几何模型
传统车辆转向采用阿克曼几何原理，这意味着：
- 内侧轮转向角大于外侧轮（通常相差2-3度）
- 所有车轮的延长线应交于一点，即瞬时转向中心
- 转向半径与转向角呈非线性关系
在MATLAB中建模时，我们简化处理为单轮模型，用以下公式计算转向半径：
matlab复制R = L / tan(δ) % L为轴距，δ为转向角
实际工程中，乘用车轴距通常在2.4-3.0米之间，最大转向角限制在30-35度（约0.52-0.61弧度）。过大的转向角会导致轮胎侧偏角超出线性区，影响控制精度。
1.2 双阶段泊车轨迹规划
典型的平行泊车分为两个阶段：
- 初始倒车阶段：方向盘向右打满倒车，使车尾切入车位
- 姿态调整阶段：方向盘向左回正，同时继续倒车完成泊入
在MATLAB实现中，我用两个连续圆弧轨迹来模拟这个过程。关键参数包括：
matlab复制phase1_distance = 3.2; % 第一阶段行驶距离
phase2_distance = 2.8; % 第二阶段行驶距离
phase2_steer_ratio = 0.8; % 第二阶段转向比例
实际调试中发现，第二阶段转向角取最大值的80%能更好避免转向过度。这个经验值来自实车测试数据，不同车型可能需要微调。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. MATLAB实现详解
2.1 车辆参数初始化
首先定义车辆基本参数，这些值需要根据目标车型调整：
matlab复制% 车辆机械参数
L = 2.5; % 轴距(m)
wheel_base = 1.5; % 轮距(m)
car_length = 4.3; % 车长(m)
max_steer = 30*pi/180; % 最大转向角(rad)
% 场景参数
start_pose = [0, 0, 0]; % 初始位姿[x,y,θ]
target_spot = [3.5, -4.2]; % 车位中心坐标
road_width = 6; % 道路宽度(m)
obstacle_gap = 0.5; % 与障碍物安全距离
2.2 轨迹生成函数
核心算法是calc_trajectory函数，它基于运动学模型计算圆弧路径：
matlab复制function [x,y,theta] = calc_trajectory(init_pose, steer_angle, distance)
if abs(steer_angle) < 1e-3 % 处理直行情况
x = init_pose(1) + linspace(0, distance*cos(init_pose(3)), 100);
y = init_pose(2) + linspace(0, distance*sin(init_pose(3)), 100);
theta = init_pose(3) * ones(1,100);
else
R = L / tan(steer_angle); % 转弯半径
omega = distance / R; % 转向角度变化量
theta = init_pose(3) + linspace(0, omega, 100);
x = init_pose(1) + R*(sin(theta) - sin(init_pose(3)));
y = init_pose(2) + R*(cos(init_pose(3)) - cos(theta));
end
end
特别注意：当转向角接近0时需要特殊处理，否则会出现除以零错误。这是实际项目中常见的边界条件问题。
2.3 轨迹拼接与可视化
生成两段轨迹并拼接：
matlab复制% 第一阶段：倒车右转
steer1 = -max_steer;
[x1,y1,theta1] = calc_trajectory(start_pose, steer1, phase1_distance);
% 第二阶段：倒车左转
steer2 = max_steer*phase2_steer_ratio;
[x2,y2,theta2] = calc_trajectory([x1(end),y1(end),theta1(end)],...
steer2, phase2_distance);
% 合并轨迹
x = [x1, x2]; y = [y1, y2]; theta = [theta1, theta2];
可视化时用旋转矩阵绘制车辆轮廓：
matlab复制for k = 1:10:length(x)
% 计算车身四个角点
corners = [-car_length/2, -wheel_base/2;
car_length/2, -wheel_base/2;
car_length/2, wheel_base/2;
-car_length/2, wheel_base/2];
% 应用旋转和平移
rot = [cos(theta(k)), -sin(theta(k));
sin(theta(k)), cos(theta(k))];
rotated_corners = (rot * corners')' + [x(k), y(k)];
% 绘制
patch(rotated_corners(:,1), rotated_corners(:,2), 'w', 'EdgeColor','b');
end
3. 工程实践中的关键问题
3.1 坐标系转换陷阱
MATLAB默认使用数学坐标系（y轴向上为正），而车辆工程常用：
- ISO坐标系：x向前，y向左，z向上
- SAE坐标系：x向前，y向右，z向下
这导致三个常见错误：
- 转向角方向混淆（左转为正还是负）
- 轨迹显示上下颠倒
- 传感器数据对齐错误
解决方案是建立统一的坐标转换函数：
matlab复制function [iso_x, iso_y] = math2iso(math_x, math_y)
iso_x = math_x;
iso_y = -math_y; % y轴反向
end
3.2 参数敏感性分析
通过蒙特卡洛仿真发现：
- 轴距误差影响最大 - 每10cm误差导致末端位置偏差约15cm
- 转向角精度要求高 - 1度误差产生约5cm偏差
- 车速波动影响较小（在低速范围内）
建议在实际系统中：
- 定期标定轴距参数
- 使用高精度转向角传感器
- 控制车速在5km/h以下
3.3 实时轨迹修正策略
纯几何方法在以下场景会失效：
- 车位被部分占用
- 初始位置存在偏差
- 有动态障碍物
改进方案是结合距离传感器数据实时调整：
matlab复制while norm([x(end),y(end)] - target_spot) > 0.2
% 获取当前传感器数据
[front_dist, rear_dist] = read_ultrasonic_sensors();
% 调整第二阶段轨迹
if rear_dist < safe_distance
phase2_distance = phase2_distance * 0.9;
regenerate_trajectory();
end
end
4. 完整代码优化建议
将上述模块整合后，建议进行以下优化：
- 参数配置文件化：
matlab复制% 保存为car_params.m
vehicle.L = 2.5;
vehicle.max_steer = deg2rad(30);
parking.target_size = [2.5, 5.0]; % 车位长宽
- 增加碰撞检测：
matlab复制function collision = check_collision(corners, obstacles)
for i = 1:size(obstacles,1)
if any(inpolygon(obstacles(i,1), obstacles(i,2), corners(:,1), corners(:,2)))
collision = true;
return;
end
end
collision = false;
end
- 可视化增强：
matlab复制% 绘制车位和道路标记
rectangle('Position',[target_spot(1)-1.25, target_spot(2)-2.5, 2.5, 5],...
'EdgeColor','g','LineWidth',2);
% 添加轨迹箭头
quiver(x(1:10:end), y(1:10:end),
cos(theta(1:10:end)), sin(theta(1:10:end)), 0.5, 'r');
这个MATLAB实现虽然简化，但包含了自动泊车系统的核心算法。在实际项目中，还需要考虑车辆动力学、执行器延迟、传感器噪声等因素。建议先用这个模型理解基本原理，再逐步增加复杂度。