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_{k1} x_k v * cos(θ_k) * dt y_{k1} y_k v * sin(θ_k) * dt θ_{k1} θ_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个阶段k0,1,...,N每个阶段的状态是(x_k, y_k, θ_k)控制输入是(v_k, δ_k)。那么问题可以形式化为最小化目标函数例如J Σ (δ_k)^2最小化转向动作使路径平滑或J N最小化时间。满足约束运动学约束x_{k1} f(x_k, u_k)即我们之前离散化的运动方程。路径约束g(x_k) 0即车辆矩形在每一个阶段都不与障碍物碰撞。边界约束起点x_0和终点x_N固定。控制量约束δ_min δ_k δ_maxv_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个有代表性的对比实验。论文像实验报告避免大段大段的代码截图。核心算法用伪代码关键思路用流程图结果用图表。代码可以放附录但正文应以叙述、解释和分析为主。这道“自动泊车”题是一个将经典控制理论、计算几何和优化算法应用于鲜活实际问题的完美范例。它考察的绝不仅仅是编程能力更是将模糊的现实问题转化为清晰数学问题的抽象能力以及对多种建模工具的理解、选择和融合能力。从快速但粗糙的几何方法到通用但随机的采样方法再到精确但复杂的优化方法每一种选择都体现了你对问题不同层面的把握。最终的胜出者往往是那些不仅实现了算法更能深刻理解其内在联系与适用边界并用严谨的实验和清晰的论述将其展现出来的队伍。
