自动泊车路径规划：从车辆运动学建模到RRT*与最优控制算法实践-尧图网络科技

1. 赛题回顾与核心问题拆解

2022年中青杯数学建模竞赛的C题，题目是“自动泊车问题”。这个题目一出来，当时就在我们几个建模老手的小群里炸开了锅。为啥？因为它太“接地气”了。不像一些纯理论推导或者数据预测的题目，泊车问题是每个开车的人，甚至每个关注自动驾驶技术的人都能直观感受到的。题目要求我们建立一个模型，来模拟和优化自动泊车的路径规划与控制策略。简单来说，就是给你一辆车的尺寸、一个停车位的尺寸和位置，以及车辆初始的位姿（位置和朝向），你需要设计一套算法，让这辆车能安全、高效、平稳地停进去，不能撞到车位线，也不能刮蹭到旁边的车（题目中可能以虚拟障碍物形式给出）。

听起来是不是有点像我们考驾照时的“倒车入库”？但建模竞赛的要求可比驾考复杂和深刻得多。驾考你只需要记住几个“点”，打几圈方向盘，凭感觉和经验就能过。但数学建模要求你把这一切“感觉”和“经验”全部量化、公式化，变成一个可以被计算机理解和执行的精确数学模型。这背后涉及的核心知识板块非常明确：运动学建模、路径规划算法、优化理论，以及控制策略。题目通常会提供车辆的最小转弯半径、轴距、轮距等参数，这就是在引导你建立车辆的运动学模型。而“安全高效”的要求，则直接指向了路径规划中的最优控制问题，比如如何找到一条从起点到终点的无碰撞路径，并且这条路径还要满足车辆运动学约束（不是你想怎么拐就能怎么拐），同时可能还要优化时间、能耗或者方向盘的转动幅度。

所以，面对这道题，第一步绝不是急着打开MATLAB或者Python开始敲代码。而是必须静下心来，把题目中每一句话、每一个参数、每一个要求都“翻译”成数学语言。比如，“安全”意味着路径上的每一个点，车辆的外轮廓（通常简化为矩形）都必须完全在停车位的边界线之内，且与任何障碍物（可能是相邻车位的模拟）保持安全距离。这本质上是一个几何约束问题。“高效”可能意味着路径长度最短，或者完成泊车动作的时间最少，或者方向盘的累计转向角度最小，这构成了目标函数。而车辆如何从A点移动到B点，则受微分方程（运动学方程）的约束。这样一来，一个复杂的工程问题，就被清晰地拆解成了在微分方程和几何约束下，寻找某个目标函数最优解的标准数学优化问题。这个拆解过程，是解题成功的一半。

2. 模型构建的核心：车辆运动学与几何约束

要解决自动泊车问题，首先得让你的“模型车”动起来，并且是按照真实汽车的物理规律来动。这里我们通常采用自行车模型来进行运动学建模。这是车辆动力学中一个非常经典且实用的简化模型。

为什么叫自行车模型？因为它把四个轮子的汽车，简化成了前后两个轮子的自行车。假设两个前轮（转向轮）合并为一个，位于车辆前轴中心；两个后轮（驱动轮）合并为一个，位于车辆后轴中心。这个简化虽然忽略了车辆的侧滑等复杂动力学特性，但对于低速（泊车典型场景）下的路径规划来说，精度完全足够，且能极大地降低模型复杂度。

在这个模型下，我们定义几个关键参数：

轴距 (L)：前后轮轴之间的距离，题目一般会给出。
前轮转角 (δ)：这是我们能控制的主要输入量，方向盘的转动直接对应这个角度的变化。它有一个最大值限制，对应方向盘打死的情况，这决定了车辆的最小转弯半径 (R_min)。关系是：R_min = L / tan(δ_max)。
车辆位姿 (x, y, θ)：通常以后轴中心点作为车辆的参考点。(x, y)是该点的坐标，θ是车辆的朝向角（车身纵轴与X轴的夹角）。

那么，车辆的运动学微分方程可以描述为：

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

其中，v是车辆后轴中心点的速度（假设后轮驱动）。在纯粹的路径规划阶段，我们有时会进一步简化，假设速度v是恒定的（比如设为1），那么问题就聚焦在如何控制前轮转角δ上。

注意：这里有一个非常重要的细节处理。在仿真中，我们通常对时间t进行离散化，采用欧拉法进行数值积分。例如，设定一个很小的时间步长dt（如0.1秒），那么车辆的位姿更新公式为：
x_{k+1} = x_k + v * cos(θ_k) * dt y_{k+1} = y_k + v * sin(θ_k) * dt θ_{k+1} = θ_k + (v / L) * tan(δ_k) * dt
这个离散化的过程，就是把连续的微分方程模型，转化为计算机可以迭代计算的离散模型，是后续所有仿真和优化的基础。

接下来是几何约束，也就是确保不撞墙、不压线。我们需要判断车辆在每一个位姿下是否与障碍物发生碰撞。通常将车辆简化为一个矩形。那么，如何判断一个旋转的矩形是否在一个多边形（车位）内部，且与其它矩形（障碍物）无重叠呢？

车辆轮廓计算：已知后轴中心点(x, y)、车长L_car、车宽W_car和朝向角θ，可以计算出车辆四个角点的坐标。这涉及基本的几何旋转和平移运算。
碰撞检测：
- 与车位边界的碰撞：可以判断车辆的四个角点是否都在车位多边形内部。对于矩形车位，这等价于判断每个角点的坐标是否在[x_min, x_max]和[y_min, y_max]的区间内，并考虑一个安全裕量epsilon（比如0.1米）。
- 与静态障碍物的碰撞：如果题目给出了其他停车车辆作为障碍物，同样简化为矩形。那么问题就转化为判断两个旋转矩形是否相交。一个高效的方法是采用分离轴定理。简单来说，如果能找到一条直线（轴），使得两个矩形在该直线上的投影不重叠，那么它们就没有碰撞。对于矩形，只需要检查四条边所在的方向轴即可。
- 自我碰撞约束：在规划路径时，车辆自身不能出现过于极端的姿态，比如转角δ需要限制在[-δ_max, δ_max]之间，有时角速度dθ/dt也需要平滑性约束，以避免规划出的路径理论上可行但实际控制无法跟踪。

把这些运动学方程和几何约束用数学公式和代码清晰地表达出来，你的模型就有了“身体”和“活动边界”。这是整个工作的基石，后续的所有优化算法都是在这个框架内寻找最优的“动作序列”。

3. 路径规划算法选型与对比

有了模型，下一步就是寻找一条从起点(x0, y0, θ0)到终点(xf, yf, θf)（终点通常是车位中心，车头朝里）的可行路径。这里的“可行”指满足上述所有运动学和几何约束。如果还想“最优”，就需要定义一个评价函数。这是本题最核心、最体现建模功力的部分。当时我们团队主要评估了三种主流思路：

3.1 基于几何的预设路径方法

这是一种非常直观的思路。观察人类驾驶员倒库，无非是“直行-打满方向倒车-回正调整”几个阶段的组合。我们可以用几种基本的曲线来拼接成完整路径：

直线段：对应方向盘回正，直行或倒车。
圆弧段：对应方向盘打满，车辆以最小转弯半径行驶。
回旋曲线段：如Dubins路径或Reeds-Shepp路径，它们是由最大曲率的圆弧和直线组成的最短路径，非常适合这种有最小转弯半径约束的系统。

Dubins路径是其中经典的代表。它专门解决“在最小转弯半径约束下，从一个位姿到另一个位姿的最短路径”问题。它的解由不超过三段的基本片段（L: 左转圆弧， S: 直线， R: 右转圆弧）组成，例如“LSL”、“RSR”、“LSR”等。

优点：

计算速度极快。Dubins路径有解析解，几乎可以瞬间算出。
理论优美。它提供了在运动学约束下的最短路径，是一个很好的基准。
易于实现。代码实现相对固定。

缺点与我们的应对思考：

对环境障碍物不敏感。标准的Dubins路径只考虑起终点位姿和转弯半径，不考虑中间的环境障碍。对于空旷场地到空旷车位的理想情况可行，但一旦车位旁有障碍物，直接计算出的Dubins路径很可能穿墙而过。
解决方案：我们不能直接使用Dubins路径作为最终答案，但可以把它作为一个强大的**“启发式”** 或**“路径原型”**。例如，可以先计算一条Dubins路径，如果它碰撞了，我们可以尝试调整中间的“拼接点”，或者以Dubins路径为参考，在其附近用更精细的搜索算法（如下文的采样法或优化法）进行局部调整和优化。这样既利用了Dubins快速找到大致方向的能力，又通过后续处理规避了其缺点。

3.2 基于采样的随机规划方法

当环境复杂、约束众多时，解析方法往往束手无策。这时，基于采样的随机规划方法显示出强大的威力，其代表就是快速探索随机树。

它的核心思想非常“暴力美学”：不试图去计算一条精确的路径，而是通过随机撒点的方式，在状态空间（这里是(x, y, θ)）中生长一棵树。树的根节点是起点，然后循环执行以下步骤：

随机采样：在状态空间里随机采一个点q_rand。
最近邻查找：在现有的树中找到距离q_rand“最近”的节点q_near。这里的“距离”需要自定义，通常结合位置和角度差。
扩展：从q_near出发，朝着q_rand的方向“走”一小步（例如，施加一个固定的控制量δ一小段时间dt），得到一个新状态q_new。这一步需要调用我们之前写好的运动学模型进行仿真。
碰撞检测：检查从q_near到q_new的这段轨迹是否发生碰撞。如果安全，就把q_new加入树中，作为q_near的子节点。
循环：不断重复，直到某个新节点q_new进入了终点区域（位置和角度都足够接近目标），然后从终点节点回溯到起点，就得到了一条可行路径。

优点：

通用性强。几乎可以处理任何复杂的约束和障碍物形状，只要你能写出碰撞检测函数。
概率完备性。只要解存在，只要采样次数足够多，就一定能找到解。

缺点与实操心得：

路径质量可能不高。由于随机性，找到的路径可能绕远、扭动，不光滑，不符合“高效”的要求。
参数敏感。采样步长、采样区域偏向（是否偏向终点采样）等参数对算法效率和结果影响很大。
我们的实现技巧：纯RRT在泊车这种相对狭窄、对终点姿态要求精确的场景下，效率可能不高。我们采用了RRT* 的变种。RRT在扩展时会考虑“重布线”，即尝试将新节点连接到更优的父节点上，从而渐进地优化路径成本。此外，我们强烈建议采用双向RRT，即同时从起点和终点生长两棵树，当两棵树“连接”上时路径就找到了，这能显著提高在狭窄空间中的搜索效率。在编写代码时，碰撞检测函数的效率至关重要，需要高度优化，因为它会被调用成千上万次。

3.3 基于最优控制的数值优化方法

这是最“正统”也最复杂的方法，直接将泊车问题建模为一个最优控制问题。我们将连续的时间离散成N个阶段（k=0,1,...,N），每个阶段的状态是(x_k, y_k, θ_k)，控制输入是(v_k, δ_k)。那么问题可以形式化为：

最小化目标函数：例如，J = Σ (δ_k)^2（最小化转向动作，使路径平滑）或J = N（最小化时间）。满足约束：

运动学约束：x_{k+1} = f(x_k, u_k)，即我们之前离散化的运动方程。
路径约束：g(x_k) <= 0，即车辆矩形在每一个阶段都不与障碍物碰撞。
边界约束：起点x_0和终点x_N固定。
控制量约束：δ_min <= δ_k <= δ_max，v_min <= v_k <= v_max（泊车时速度很慢，可正可负，代表前进和倒车）。

然后，利用数值优化工具（如MATLAB的fmincon，或更专业的IPOPT、CasADi框架）来求解这个大规模、非线性、带约束的优化问题。

优点：

能得到真正意义上的“最优”解。在给定的目标函数下，这是数学上最严谨的解决方案。
路径平滑，控制量连续，非常接近实际车辆控制器的需求。

缺点与巨大挑战：

计算量大，求解困难。问题非凸（因为几何约束和运动学方程都是非线性的），容易陷入局部最优，甚至找不到可行解。
对初值敏感。优化求解器需要一个初始猜测（比如一条粗糙的可行路径），如果初值给得不好，求解很容易失败。
我们的实战策略：我们不会一开始就直接硬解这个最优控制问题。通常的流程是：先用RRT*快速生成一条粗糙但可行的路径，将这条路径上的状态点和控制量作为最优控制问题的初始猜测。然后，将这条路径“松弛”，比如允许它稍微违反一点碰撞约束但施加惩罚项，再用优化器进行“精修”。这样，优化器的工作就从“大海捞针”变成了“精雕细琢”，成功率和效率都大大提升。在建模论文中，即使你最后因为时间关系没能完全跑通整个优化流程，清晰地阐述这个“分层规划”的思路（采样规划提供初值，数值优化进行精炼）也能获得很高的评价。

4. 模型求解、仿真与论文呈现要点

选择了合适的模型和算法后，就进入了实现和论文写作阶段。这部分是将思路落地的关键，也是评委评判你工作扎实与否的主要依据。

4.1 编程实现与仿真框架

我们当时使用的是Python，生态丰富，可视化方便。核心库包括：

NumPy：数值计算核心。
SciPy：用于优化求解（如果采用最优控制方法）。
Matplotlib：用于绘制车辆轨迹、车位环境、障碍物，制作动态仿真动画。一个能动的仿真画面比你写十段文字描述都管用。
（可选）CasADi：一个强大的符号-数值优化框架，专门用于解决最优控制问题，学习曲线较陡但功能强大。

仿真程序的架构应该是模块化的：

参数定义模块：集中存放车辆参数（L, W, R_min）、车位参数、起点终点位姿。
模型模块：实现车辆运动学更新函数next_state = kinematic_model(current_state, control, dt)。
碰撞检测模块：实现函数is_collision(vehicle_vertices, parking_polygon, obstacle_list)。
规划器模块：实现你选择的核心算法，如RRTStarPlanner或DubinsHybridPlanner。它调用模型和碰撞检测模块。
主程序与可视化：调用规划器，得到路径（一系列状态和控制量），然后进行前向仿真验证，并绘制结果。

一个至关重要的验证步骤：规划出的路径是一系列离散的点。你需要写一个路径跟踪仿真，用规划出的控制量序列（或由路径点反算出的控制量）作为输入，从头到尾运行一遍运动学模型，并实时进行碰撞检测。这能验证你的规划结果是否真的可行。很多时候，规划算法本身可能没有考虑离散化误差或数值积分误差，导致“纸上谈兵”的路径在实际跟踪中会出轨。

4.2 灵敏度分析与模型拓展

做完基础案例后，论文要出彩，必须进行深入的分析和拓展。灵敏度分析是标配：

改变起点位置：如果车辆初始位置更偏左、更偏右、更远或更近，你的算法还能成功泊入吗？成功率和路径形状如何变化？可以绘制一个“可泊入区域”图。
改变车位大小：将车位宽度缩小到仅比车宽多出20厘米（模拟极限侧方停车），你的算法表现如何？规划时间是否激增？
改变车辆参数：比如轴距更长（转弯半径更大）的车辆，是否更难泊入？你的算法能否自适应？
增加动态不确定性：这是一个高阶的加分项。假设车辆的控制存在微小误差（比如方向盘转角有±2度的偏差），或者定位有噪声，你规划的路径是否依然鲁棒？可以通过在仿真中人为加入随机噪声来测试。

4.3 论文写作的核心章节布局

你的论文是向评委展示工作的唯一窗口。结构必须清晰，逻辑必须自洽。

问题重述与分析：不要照抄题目，要用自己的话精炼地概括问题，并完成我在第一部分提到的“数学翻译”，明确指出决策变量、目标函数和约束条件。
模型假设与符号说明：列出所有合理的简化假设（如地面水平、轮胎无滑移、忽略动力学等）。用表格清晰列出所有符号及其含义、单位。
模型建立：这是核心。分小节阐述：1) 车辆运动学模型（附公式和推导）；2) 几何与碰撞约束模型（附示意图和判断公式）；3) 最优控制问题形式化描述（如果采用）。将模型框图放在这里非常直观。
算法设计：详细说明你采用的算法流程。如果是RRT*，用伪代码描述并配以流程图。解释清楚关键步骤（采样、最近邻、扩展、碰撞检测、成本计算）的实现细节。如果是分层策略，说明各层如何衔接。
仿真实验与结果分析：这是展示工作量部分。首先给出基础案例的仿真结果：包括环境示意图、规划出的路径图、控制量（前轮转角、速度）随时间变化曲线。然后，系统地进行上述的灵敏度分析，用图表展示结果。最后，可以对不同算法（如果你实现了对比）在成功率、规划时间、路径长度等指标上进行对比，用表格呈现。
模型评价与推广：客观评价自己模型的优点（如考虑了真实运动学约束、能处理复杂障碍）和缺点（如计算效率有待提升、未考虑动态不确定性）。提出可行的改进方向，例如引入更精细的车辆模型、考虑实时避障、与机器学习结合进行轨迹预测等。

4.4 那些容易丢分的“坑”

只有结果图，没有分析过程：放一张漂亮的路径图是必须的，但评委更想看你是如何得到这个结果的。参数怎么调的？算法迭代了多少次？计算耗时多少？遇到什么困难，怎么解决的？
忽略单位与量纲：题目给的参数是米还是厘米？角度是弧度还是度？在整个建模和计算中必须统一，并在文中明确说明。在公式推导中带上单位是很好的习惯。
模型与算法描述脱节：前面写了一堆微分方程，后面算法部分完全没体现这些方程的作用，直接用了几何拼接。必须清晰地指出，算法中的状态更新步骤正是对应了微分方程的离散化。
缺乏对比与验证：只用一种情况、一种参数跑了一遍就完事。建模竞赛鼓励你探索不同场景。即使时间有限，也要设计2-3个有代表性的对比实验。
论文像实验报告：避免大段大段的代码截图。核心算法用伪代码，关键思路用流程图，结果用图表。代码可以放附录，但正文应以叙述、解释和分析为主。

这道“自动泊车”题，是一个将经典控制理论、计算几何和优化算法应用于鲜活实际问题的完美范例。它考察的绝不仅仅是编程能力，更是将模糊的现实问题转化为清晰数学问题的抽象能力，以及对多种建模工具的理解、选择和融合能力。从快速但粗糙的几何方法，到通用但随机的采样方法，再到精确但复杂的优化方法，每一种选择都体现了你对问题不同层面的把握。最终的胜出者，往往是那些不仅实现了算法，更能深刻理解其内在联系与适用边界，并用严谨的实验和清晰的论述将其展现出来的队伍。