1. 项目背景与核心挑战从数学建模到自动泊车去年带队参加MathorCup选的就是C题自动泊车。说实话当时看到题目描述第一感觉是“这题有得做”。它不像一些纯理论优化题那么抽象也不像一些数据挖掘题需要处理海量脏数据。自动泊车听起来很具体有明确的物理场景和工程目标但恰恰是这种“具体”对数学建模能力提出了非常综合的考验。题目本质上要求我们为一个虚拟的车辆在复杂的约束条件下比如车位尺寸、车辆运动学、障碍物规划出一条从起点到泊入终点的最优或可行路径。这背后是路径规划、最优控制、几何计算和数值仿真等多个领域的交叉。很多初次接触这类问题的同学容易陷入两个极端要么被“自动泊车”这个工程名词吓到觉得需要非常深厚的车辆工程或机器人学背景要么就把它简单理解为一个“找最短路径”的图论问题忽略了车辆本身“不能横着走”的运动学约束。这就是这道题的第一个核心挑战如何用准确的数学模型描述一辆真实汽车的运动。你不能把它当成一个可以任意方向移动的点必须考虑它的前轮转向角、轴距、最小转弯半径等。第二个挑战在于环境建模与碰撞检测。车位是规则的但车辆轮廓是不规则的矩形如何高效、精确地判断在整个泊车过程中车辆与边界、与障碍物可能是其他车或柱子是否发生干涉第三个挑战是求解策略的选择。这是一个典型的非凸、非线性优化问题搜索空间巨大直接上标准优化算法如梯度下降很可能陷入局部最优或者根本找不到解需要设计合理的搜索或优化策略。我们当时的解题思路就是围绕这三个挑战展开的。下面我将结合我们当时的方案和后续的一些反思拆解整个解题过程并附上核心思路的伪代码和实现要点。你会发现数学建模的魅力就在于用清晰的数学语言定义问题然后用算法和计算去逼近解决方案。2. 车辆运动学建模阿克曼转向与车辆位姿描述一切规划的基础是准确知道车辆如何运动。对于低速泊车场景我们通常采用自行车模型Bicycle Model或更具体的阿克曼转向几何Ackermann Steering Geometry简化。这里的关键是理解“前轮转向角”与“车辆整体旋转”之间的关系。假设车辆轴距为L前轴到后轴的距离后轴中心为参考点(x, y)车身朝向角为θ与水平轴夹角。给定一个前轮转向角δ通常有最大值限制如 ±30°在很短的时间Δt内车辆以后轴中心为基准的瞬时运动可以近似描述。但更常用的是以前轴中心或后轴中心为参考的微分方程。以后轴中心为参考点的模型是路径规划中常用的。其运动学方程为dx/dt v * cos(θ) dy/dt v * sin(θ) dθ/dt v / L * tan(δ)其中v是后轴中心的线速度即车速。这个模型清晰表明车辆的朝向变化率与车速、转向角正切值成正比与轴距成反比。最小转弯半径 R_min L / tan(δ_max)这是一个非常重要的参数意味着车辆无法做出比这个半径更急的转弯。在离散仿真中我们采用数值积分。假设控制输入是速度v和转向角δ在时间步长Δt内θ_{k1} θ_k (v_k * Δt / L) * tan(δ_k) x_{k1} x_k v_k * Δt * cos(θ_{k1}) // 注意这里常用θ_{k1}或(θ_kθ_{k1})/2来提高精度 y_{k1} y_k v_k * Δt * sin(θ_{k1})注意这里有一个细微但重要的点。严格来说在Δt内转向角δ保持不变车辆走一段圆弧。更精确的更新公式对于恒定v和δ是if |δ| ε: // 非直线行驶 R L / tan(δ) dθ (v * Δt) / R θ_{k1} θ_k dθ // 以后轴中心为圆心半径为R的圆弧上移动 x_{k1} x_k R * (sin(θ_{k1}) - sin(θ_k)) y_{k1} y_k - R * (cos(θ_{k1}) - cos(θ_k)) // 注意正负号与坐标系定义有关 else: // 近似直线行驶 θ_{k1} θ_k x_{k1} x_k v * Δt * cos(θ_k) y_{k1} y_k v * Δt * sin(θ_k)在精度要求不极端高的规划中第一种欧拉积分方法因其简单而被广泛采用但需要意识到其误差会随着Δt增大而累积。车辆轮廓计算规划中我们需要判断碰撞因此必须知道车辆四个角或更多边界点的全局坐标。已知后轴中心点(x, y)、朝向θ、车长L_f后轴到车头、车宽W可以计算出四个角点假设后轴中心在车辆几何中心后方L_r处// 定义车辆轮廓角点相对于后轴中心的局部坐标车辆坐标系车头方向为x轴 局部坐标 front_right [L_f, W/2], front_left [L_f, -W/2], rear_right [-L_r, W/2], rear_left [-L_r, -W/2] // 旋转并平移至全局坐标系 对于每个局部点 [px_local, py_local] px_global x px_local * cos(θ) - py_local * sin(θ) py_global y px_local * sin(θ) py_local * cos(θ)这样我们就得到了车辆在任意位姿下的精确轮廓为碰撞检测做好了准备。3. 环境与碰撞检测建模几何判断与安全裕度有了运动的车辆接下来要定义它不能撞到什么。题目通常会给出停车位的精确尺寸长、宽、入口位置以及可能的障碍物信息如柱子的位置和半径、其他车辆的停放位置等。环境建模的核心是将所有这些约束转化为计算机可以快速进行逻辑判断的形式。1. 边界约束车位线这是最基础的约束。通常假设车位是一个矩形区域。车辆在泊入完成后的最终状态必须完全位于该矩形内。但在规划过程中车辆是可以超出这个矩形的比如需要先摆出角度。因此碰撞检测是针对“不可穿越”的边界比如墙壁、路沿。在数学上我们可以将车位边界线表示为线段并规定车辆轮廓多边形与这些线段不能有交集。2. 障碍物约束障碍物可能是其他车辆也近似为矩形或柱子圆形。对于矩形障碍物问题转化为两个凸多边形之间的碰撞检测。对于圆形障碍物则是判断车辆轮廓点是否在圆内或者更精确地判断线段车辆边与圆是否相交。高效的碰撞检测实现 对于矩形-矩形碰撞由于车辆和障碍物都是凸多边形可以使用分离轴定理Separating Axis Theorem, SAT。原理是如果存在一条直线轴使得两个多边形在该直线上的投影不重叠则它们没有碰撞。对于矩形只需要检查两条边所在法线方向共4个方向即可。函数 检测矩形碰撞(矩形A, 矩形B): 对于每个矩形计算其四个角点的全局坐标。 定义待检查的轴矩形A的两条边的法线矩形B的两条边的法线。 对于每一个轴 将矩形A的所有顶点投影到该轴上得到投影区间[A_min, A_max]。 将矩形B的所有顶点投影到该轴上得到投影区间[B_min, B_max]。 如果 A_max B_min 或 B_max A_min 返回 “无碰撞” // 在此轴上分离 如果所有轴上都未分离 返回 “发生碰撞”对于矩形-圆形碰撞可以简化为判断圆心到矩形四条边的最短距离是否大于圆的半径。也可以将圆形近似为一个小正方形进行快速但保守的估计。3. 引入安全裕度在实际编程中直接使用理论尺寸进行碰撞检测是非常危险的因为数值计算有误差且规划出的路径需要一定的安全余量。通常的做法是进行“膨胀Inflation”。即将障碍物的尺寸在检测时略微扩大例如每边扩大0.1米或者将车辆的轮廓略微扩大。这样即使规划路径紧贴理论安全边界实际检测时仍能通过为控制和执行留出容错空间。这是一个非常重要的工程经验。4. 离散化与连续检测我们的路径是由一系列离散的位姿点组成的。仅仅检查这些离散点是否碰撞是不够的因为车辆在两个点之间运动时也可能发生碰撞。因此需要进行连续碰撞检测。一个实用且足够精确的方法是在两点之间进行插值例如按更小的步长Δs生成中间点然后检查这些中间点的位姿。虽然计算量增大但能显著提高安全性。在算法设计中这通常作为一个可配置的参数。4. 路径规划策略从几何法到搜索算法这是整个问题的核心也是算法创新的主战场。目标是为车辆生成一条从初始位姿(x0, y0, θ0)到目标位姿(x_goal, y_goal, θ_goal)的无碰撞路径。针对自动泊车这种相对结构化、空间受限的场景有几种典型的思路。4.1 基于几何的预设路径库这是最直观的方法尤其适用于标准的垂直泊车或平行泊车。其思想是总结人类司机的泊车经验将其抽象为几条固定的几何路径如圆弧和直线组合。例如常见的“倒车入库”路径可能由三段组成1) 直线向前行驶使车辆到达一个特定的“起始位置”2) 方向盘打满倒车走一段圆弧3) 回正方向盘直线倒车入库。优点计算速度快路径光滑可预测易于实现和控制跟踪。缺点灵活性极差。只能应对非常标准、无障碍的场景。一旦初始位置偏离预设或者车位旁有障碍物该方法很可能失效。在数学建模竞赛中如果题目场景非常标准这可以作为基础方案但通常难以拿到高分因为它没有体现“规划”和“优化”的过程。4.2 随机采样算法快速探索随机树RRT及其变种当环境复杂、约束多时确定性几何方法束手无策这时需要更通用的规划算法。RRT系列算法在机器人路径规划中应用极广它非常适合解决高维、非凸空间中的路径规划问题。基本RRT流程初始化树T根节点为初始位姿。循环直到达到最大迭代次数或找到路径 a.随机采样在状态空间x, y, θ中随机生成一个点q_rand。 b.最近邻查找在树T中找到距离q_rand最近的节点q_near距离需要自定义通常是位姿的加权欧氏距离。 c.控制扩展从q_near出发施加一个控制输入如固定步长的 (v, δ) 组合通过车辆运动学模型生成一个新的位姿q_new。这一步模拟了车辆的实际运动。 d.碰撞检测检查从q_near到q_new的轨迹段是否无碰撞。 e.添加节点如果无碰撞将q_new加入树T并将q_near设为其父节点。 f.目标判断如果q_new距离目标位姿足够近则规划成功可以通过回溯父节点得到路径。RRT的优缺点优点概率完备性只要解存在给定无限时间总能找到不依赖于环境的具体结构能很好地处理复杂约束。缺点生成的路径通常不是最优的可能非常曲折、不光滑。而且纯随机采样效率较低。改进策略用于竞赛提分RRT*在RRT的基础上增加了“重布线”和“父节点重选”步骤能渐进地优化路径成本如路径长度最终收敛到最优解。计算量更大但路径质量显著提升。偏向目标采样不是完全随机采样而是以一定概率如10%直接采样目标点作为q_rand引导树向目标生长加快收敛。双向RRT同时从起点和终点生长两棵树当两棵树“连接”时即找到路径。效率通常比单树RRT高。考虑动力学平滑在扩展时不仅考虑位置还考虑速度、转向角变化的连续性使生成的路径更易于车辆跟踪。4.3 基于优化的方法将路径规划表述为一个非线性优化问题。定义一系列路径点或控制输入序列作为优化变量以路径长度、平滑度转向角变化率等为目标函数以车辆运动学方程、边界约束、碰撞避免约束为约束条件然后调用优化求解器如IPOPT、SNOPT求解。优点能直接得到平滑、最优的路径理论上是更优雅的解决方案。缺点问题非凸对初值非常敏感。如果初始猜测不好求解器极易陷入局部最优或失败。计算量大实时性较差。在数模竞赛有限的时间内完整实现并调试好一个非线性优化求解流程挑战很大。我们的选择与折衷 在竞赛的有限时间内我们采用了“偏向目标采样 碰撞检测 路径后处理”的RRT变种方案。理由如下可靠性优先RRT系列算法能保证在复杂环境下找到可行解这是完成题目的基础。实现可控算法逻辑相对清晰编码实现难度适中易于调试。便于改进我们可以在基本RRT框架上加入一些启发式规则来提升路径质量例如在采样时更多地采样车位入口附近的区域在扩展时优先尝试那些能使车辆朝向更对准目标位姿的控制输入。后处理优化RRT生成的原始路径可能由许多小线段组成且不平滑。我们设计了一个后处理步骤对路径节点进行抽稀删除共线的中间点然后在满足曲率约束最小转弯半径的前提下用样条曲线或圆弧-直线组合对路径进行平滑。这样最终输出的路径既保证了可行性又提升了质量。5. 算法实现细节与编程要点理论需要代码来实现。这里分享我们在MATLAB数模竞赛常用中实现核心RRT路径规划器的关键代码结构和注意事项。5.1 数据结构设计% 定义车辆参数结构体 vehicle.L 2.8; % 轴距 (m) vehicle.Lf 1.0; % 后轴到车头距离 vehicle.Lr 1.0; % 后轴到车尾距离 (假设对称) vehicle.width 1.8; % 车宽 (m) vehicle.max_steer deg2rad(30); % 最大转向角 (rad) vehicle.wheelbase vehicle.L; % 同轴距 % 定义节点结构体树中的每个节点 node.state [x, y, theta]; % 位姿 node.parent []; % 父节点索引 node.cost 0; % 从根节点到该节点的累积成本如路径长度 node.control [v, delta]; % 导致到达此状态的控制输入 % 定义规划参数 planning_params.max_iter 5000; % 最大迭代次数 planning_params.goal_tolerance [0.1, 0.1, deg2rad(5)]; % [x, y, theta] 容差 planning_params.step_size 0.5; % 扩展步长 (m) planning_params.sample_goal_rate 0.1; % 采样目标点的概率5.2 核心RRT循环伪代码function path rrt_planner(start, goal, obstacles, vehicle, params) % 初始化树 tree(1).state start; tree(1).parent 0; tree(1).cost 0; tree(1).control [0, 0]; for i 1:params.max_iter % 1. 采样 if rand() params.sample_goal_rate q_rand goal; else q_rand sample_random_state(); % 在自由空间采样 end % 2. 寻找最近邻 (需要考虑角度使用加权距离) min_dist inf; q_near_idx 1; for j 1:length(tree) dist weighted_distance(tree(j).state, q_rand); if dist min_dist min_dist dist; q_near_idx j; end end q_near tree(q_near_idx).state; % 3. 控制扩展 (关键步骤) % 尝试从q_near向q_rand方向“生长” [q_new, control_applied, trajectory_segment] steer(q_near, q_rand, vehicle, params.step_size); % steer函数计算从q_near到q_new所需的控制(v, delta)和中间轨迹点 % 4. 碰撞检测 (对整段轨迹) if check_collision(trajectory_segment, vehicle, obstacles) continue; % 发生碰撞放弃该扩展 end % 5. 添加新节点 new_node.state q_new; new_node.parent q_near_idx; new_node.cost tree(q_near_idx).cost segment_length(trajectory_segment); new_node.control control_applied; tree [tree, new_node]; % 6. 检查是否到达目标 if is_goal_reached(q_new, goal, params.goal_tolerance) path extract_path(tree, length(tree)); % 回溯得到路径 return; end end % 迭代结束未找到路径 path []; error(Path not found within max iterations.); end5.3 关键函数详解steer 与碰撞检测steer函数这是连接离散规划与连续运动学的桥梁。一个简单的实现是计算q_near到q_rand的方向角将其与q_near的当前朝向角θ_near做差得到期望的航向角变化。然后根据车辆运动学反解出一个可行的转向角δ需限制在±max_steer内并以固定速度v可正可负代表前进或倒车行驶固定步长step_size通过运动学模型积分得到q_new。更高级的steer函数可以尝试多个不同的(v, δ)组合选择使新状态最接近q_rand的那个。check_collision函数这是性能瓶颈。为了提高效率我们采用了分层检测粗略检测计算trajectory_segment的包围盒与障碍物的包围盒进行快速重叠判断如果不重叠直接通过。精细检测对于粗略检测有重叠的对轨迹段进行插值例如每0.05米一个点对每个插值点计算车辆轮廓使用SAT方法或简单矩形-矩形/矩形-圆检测判断是否碰撞。提前终止一旦检测到碰撞立即返回true避免不必要的计算。5.4 路径后处理与平滑RRT生成的路径是一系列状态点[state1, state2, ..., stateN]对应的控制输入可能忽前忽后、转向角突变不适合车辆直接跟踪。function smooth_path path_smoothing(raw_path, vehicle) % 1. 路径抽稀删除与前后点共线的中间点减少点数 simplified_path simplify_path(raw_path); % 2. 速度/方向规划根据相邻点位的朝向变化决定该段是前进还是倒车。 % 基本原则如果相邻点位的朝向角变化很小可以认为是直线速度方向一致。 % 如果变化大可能是转弯需要结合转向角判断。一个简单策略是如果从点i到点i1需要 % 的转向角通过运动学反解在合理范围内则赋予一个固定的前进或倒车速度符号。 % 3. 曲线拟合可选但推荐使用样条曲线如三次样条对简化后的路径点(x,y)进行拟合。 % 然后根据样条曲线计算曲率确保曲率处处小于车辆最大曲率(1/R_min)。 % 如果曲率超限则需要调整拟合点或采用分段圆弧-直线拟合。 % 分段圆弧-直线拟合Dubins Path或Reeds-Shepp Path能保证路径严格满足车辆运动学 % 但实现更复杂。在竞赛中只要说明采用了平滑处理并展示平滑前后的对比图就能体现完整性。 smooth_path simplified_path; % 此处简化为返回抽稀路径 end6. 仿真验证与结果分析如何呈现你的工作在数学建模论文中算法和模型需要靠仿真结果来证明其有效性。这部分不仅仅是“跑个程序出个图”而是要有理有据地展示你的方案解决了问题。6.1 设计仿真实验不要只展示一个完美场景。设计多个有代表性的测试场景来全面评估算法场景A标准垂直泊车初始位置正对车位有充足空间。用于验证算法基本功能。场景B狭窄空间泊车车位两侧有紧邻的障碍车。用于测试算法的避障能力和规划精度。场景C非常规初始位姿车辆起始位置与车位轴线有较大夹角或偏移。用于测试算法的鲁棒性和搜索能力。场景D有柱状障碍物车尾附近有柱子。用于测试对不同形状障碍物的处理能力。对于每个场景记录并展示规划成功率在给定的最大迭代次数内算法是否能找到路径。规划时间从开始到找到路径所需的CPU时间或迭代次数。这体现了算法效率。路径质量指标路径总长度越短越好。转向角变化剧烈程度可以用转向角变化的绝对值积分或最大值来衡量。越平滑车辆跟踪越容易。换挡次数路径中前进和倒车切换的次数。越少越好。最终泊入精度车辆最终位姿与目标位姿的误差位置误差和角度误差。6.2 可视化呈现一图胜千言。在论文中必须包含高质量的可视化结果。动图/序列图展示车辆从起始位置沿着规划出的路径一步步运动到车位内的过程。可以用MATLAB的plot和pause函数生成序列图或者直接录制屏幕生成动图作为附件。路径对比图将不同场景下规划出的路径画在同一张图上用不同颜色和线型区分。同时画出车辆在关键节点如开始转弯、切换方向的轮廓。算法过程图对于RRT可以展示搜索树的生长过程用浅色细线表示探索过的树枝用加粗的彩色线表示最终找到的路径。这能直观体现算法的搜索逻辑。数据表格将上述指标成功率、时间、长度、误差等汇总在一个表格中清晰对比不同场景下的算法性能。6.3 敏感性分析与参数调优这是体现建模深度的关键。不要只给出一组“能用”的参数。分析关键参数的影响例如研究step_size扩展步长对规划成功率和路径质量的影响。步长太短树生长慢规划时间长步长长可能“跨过”狭窄通道导致规划失败。通过一组实验展示其影响趋势并说明你最终选择某个值的理由。与简单方法对比如果你实现了基于几何的预设路径库可以将RRT规划的结果与之对比。在标准场景下几何法可能更快更优但在复杂场景下RRT的优越性就体现出来了。这种对比能有力地支撑你选择更复杂算法的决策。讨论局限性诚实地指出你方法的局限性。例如RRT在极端狭窄、需要“多次揉库”的场景下可能因为随机性而效率很低甚至找不到解。可以提出改进方向如引入更智能的采样策略在狭窄通道附近提高采样密度或者结合局部优化。7. 从解题到论文经验总结与避坑指南最后结合我们参赛和后续复盘的经验分享一些超越代码和算法本身的要点这些往往决定了论文的最终档次。7.1 论文写作的“技术性”表达数学建模论文不是实验报告它需要清晰的逻辑和专业的表述。问题重述与分析不要照抄题目。要用自己的语言结合你建立的模型将问题分解为“车辆建模”、“环境建模”、“路径规划”、“碰撞约束”等子问题并分析其难点非完整性约束、非凸优化等。模型假设列出清晰合理的假设。例如“假设地面水平且摩擦系数足够大忽略打滑”、“假设车辆为刚性矩形”、“假设障碍物位置精确已知且静止”。好的假设能简化问题同时体现你的思考。符号说明制作一个规范的表格列出所有文中出现的关键变量、符号及其含义和单位。这是专业性的体现。模型建立这是核心章节。分小节阐述车辆运动学模型、环境与碰撞检测模型、路径规划模型RRT算法。对于算法要用流程图可以用Visio或PPT画贴图结合公式、伪代码来描述。伪代码要简洁明了突出关键步骤。模型求解与结果分析对应仿真验证部分。说明你使用的软件MATLAB/Python、参数设置、实验设计。然后展示结果并进行分析。分析不要只说“如图所示”要解读数据“从表1可以看出在场景B下规划时间增加了约150%这是因为狭窄空间限制了采样有效性算法需要更多探索...”。模型评价与推广客观评价自己模型的优点通用性强、能处理复杂障碍和缺点随机性导致解不稳定、路径可能非最优。提出可行的改进方向如采用RRT*、结合曲线平滑优化。最后简要说明模型稍作修改后可应用于其他场景如园区AGV调度、仓库机器人搬运。7.2 编程实现中的“坑”角度处理程序中所有角度务必统一使用弧度制。计算朝向角差时注意处理圆周跳变如从 -π 到 π 的跃迁使用atan2函数并规范化角度到[-π, π]区间。碰撞检测的精度与效率这是调试中最耗时的地方。务必编写独立的测试用例例如让车辆静止在已知碰撞/不碰撞的位置验证你的检测函数是否正确。效率优化如空间划分、粗略检测在迭代次数多时至关重要。随机数种子RRT具有随机性。为了结果可复现在程序开始时固定随机数种子如rng(1)。在论文中也可以展示不同随机种子下的成功率统计以说明算法的平均性能。可视化调试边写代码边画图。每扩展一个新节点就刷新一下树和障碍物的图。这能帮你快速定位算法逻辑错误如树不生长、总是碰撞。7.3 团队协作与时间管理MathorCup时间紧。建议分工一人主攻模型与算法设计出伪代码一人主攻编程实现与调试一人主攻论文写作与可视化。但三者需要紧密沟通。第一天必须完成问题分析和模型框架搭建第二天完成核心算法编程和基础仿真第三天集中进行多场景测试、参数调优、结果分析和论文撰写。最后留出时间整合、检查与排版。自动泊车问题是一个经典的跨学科问题它完美地结合了理论建模和工程实践。通过这次竞赛我们不仅学会了一套解决特定问题的方法更重要的是掌握了“面对一个复杂工程问题如何将其分解、建模、算法选型、实现验证并有效呈现”的完整流程。这份经验对于今后从事任何与技术研发相关的工作都是极其宝贵的。