基于MATLAB的平行与垂直泊车路径规划仿真实现

泊车路线规划这个方向，我前前后后折腾了不少版本。最早是在MATLAB里跑通了一个平行泊车的圆弧规划，后来又补上了垂直泊车工况，两个程序放在一起，配合一份简单的参考说明，整个流程算是完整了。这次就把项目里的核心思路、运动学模型、路径生成代码和踩过的坑一起整理出来，给正在做自动泊车（APA）算法验证、或者拿这个方向做课程设计和毕业设计的同学一点参考。

这套仿真的价值在于：你不需要一上来就碰实车，也不需要复杂的ROS和仿真环境，只要一台装好MATLAB的电脑，就能把“规划路径→车辆跟踪→碰撞检测→动画回放”这条链路完整跑通。平行泊车和垂直泊车是自动泊车里最典型的两类工况，覆盖了大部分城市停车场景，把这两个搞明白，后面扩展到斜列泊车只是改参数的事。

1. 为什么泊车路线规划选MATLAB仿真最省事

1.1 几何法与优化法怎么选，哪些场景适合

泊车路线规划的主流做法可以分成两类：一类是几何法，把路径看成圆弧和直线的组合；另一类是数值优化法，通过MPC、多项式曲线拟合等方式求解合理路径。

几何法的核心思想非常朴素——低速泊车时车辆运动可以简化为“圆弧段+直线段”的拼接，只要每段圆弧的半径大于等于车辆最小转弯半径，段与段之间保持相切，就能保证车辆可以沿路径行驶。这种做法的最大优点是可解释性强，每一段路径的几何含义都很清楚，调试时可以逐个段去分析位置、角度和曲率。

优化法虽然能处理更复杂的约束，比如动态障碍物、曲率连续、舒适性指标，但计算量更大，参数调起来也麻烦。对于一个以验证核心算法为目的的仿真项目，几何法完全够用。实际工程中很多APA系统也是先用几何法生成一条可行路径，再用优化法做平滑，并没有必要一上来就上重型算法。

我在这个项目里选择几何法，还有一个原因：平行泊车和垂直泊车的车位结构非常规整，物理边界清晰，圆弧直线的组合方式就能给出很自然的入库轨迹。与其花大量时间调优化目标，不如先跑通一条简单可靠的路径，再在这个基础上做扩展。

1.2 MATLAB在运动学仿真里的天然优势

用MATLAB做这件事，最大的感受是“所见即所得”。矩阵运算、复数坐标、二维绘图都是强项，路径规划过程中的圆心坐标、切点位置、车身包络矩形，全部可以通过plot、patch、fill这几个函数画出来，方便到不行。

另外，运动学模型的迭代本质上就是状态方程一步一步往后推，MATLAB的循环虽然效率比不上C，但仿真步长取的合适的话，几千步计算也是毫秒级完成，完全不影响体验。再加上调试时可以直接在命令行看中间变量，比在C++工程里打断点看矩阵值要顺手太多。

当然也有要注意的地方：MATLAB的坐标系是默认的xy平面，画图时注意x方向是车辆前进方向，y方向是侧向方向，车辆航向角定义为x轴正方向逆时针旋转的角度。这个约定如果和车辆工程里的定义混了，后面算圆心坐标时会绕很多弯路。

2. 先把车辆运动学模型和泊车参数搞明白

2.1 自行车模型的推导与最小转弯半径

路径规划不能脱离车辆运动学约束。低速泊车场景下，可以把车辆简化为自行车模型（单车模型），用后轴中心的位置来描述车辆运动。

设后轴中心坐标为(x, y)，航向角为θ，前轮转角为δ，轴距为L，车辆速度为v，则运动学方程为：

dx/dt = v * cos(θ) dy/dt = v * sin(θ) dθ/dt = v * tan(δ) / L

后轴中心的转弯半径和前轮转角的关系是：

R = L / tan(δ)

当δ取最大值时，R就是最小转弯半径R_min。这个参数是整个路径规划的硬约束，所有规划出的圆弧半径都不能小于它。

很多初学者会把坐标放在车辆重心，但泊车仿真用后轴中心更合适，因为运动学方程的形式最简洁，而且车辆矩形包络可以通过后轴中心坐标、轴距、前后悬和车宽计算出来，碰撞检测也方便。

我用的车辆参数如下：

最小转弯半径的实测值一般会比理论值略大，因为轮胎侧偏和转向系统结构会造成附加约束。仿真里我通常会把理论值乘以1.1作为规划用值，留出安全余量，防止车辆实际开的时候一把停不进去。

2.2 平行泊车与垂直泊车的关键尺寸设定

不同停车工况对车位尺寸的要求差别很大。平行泊车因为车辆要顺着道路方向滑入车位，车位长度是关键约束；垂直泊车主要看车位宽度和车道宽度。

平行车位我按标准车位设定：长6.2m，宽2.5m，车位旁边车道的宽度按5.5m取。车辆在车位外准备倒车的位置，离车位边界横向距离约为1.0m到1.2m，这个距离太小容易蹭到旁边车辆，太大会导致规划的圆弧无法收敛进车位。

垂直车位按长5.3m、宽2.4m、车道宽5.5m设定。车辆在车位正前方，车头朝向车位外，倒车入库所需的横向空间比平行泊车小，主要考验的是纵向位置的控制。

这些尺寸在程序里都做成常量，放在文件头部，方便改。仿真的时候把车位画出来，和规划的轨迹放在同一张图里，一眼就能看到车辆有没有超出边界。

3. 平行泊车路径规划：从几何解算到代码落地

3.1 平行泊车的三段式路径是怎么设计的

平行泊车路径规划的经典做法是“圆弧-直线-圆弧”三段式。核心思路是：先把车从起始位置开到车位前方合适的位置，然后通过一段大半径圆弧让车尾进入车位，中间用直线段调整姿态，最后再通过一段反向圆弧把车身摆正。

具体过程大致如下：

1. 从起始位置沿直线前进，让车尾超过车位前端边界一定距离。
2. 右转打满，车辆后退，沿半径为R的圆弧运动，车尾逐渐向车位内移动。
3. 车辆姿态达到某个中间角度后，回正方向沿直线后退，进一步深入车位。
4. 左转打满，车辆继续后退，沿反向圆弧运动，把车头拉回车位方向。
5. 车身基本平行于车位后，回正方向，直线微调至最终位置。

关键是要保证圆弧与直线段、圆弧与圆弧的连接点处切线方向一致，否则规划出来的路径虽然画起来好看，实际车辆却没法连续转向通过。在代码里，这一点的实现方式是：记录上一段结束时车辆的航向角，下一段起始时从该航向角继续推进。

平行泊车的几何解算，本质上是确定各段圆弧的圆心坐标和转过的圆心角。假设最小转弯半径为R，初始位置为(x0, y0)、航向角θ0=0，则向右转弯时圆心在车辆右侧：

C1 = (x0 + R * sin(θ0), y0 - R * cos(θ0))

车辆绕C1转过的角度取决于目标的中间姿态角。第二个圆弧的圆心同理，根据中间姿态角和目标位置反推。

实际代码我不会去手算每段的圆心角，而是用运动学模型迭代生成圆弧段。这样写代码更简单，也可以直接观察每一步车辆的位置变化，便于排查碰撞。

3.2 平行泊车核心实现代码与运行效果

这段代码是仿真的核心。先写一个通用的车辆运动学迭代函数：

function pose = updatePose(pose, v, delta, L, dt) % 自行车模型一步更新 % pose = [x; y; theta] pose(1) = pose(1) + v * cos(pose(3)) * dt; pose(2) = pose(2) + v * sin(pose(3)) * dt; pose(3) = pose(3) + (v / L) * tan(delta) * dt; end

再写平行泊车主程序：

% 平行泊车路径生成 L = 2.7; R_min = 4.5 * 1.1; % 留10%余量 delta_max = atan(L / R_min); % 初始位姿和目标位姿（后轴中心） startPose = [0; 3.0; 0]; % 车位外道路位置 targetPose = [5.5; -1.2; 0]; % 最终停在车位内，姿态水平 dt = 0.05; pose = startPose; traj = pose; v = -1.0; % 倒车速度 % 1. 直线倒车：让车尾落到合适位置 t1 = 1.0; for i = 1:round(t1/dt) pose = updatePose(pose, v, 0, L, dt); traj = [traj pose]; end % 2. 右转打满倒车：车尾进入车位 t2 = 4.2; for i = 1:round(t2/dt) pose = updatePose(pose, v, -delta_max, L, dt); traj = [traj pose]; end % 3. 回正直线倒车：调整横向位置 t3 = 0.8; for i = 1:round(t3/dt) pose = updatePose(pose, v, 0, L, dt); traj = [traj pose]; end % 4. 左转打满倒车：摆正车身 t4 = 4.0; for i = 1:round(t4/dt) pose = updatePose(pose, v, delta_max, L, dt); traj = [traj pose]; end % 5. 回正直线微调 t5 = 0.5; for i = 1:round(t5/dt) pose = updatePose(pose, v, 0, L, dt); traj = [traj pose]; end

这里的几个时间参数（t1到t5）不是拍脑袋定的，我是先按几何关系估算，再通过仿真迭代修正的。比如右转打满倒车4.2秒，估算依据是：速度1m/s，时间4.2s，路径弧长4.2m，圆弧半径约4.95m，对应圆心角约0.85弧度，也就是约49度。这个角度正好能让车尾进入车位并让车辆形成入库姿态。

仿真效果方面，我跑完这段程序后，把车辆矩形包络逐帧画出来，可以明显看到车辆从车位外部道路经圆弧滑入车位，姿态从水平转为倾斜再转回水平，最终稳稳停在车位中间。车身和车位边界之间的间距大约有0.2到0.3m，符合实际泊车时的容差需求。

4. 垂直泊车路径规划：坐标系变换和路径组合

4.1 垂直泊车和水平泊车规划差异

垂直泊车对路径的要求和平行泊车很不一样。平行泊车最重要的是纵向进给量要大，车辆需要沿着道路方向走很长的距离才能把自己送进侧向车位；垂直泊车则更多依赖横向摆動，车辆要在比较短的纵向距离内完成约90度的姿态变化。

另一个差异是进入车位的方向。平行泊车最终车身方向与道路平行，所以两个圆弧的方向是相反的，中间夹一个直线段；垂直泊车最终车身方向与道路垂直，路径规划本质上是一个“大角度转向”的过程，需要让车辆从道路方向平滑地转到车位方向，往往两个圆弧的转向方向相同，中间用直线或圆弧过渡。

垂直泊车的典型路径是“左转圆弧-直线-右转圆弧”结构。从起始位置开始，车辆略微前进或后退，然后左转打满倒车，使车头向车位外侧摆动、车尾对准车位入口；回正方向沿直线倒车，让车辆接近车位；最后右转打满倒车，把车身摆正进入车位。

和平行泊车相比，垂直泊车对起始位置的前后距离更敏感。起始位置离车位太近，车辆没有足够的空间完成姿态转变；太远，又会导致倒车距离过长。我调参时发现，起始位置在车位正前方约2倍车长的位置最合适，既能保证路径平滑，又不容易碰到车位对面的边界。

4.2 垂直泊车核心实现代码与运行效果

垂直泊车的主程序和水平泊车很类似，变化在于转向角序列不同。我把仿真代码整理成了相对独立的一个函数，方便复用：

function traj = planVerticalParking(startPose, targetPose, L, delta_max, dt) % 垂直泊车路径规划：圆弧-直线-圆弧 v = -0.8; pose = startPose; traj = pose; % 1. 左转打满倒车，让车尾转向车位 for i = 1:round(3.5/dt) pose = updatePose(pose, v, delta_max, L, dt); traj = [traj pose]; end % 2. 回正方向，直线倒车 for i = 1:round(2.0/dt) pose = updatePose(pose, v, 0, L, dt); traj = [traj pose]; end % 3. 右转打满倒车，摆正车身进入车位 for i = 1:round(3.0/dt) pose = updatePose(pose, v, -delta_max, L, dt); traj = [traj pose]; end % 4. 回正方向，直线微调 for i = 1:round(0.5/dt) pose = updatePose(pose, v, 0, L, dt); traj = [traj pose]; end end

垂直泊车的起始位置和后轴中心坐标需要仔细设计。我用的起始位姿是[0; 4.5; 0]，车位中心在[2.6; -1.2]，车位深度5.3m。仿真时把车辆包络画出来，能看到车辆先是左转摆角度，然后直线接近车位，最后右转一把滑入车位，整体过程一气呵成，没有多余的折返。

调参时要注意圆弧段的圆心角不能太小，否则车辆姿态转不过来，最后会斜停在车位里。判断方法是看最终航向角是否接近0，误差应该控制在0.02 rad以内。如果误差大，优先调整第二段直线倒车的时长，因为它决定了车辆进入车位的深度和姿态修正能力。

5. 碰撞检测和动画显示：仿真好不好看全看这里

5.1 用矩形包络描述车辆轮廓

光画出后轴中心的轨迹是不够的，车辆是一个有长有宽的矩形，泊车仿真必须考虑车身轮廓会不会碰到车位边界。我用四个角点描述车辆轮廓，以后轴中心坐标、航向角θ和车辆尺寸计算：

function corners = vehicleCorners(x, y, theta, Lf, Lr, W) % 返回车辆矩形四个角点坐标 [x1 x2 x3 x4; y1 y2 y3 y4] dx = [Lf, -Lr, -Lr, Lf]; dy = [W/2, W/2, -W/2, -W/2]; c = cos(theta); s = sin(theta); R = [c -s; s c]; corners = R * [dx; dy] + [x; y]; end

这里Lf是前悬，Lr是后悬，W是车宽。注意Lf和Lr不要搞反，不然车辆前后方向就反了，碰撞检测会失真。我最初写代码时就是把前悬和后悬的值填反了，导致明明轨迹看着没问题，碰撞检测却一直报错。

5.2 分离轴定理做碰撞检测

碰撞检测我用的方法是分离轴定理（SAT），原理很简单：如果两个凸多边形不相交，则一定存在一条分离轴，使得两个多边形在该轴上的投影不重叠。对于矩形和车位边界这种凸多边形场景，效率高且实现简单。

MATLAB里可以自己写一个简短版本：

function isCollision = checkCollision(cornersCar, parkingSpot) % parkingSpot是车位四角坐标 polygons = {cornersCar, parkingSpot}; for k = 1:2 poly = polygons{k}; for i = 1:size(poly, 2) j = mod(i, size(poly, 2)) + 1; edge = poly(:, j) - poly(:, i); axis = [-edge(2); edge(1)]; axis = axis / norm(axis); proj1 = axis' * cornersCar; proj2 = axis' * parkingSpot; if max(proj1) < min(proj2) || max(proj2) < min(proj1) isCollision = false; return; end end end isCollision = true; end

实际使用中，我还会在车位边界内侧加一层安全缓冲，也就是把碰撞检测用的车位矩形向外或向内偏置0.1m，这样车辆轨迹即使贴着边界过去，仿真也不会一帧一帧报告“碰撞”，减少误报。

5.3 动画刷新的几个小技巧

动画显示如果写不好，很容易卡顿，尤其是轨迹点几千个的时候。我的经验是不要在每一帧都重绘所有元素，而是把车位、道路、轨迹这些静态元素画一次，只在循环里更新车辆矩形的位置：

figure; hold on; % 画车位和道路（静态） fill(spot_x, spot_y, [0.9 0.9 0.9], 'EdgeColor', 'k'); plot(traj(1, :), traj(2, :), 'b--', 'LineWidth', 1.5); % 车辆矩形句柄（动态） hCar = fill(car_x, car_y, [0.2 0.4 0.8]); for i = 1:size(traj, 2) corners = vehicleCorners(traj(1, i), traj(2, i), traj(3, i), Lf, Lr, W); hCar.XData = corners(1, :); hCar.YData = corners(2, :); drawnow limitrate; end

drawnow limitrate比drawnow性能好很多，它限制了渲染频率，避免每一步都刷新屏幕造成卡顿。如果需要导出动图或视频，建议在循环里采集帧数据，最后一次性写入视频文件，不要边仿真边录制，会掉帧。

6. 泊车仿真常见问题与参数调试经验

6.1 规划失败、车位倒不进去怎么办

这个问题我在调试时遇到过很多次，原因大多数是起始位置或中间状态不对。平行泊车时，如果起始位置离车位横向距离太近，车辆圆弧轨迹的外摆空间不足，很容易在路径中途就撞到车位旁边已经停放的车辆。垂直泊车时，如果起始位置离车位太远，车辆需要很大的纵向行程才能完成入位，容易超出道路边界。

处理思路是：先检查车辆起始位置和车位边界的几何关系是否合理。平行泊车建议离车位边界横向距离保持在1.0m到1.5m之间，垂直泊车建议离车位入口3.0m到5.0m。如果还是不行，就要把规划段数增加，比如平行泊车从三段变成五段，给车辆更多姿态调整的自由度。

另一个常见原因是圆弧段比例失衡。如果第一段圆弧转过的角度过大，车辆会过早进入车位深处；如果过小，车辆无法形成足够的侧向偏移。我一般先画出后轴中心轨迹，观察轨迹的形态再调时间参数，而不是盲目改代码。

6.2 轨迹折返、抖动和曲率突变

轨迹出现折返，多半是转向序列设计有问题。比如平行泊车中间直线段的方向写反了，或者两段圆弧之间的过渡点航向角不一致。排查技巧是在关键切换点打印车辆的位姿，对比设计意图和实际状态。

抖动通常来自采样步长过大。运动学模型中，dt取0.05s一般够用，但如果你把速度设到2m/s甚至更高，单步位移会超过0.1m，车辆矩形包络看起来就会一顿一顿。把dt缩小到0.02s即可，代价是循环次数增加，但MATLAB处理起来依然轻松。

曲率突变的问题主要出在圆弧和直线连接的瞬间，前轮转角从-delta_max直接跳到0，对应曲率从1/R突变到0。对于纯路径规划仿真，这个影响不大；如果后续要接跟踪控制器，建议增加一段过渡圆弧或使用回旋曲线做平滑。

6.3 碰撞检测误报与安全余量

碰撞检测误报是最容易让人抓狂的。一种常见情况是车辆轮廓计算时后悬和前悬数值用反，导致车头车尾颠倒，明明没有碰撞却判断为碰撞。另一种是车位边界画的时候用的是外边界，但实际车辆是停进车位内部，应该用车位内边界做检测。

解决方法是把车辆轮廓和车位一起画出来，逐帧检查车辆矩形是否真的碰到边界。我调这个项目时，会特意在碰撞检测函数里加上断点，当检测到碰撞时停下来，打印当前的车辆轮廓和车位坐标，几秒钟就能看出问题在哪。

安全余量方面，建议在车位内边界向中心收缩0.1m作为碰撞检测边界。这样既能保证车辆不实际触边，又能容纳路径跟踪过程中的微小误差。

6.4 仿真动画卡顿和导出视频

动画卡顿主要原因是更新频率过高。drawnow每帧都强制渲染，几千步下来界面会明显变慢。我改成每5帧或每10帧刷新一次，视觉上基本无感，性能提升明显。

导出视频时使用VideoWriter：

writerObj = VideoWriter('parking_simulation.avi'); open(writerObj); for i = 1:size(traj, 2) % 更新车辆矩形 % ... writeFrame(writerObj, getframe(gcf)); end close(writerObj);

这样录出来的视频帧率稳定，适配后期汇报和文档演示。有一点要特别注意：getframe需要图形窗口可见，如果设置了figure('Visible', 'off')，录出来的视频会是一片空白，别问我怎么知道的。

从这套平行泊车和垂直泊车仿真里，我最大的体会是：泊车路线规划的核心不是公式推导有多复杂，而是能不能把几何约束、车辆尺寸约束完整地落到代码里。路径规划的每个参数，都值得回归到车辆实际尺寸和运动学极限去验证一遍。现在这套程序只能算基础版本，后续打算再扩展斜列泊车工况，加上回旋曲线做曲率平滑，再配合PID跟踪控制形成完整闭环。如果你也在做类似方向，希望这篇文章能帮你少踩几个坑。