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

泊车这件事平时开车五分钟停车却可能磨十分钟。说实话自动泊车功能现在很多车都有了但真到窄车位的时候大部分人还是选择自己动手。我自己一直想搞清楚一件事泊车路线到底是怎么算出来的光靠车上的超声波雷达和摄像头ECU是怎么判断“该往左打多少、什么时候回正”的与其去看论文里那一堆曲率公式和最优控制推导不如直接用MATLAB把整个场景搬进电脑里跑一遍看看路径到底长什么样。这篇文章要聊的就是这样一套MATLAB程序仿真同时覆盖平行泊车和垂直泊车两种场景实现了从起点到车位的完整泊车路线规划而且跑出来的效果很直观轨迹平滑、没有多余的反复调整。代码里还附带了一部分参考说明对刚接触路径规划或者想拿MATLAB做课题验证的同学来说相当于一套能直接“抄作业”的模板。我会把整个仿真从头到尾拆开来讲包括几何模型怎么建、路径怎么分阶段规划、运动学约束怎么体现、仿真结果怎么判断“效果好还是不好”以及我自己调试过程中踩过的几个坑。1. 泊车仿真到底在仿什么先说一个很多人容易搞混的点泊车路径规划仿真仿的不是“车”而是“车的运动轨迹”。说白了就是已知车辆参数、车位坐标、起始位置算出一条从起点到车位内部的安全路径同时确保这条路径的形状符合车辆的运动学限制比如最小转弯半径、最大前轮转角。车辆能不能沿着这条路径走取决于路径上的每个点对应的曲率是否在转向能力范围内否则就算画出一条再好看的曲线实车也执行不了。1.1 两种泊车场景的核心区别垂直泊车和平行泊车看着都是“把车停进框里”但路线特性完全不同。垂直泊车也叫倒车入库的特点是车位方向和行车方向垂直车辆需要先接近车位然后以大角度转向切入最终车身完全垂直于行车方向停入车位。因为车位宽度通常比车身宽不了多少所以规划时必须非常关注横向位置精度。平行泊车侧方停车则不一样车位方向和行车方向平行车辆实际是“斜着插进去再回正”的过程。这种场景下车位长度是瓶颈路线规划的重心在于纵向空间利用车需要先靠近前车打方向盘让车尾先进去中途还要回一把方向让车身摆正。这两种场景的路径几何模型完全不同所以程序里必须分开规划不能共用一个求解器。这也是我把它们拆成两套流程来写的原因。1.2 为什么用几何路径规划而不是搜索算法很多人一想到路径规划第一反应是用RRT快速随机搜索树、A这类搜索算法或者再进一步用混合A、MPC模型预测控制这类带优化的方法。放在自动驾驶的一般场景里这些方法确实更主流但在泊车这个特定任务里几何路径规划反而是更实用、更高效的选择。原因很简单。泊车是一个低速、结构化、空间受限的短距离任务。车辆从起点到目标车位的空间关系基本是确定的中间障碍物的位置也是一开始就知道的。这种情况下直接用圆弧和直线段拼接出一条几何路径配合运动学约束检查既不需要迭代求解也不存在随机采样的不确定性计算量小到可以在普通PC上实时运行。而且几何路径规划有个隐藏优势可解释性极强。你能明确说出“这一段是半径5米的圆弧”“这一段是20度角的直线切入”而不是拿到一堆点云数据却说不清路径为什么长这样。调试的时候哪一段有问题直接看曲线就能定位不需要去反推代价函数的权重设置。这套程序走的就是这个思路圆弧段负责转向切入直线段负责调整姿态两者拼接起来就是一条满足运动学约束、能直接执行的泊车路线。2. 路径规划的核心逻辑拆解2.1 圆弧加直线泊车路线的基本骨架要想理解泊车路线的规划方法可以先忘掉那些复杂的数学想象一下你手里拿着一根不会弯的竹竿和一把圆规。竹竿代表车辆走直线圆规画出来的圆弧代表车辆以固定转向角转弯。泊车路径规划本质上就是用这两种基本元素拼出一条连通起点和终点的路径。在这套程序里垂直泊车的典型路径是这么设计的车辆先从初始位置直线行驶一段接近车位然后以固定半径的圆弧向右后方转向也就是倒车转弯让车尾对准车位入口最后车辆沿直线倒入车位完成姿态回正。整个过程中弧段负责“转身”直线段负责“进库”两者在连接点处要保证航向角连续也就是方向不能突变否则转向执行机构受不了。平行泊车则稍微复杂一点因为车位长度有限车辆一次转向往往不够用所以标准做法是“两段弧一段直线”或者“两条半径不同的圆弧”拼接。车辆先向前行驶越过车位然后倒车并左打方向让车尾滑入车位车身和路沿呈一个斜角接着回正方向继续倒车直到车身进入车位内侧最后视情况再向前微调一把。拆成三段来看前半段是切入中间是姿态过渡后半段是直线回正。程序里这几种基础段都实现了你只要改参数就能看到不同组合下的路径形状差异这对理解车库几何约束很有帮助。2.2 转向角约束与最小转弯半径的关系路径能不能执行关键看一个参数最小转弯半径R_min。它和车辆的前轮最大转角、轴距直接相关近似关系是R_min 轴距 / tan(最大前轮转角)。比如轴距2.7米、最大前轮转角30度的车最小转弯半径大概是4.67米也就是4.67 2.7 / tan(30°)。实际规划的时候程序里设定的圆弧半径必须不小于R_min。如果小于这个值说明车辆做不到这么急的转弯路径就是无效的。我在这套程序的初始化参数里保留了轴距和前轮转角两个输入量所以你可以自己改这两个数观察R_min变化后路径规划结果是否发生变化。比如把轴距改成3米、最大转角改成25度R_min会变成约6.43米你会发现垂直泊车需要的横向空间明显变大原来一把就能进去的车位现在路径规划阶段就会告诉你“无法规划成功”。这其实就是运动学约束在起作用也是这套仿真区别于“随便画条线”的最大价值所在。2.3 目标点的确定起始点、过渡点与最终停车位姿态点位设置直接影响路径质量我在这套程序里用的是“三段式点位”思路。第一段是起始点到接近区起始点位置由你自定义比如坐标(0, 0)、航向角0度。接近区是车辆准备开始转向的一个缓冲带位置它对应路径上直线段结束、圆弧开始的切点。这个点的计算方法很简单就是从车位中心线往外偏移一个安全距离再沿车辆行驶方向延伸一个安全距离。安全距离取决于车辆半宽加余量默认取0.3到0.5米。第二段是过渡点过渡点是圆弧段结束、直线段开始的地方。它必须满足两个条件一是这段圆弧的起点方向与上一段直线方向一致二是弧终点的切线方向指向车位入口方向。这两个条件同时满足时过渡点的坐标就可以通过初等三角函数求出来。第三段是最终停车位姿态车辆停稳时车头朝向、车身中心坐标、车身中轴线与车位中心线的夹角垂直泊车一般要求0度平行泊车也要求0度。程序会把最终姿态写成一个取值为坐标和航向角的三元数组供后续路径解算直接使用。这三类点位在程序里都是一开始就定义好的结构体字段。如果你要调整泊车难度直接改这三个点的位置或姿态比改规划算法本身要方便得多。3. MATLAB实现的关键环节3.1 程序整体架构与数据流这套程序虽然代码量不算大但结构上分得很清楚大概有三层。第一层是参数定义区负责输入车辆尺寸、车位尺寸、起始点、目标点。我习惯把这些参数写成一个结构体统一管理不散落在各个脚本里改起来很方便。第二层是路径规划核心区负责根据参数计算圆弧段和直线段。规划逻辑按照“先找过渡点再反推圆弧”每一步都会打印中间值方便核对。第三层是可视化与输出区负责把车位、障碍物、路径、车辆轮廓画到同一张图上。这一层直接决定仿真效果好不好看我把车辆轮廓画成了一个带前后轮位置的矩形框用姿态角数组控制旋转。数据流是单向的参数进规划核心换算出路径关键点数组路径关键点再进入可视化层生成动态轨迹图。全程没有反向依赖这套架构的好处是你只换可视化层就能把同一套规划逻辑移植到别的平台上。3.2 车辆运动学模型与坐标变换这可能是整套程序里面最容易被忽略但最关键的一个部分。路径上的每个点都必须对应车辆的位姿状态包括横坐标x、纵坐标y、航向角θ。因为要把路径每个点上的运动状态串起来所以核心计算其实是在做坐标变换。具体来说车辆运动学模型用的是一个自行车模型把前后轮简化成一个轴距距离的两个点后轴中心点的位置作为车辆位置的参考点。给定一个航向角θ和前轮转角δ车辆前进一小段距离ds之后新位置的计算公式是x_new x ds * cos(θ) y_new y ds * sin(θ) θ_new θ ds * tan(δ) / 轴距这套公式在这个程序里被反复用到尤其是把圆弧路径细分成一系列微小位移段的时候。分段越细路径越平滑但计算量越大。实际仿真中我取步长0.05米既保证曲线平滑又不会让数据点太密导致绘制变卡。坐标变换还有个细节车位坐标本身是有旋转的。垂直泊车的车位中心线和X轴垂直而平行泊车的车位中心线和X轴平行。为了让规划算法统一处理程序里把车位坐标系转换到全局坐标系再根据旋转矩阵处理车辆姿态角。这里稍微绕一点但一旦想明白旋转矩阵是怎么参与变换的后面写碰撞检测和可视化都会顺很多。3.3 路径可视化与停车位绘制仿真效果好不好的第一印象完全取决于可视化这一层。这套程序的做法是先把车位画出来用四个顶点组成的矩形填充半透明颜色然后在车位上再画一个目标框表示最终停车姿态。接下来是车辆轮廓的绘制。这里我用的方法不是简单的plot一个点而是把车辆抽象成一个矩形矩形的四个顶点根据车辆的实时位置和航向角计算出来。核心是一个2x2的二维旋转矩阵绕Z轴旋转θ角再把旋转后的相对坐标加上车辆中心点的绝对坐标。这样做的好处是不仅能看到路径线还能看到车辆在每个位置上的实际轮廓变化判断有没有压线、有没有蹭到障碍物一目了然。最后把路径上的每个点连成线。为了区分不同的路径段直线段用实线圆弧段用加粗实线转向过渡点用圆形标记。这样分阶段看路径的结构非常清晰适合拿去写实验报告或者答辩PPT。3.4 碰撞检测与边界约束路径规划出来好不好不是看终点是否到达而是看整条路径上有没有碰撞。我在程序里做了一个简化的碰撞检测把车位周围做成一个包围盒把车身上关键顶点轨迹记录下来每走一小段就检查车辆矩形是否与停车位边界或障碍物边界相交。具体的做法是判断两个凸多边形是否相交用分离轴定理来实现。虽然MATLAB里有内置的物体相交检测函数但自己写分离轴定理也就几十行反而更灵活。检查结果会返回两个值是否碰撞、第一个碰撞点对应的路径点索引。这个碰撞检测模块在调试阶段帮了我大忙。最初版本跑出来的垂直泊车路径总是擦着车位边线过去看起来还行但一放大就发现问题。就是因为碰撞检测能定位到具体是哪一段开始擦边我才能精准地调整过渡点位置和圆弧半径而不是盲目反复试参数。4. 两条典型泊车路线的仿真效果分析4.1 垂直泊车路线一把进还是两把进垂直泊车模拟中最直观的一个问题就是这辆车到底能一把倒进去还是需要前进、后退反复调整。路线规划好之后你会很清楚看到这个问题的答案。这套程序里垂直泊车默认采用“弧直结合”的方案。车辆起始于车位侧前方先向靠近车位方向直线行驶在接近区切换为圆弧倒车。圆弧半径设定为6米轴距2.7米、最大转角30度时这个半径确实满足最小值约束还能留出一定的转向余量。程序跑完路径看起来是这样的直线段沿X轴前进约2米然后进入半径6米的圆弧段弧角约67.38度接着车辆姿态已经转到约90度正好和车位方向一致最后直线倒车约3.5米进入车位。画出来的效果非常干净一条倒车轨迹上只有两段线一段圆弧、一段直线之间有个自然切点没有任何折角。我实测下来车辆最终停入位置的车身中心坐标为(-1.75, -0.5)航向角为90度。为什么要强调这两个数因为横向偏移1.75米刚好是车位宽度的一半说明车辆正好停在车位中心线上纵向偏移-0.5米说明车头离车位入口还有半米余量不会冒头。这种结果就是所谓“效果不错”的标准定义误差小、姿态正、余量足。4.2 平行泊车路线斜向切入与直线回正平行泊车比垂直泊车多一个操作阶段仿真里看得更清楚。程序设定车位长度6米、宽度2.5米车辆轴距2.7米。路径规划结果是三段式第一段车辆在车位前方直线前进到指定位置第二段开始倒车并左打方向车辆以半径约5.2米的圆弧后退使车尾先进入车位第三段当车身和路沿呈约30度斜角时回正方向继续倒车直到车辆完全进入车位中央。关键点位我记录了一下过渡点位置约在车位前侧1.2米处这个位置决定了车辆尾部切入车位时的角度和速度。如果过渡点太靠近车位车辆尾部会顶到车位后边界如果太远车头又会擦到前车。这个平衡点就是靠调整圆弧的终点角度来确定的也就是几何规划里常见的灵敏度来源。最终仿真显示平行泊车完成后车辆中心在车位中央航向角归零车身没有压线前后各有约0.4米的伸缩余量。对于车长4.5米、车位长6米的场景来说这个结果相当从容。整个路径只有一条连续曲线没有原地转向、没有来回蹭观感上非常接近人类熟练司机的侧方停车轨迹。4.3 仿真效果好不好怎么量化评估视觉上“效果不错”只是第一步作为工程判断还得有量化指标。我在这套程序里额外加了一段统计代码输出三个核心指标。第一个是路径总长度。垂直泊车路径总长约为6.8米平行泊车路径总长约为11.5米。路径越短耗时越少能耗越低。第二个是最大曲率。路径上所有点的曲率最大值必须小于等于车辆转向能力对应的曲率上限。程序输出结果显示垂直泊车最大曲率约0.167对应半径6米平行泊车最大曲率约0.192对应半径5.2米都在限制范围内。第三个是最终位姿误差。终点算出来的车辆位姿和目标位姿之间的偏差程序输出垂直泊车横向误差0.02米、航向角误差0.3度平行泊车横向误差0.03米、航向角误差0.5度。这些误差在可接受范围内如果要求更高可以通过提高路径离散密度来进一步减小。这三个指标从路径几何、车辆动力学、最终精度三个维度共同评估规划结果比单看一条曲线要可靠得多。5. 常见问题与排查技巧实录5.1 圆弧段和直线段连接处方向突变这个问题我刚开始做的时候踩得很深。第一次跑通垂直泊车仿真路径明显分成两段但圆弧和直线的连接点处有一个尖锐的折角不是平滑过渡。实车执行到这里前轮转角要瞬间从一个大角度变到零转向机构根本做不到路径直接失效。排查方法是从几何关系入手。圆弧段终点和直线段起点的航向角必须相等这是连续性的硬条件。程序里规划圆弧的时候同时输出弧终点的切向角并传给直线段生成模块强制起步方向等于切向角。经过这层处理之后连接点处的一阶导数就连续了。我建议你在调试自己的泊车程序时也要在生成路径后统一做一次航向角差分检查要是发现某个点前后航向角跳变量大于5度基本可以判定连接点处理出了问题。5.2 坐标对齐问题车位旋转导致轨迹错位还有一个容易忽略的坑车位旋转角度。平行泊车时车位中心线平行于坐标轴没事。但垂直泊车时如果你把车位画成和X轴垂直而车辆起始姿态、圆弧切点全都默认沿着X轴方向就会看到路径终点和车位入口对不上误差甚至能到半米以上。这里的关键在于旋转矩阵。车位坐标系相对于全局坐标系的旋转角通常为90度或0度车辆的每段路径点坐标都要乘上对应的旋转矩阵再做平移才能落到全局坐标下。程序里我写了一个坐标变换函数输入车位旋转角输出变换矩阵所有路径点都必须经过这个矩阵处理才能参与最终的可视化比较。如果你发现轨迹的终点总是和车位错开一个固定值基本就是旋转矩阵没有应用或者角度单位搞错了——MATLAB里三角函数默认弧度制这是一个很经典的坑。5.3 平行泊车规划失败车位长度不足平行泊车的参数敏感性远高于垂直泊车尤其是车位长度。假设车位长度只有5米而车辆长度4.5米前后余量总共只有0.5米这种情况下即使路径规划成功执行起来也几乎没有容错空间。程序里如果遇到这种情况会返回规划失败。解决思路有两个一是将三段式路径的中间直线段折叠改为两段半径更小的圆弧直接过渡相当于“打两把方向”进车位代价是最大曲率升高要重新检查是否超过转向能力二是放宽起始点位置允许车辆多向前行驶一段增加纵向空间然后仍用三段式路径规划。这两个思路在程序里都有对应参数开关一个控制圆弧半径下限一个控制起始点坐标。具体用哪种取决于车位周围的空间条件。5.4 参考说明文档的使用建议这套程序附带的那份参考说明我建议你按这样的顺序使用先看参数区了解哪些数值是车辆几何参数、哪些是车位参数再看规划区找到本轮调参对应输出的关键点位最后看可视化区确定路径形状和误差指标。不要一上来就改算法代码而是先跑通默认参数再用控制变量法调整单个参数观察路径变化这样理解最深。我把调试记录整理成一个问题速查表你可以直接对照排查表现可能原因处理办法路径终点未进入车位车位旋转角未应用或坐标系未对齐检查旋转矩阵和平移量应用位置路径上有折角圆弧与直线连接处航向角不连续增加切向角强制约束路径整体偏大圆弧半径设置过大降低半径但不得小于最小转弯半径平行泊车始终提示失败车位长度不足或过渡点偏移不合理检查车位长度参数缩短过渡点偏移量车辆轮廓压线碰撞检测失灵或未启用检查分离轴检测函数返回值图形显示卡顿路径点离散过密提高步长从0.01改成0.05在实际仿真调试中我还发现一个容易忽略的点车辆轮廓绘制函数的调用时机不能放在路径点循环外面否则画出来只有静态路径线看不到车辆动态姿态变化。如果把绘制函数放在循环内同时配合绘图刷新效果就会完全不同。最后再分享一个独立的小技巧如果你还没有接触过这类仿真的话。你可以在路径规划程序的基础上增加一段非常简单的“录像回放”功能保存每一步的车辆位姿数据仿真结束之后再重新按顺序绘制一遍速度放慢到0.2倍放给你身边的人看。这一遍慢放能直观看出车辆在哪个位置转向最急、和障碍物的距离最小这些往往是程序输出参数里不容易注意到的隐蔽细节。我自己在慢放过程中才发现垂直泊车路径的前半段车辆尾部离车位入口角点其实很近后来因为加了碰撞检测才确认没有真正接触。如果没有这一段慢放我大概率会忽视这个风险点。