1. 项目背景与问题拆解从泊车难题到数学建模最近在整理过往的数学建模竞赛资料翻到了2022年Mathorcup数学建模挑战赛的C题一个关于自动泊车路径规划的题目。这个题目当时吸引了不少队伍因为它非常“接地气”——把日常生活中让人头疼的侧方停车和垂直停车抽象成了一个可以用数学模型和算法来求解的优化问题。很多同学一看到“自动泊车”、“路径规划”这些词可能第一反应是去搜索现成的机器人路径规划算法比如A*、DWA或者RRT。但数学建模的魅力恰恰在于它要求你从问题本源出发自己搭建模型框架而不是直接调用一个黑箱。今天我就结合这道赛题和大家深入聊聊如何构建一个简洁、有效且可求解的泊车运动学模型并附上完整的Matlab实现思路和代码解析。无论你是正在备赛的同学还是对车辆运动控制感兴趣的爱好者相信这篇从实战角度的拆解都能给你带来启发。这道题的核心要求是给定一个标准的停车位和一辆具有特定尺寸长、宽、轴距、最小转弯半径的小型轿车需要我们的模型能够计算出从起始位置车位外安全、平滑地泊入车位侧方或垂直的完整路径。所谓“安全”意味着在整个泊入过程中车辆的任何部分都不能与停车位的边界线发生碰撞同时路径还应尽可能短、曲率连续以符合真实的车辆操控体验。这听起来像是一个标准的轨迹优化问题但难点在于车辆的运动学约束——它不能像点一样随意移动而是受到阿克曼转向几何的制约其运动轨迹是由前轮转角决定的圆弧组合。因此我们的建模工作将围绕三个核心展开如何用数学描述车辆的运动运动学模型、如何判断车辆与环境的碰撞碰撞检测模型、以及如何搜索或优化出一条满足所有约束的可行路径路径规划算法。2. 车辆运动学模型构建从阿克曼转向到状态方程要规划路径首先必须知道车辆是如何运动的。我们这里采用经典的自行车模型Bicycle Model来简化四轮车辆。这是一个在低速场景下如泊车精度足够且计算简便的模型。它的核心假设是将左右前轮合并为一个位于车辆中轴线上的“虚拟前轮”同样后轮也合并为一个“虚拟后轮”。车辆的运动完全由这个虚拟前轮的转向角 δ 和后轮驱动速度 v 控制。2.1 模型推导与状态定义我们定义车辆的状态向量为[x, y, θ, φ]。其中(x, y)是车辆后轴中心点即后轮虚拟点在世界坐标系下的坐标。选择后轴中心作为参考点是因为它是驱动点运动方程更简洁。θ是车辆的航向角即车身纵轴线与全局X轴的夹角。φ是前轮转向角即我们的控制输入之一。这里有一个关键参数轴距L即前后轴之间的距离。根据阿克曼转向几何在低速且忽略轮胎侧偏角的假设下车辆的瞬时转弯中心ICR位于后轴延长线上并且与前轮转向角满足几何关系。由此我们可以推导出车辆的运动学微分方程dx/dt v * cos(θ) dy/dt v * sin(θ) dθ/dt (v / L) * tan(φ)这个方程组就是我们的核心模型。dθ/dt的公式是精髓它揭示了航向角的变化率与速度、转向角的正切值成正比与轴距成反比。当φ0时车辆直线行驶φ为正或负时车辆分别进行左转或右转的圆周运动。在Matlab中我们通常采用离散化的方式对模型进行数值积分。假设控制输入在很短的时间步长dt内保持不变那么状态更新公式为% 假设当前状态为 state [x; y; theta; phi]控制输入为 u [v; delta_phi] v u(1); delta u(2); % 注意这里的delta是转向角指令可能需要考虑转向角变化率限制 % 状态更新欧拉积分法 x_new x v * cos(theta) * dt; y_new y v * sin(theta) * dt; theta_new theta (v / L) * tan(phi) * dt; % 注意这里用的是当前phi而非delta phi_new phi delta * dt; % 转向角本身也是一个状态其变化率受delta控制 % 需要对phi_new进行限幅使其不超过最大转向角phi_max phi_new max(min(phi_new, phi_max), -phi_max);这里有一个重要的细节φ本身也是一个状态量它的变化率δ即方向盘转动角速度才是直接的控制输入。这更符合实际情况因为方向盘不能瞬间打到最大角度。因此完整的控制输入向量通常是u [v; δ]其中v是纵向速度可正可负代表前进或倒车δ是转向角变化率。2.2 模型参数与约束条件在实际编码前我们必须明确车辆的物理参数和约束这些是模型可行性的边界车辆尺寸长度L_car宽度W_car。注意L_car是整车长度而运动学模型中的L是轴距两者不同。在碰撞检测时需要使用L_car。轴距 (L)如前所述是运动学模型的关键参数。最大转向角 (φ_max)决定了车辆的最小转弯半径R_min L / tan(φ_max)。这是路径曲率的上限任何规划路径的瞬时转弯半径都不能小于R_min。转向角变化率限制 (δ_max)限制了方向盘转动的快慢影响了路径的平滑性和可执行性。速度范围 (v_min, v_max)泊车时速度通常较低且应支持倒车v为负值。在Matlab中我们可以将这些参数定义在一个结构体里方便管理vehicle.L 2.8; % 轴距单位米 vehicle.L_car 4.8; % 车长 vehicle.W_car 1.8; % 车宽 vehicle.phi_max deg2rad(35); % 最大转向角35度 vehicle.delta_max deg2rad(30); % 最大转向角变化率30度/秒 vehicle.v_max 2.0; % 最大前进速度米/秒 vehicle.v_min -1.5; % 最大倒车速度绝对值米/秒 vehicle.R_min vehicle.L / tan(vehicle.phi_max); % 最小转弯半径3. 碰撞检测模型把车“画”出来用几何判断规划出的路径再优美如果撞上了马路牙子或其他车也是徒劳。因此一个快速、准确的碰撞检测模块至关重要。对于矩形车辆和矩形停车位或障碍物我们通常采用“边界框检测”结合“分离轴定理SAT”的方法但针对泊车这种相对简单的环境有一种更直观且计算高效的方法车辆轮廓点检测法。3.1 车辆轮廓世界坐标计算思路很简单我们已知车辆后轴中心点(x, y)、航向角θ和车辆尺寸。可以计算出车辆四个角或更多轮廓点在世界坐标系下的位置。只要这些点都在停车位的边界线以内且与边界的距离大于一个安全阈值例如0.1米我们就认为没有碰撞。如何计算四个角点的坐标以车辆后轴中心为参考点(x, y)车辆几何中心相对于后轴中心有一个偏移。设车辆重心或几何中心到后轴的距离为l_r通常l_r (L_car - L)/2假设轴距居中。那么车辆几何中心(x_c, y_c)的坐标为l_r (vehicle.L_car - vehicle.L) / 2; x_c x l_r * cos(theta); y_c y l_r * sin(theta);然后以(x_c, y_c)为中心θ为方向可以计算出四个角点% 车辆半长半宽 half_length vehicle.L_car / 2; half_width vehicle.W_car / 2; % 四个角点相对于几何中心的局部坐标车身坐标系 corners_local [ half_length, half_width; % 右前 half_length, -half_width; % 右后 -half_length, -half_width; % 左后 -half_length, half_width]; % 左前 % 旋转并平移到世界坐标系 rotation_matrix [cos(theta), -sin(theta); sin(theta), cos(theta)]; corners_global (rotation_matrix * corners_local) [x_c, y_c];现在corners_global是一个4x2的矩阵每一行代表一个角点的(x, y)坐标。3.2 停车位边界与碰撞判断假设停车位是一个矩形由两个对角点(x1, y1)和(x2, y2)定义且x2 x1, y2 y1。那么判断车辆是否完全在车位内且无碰撞的条件是% 计算车位边界可能还需要考虑道路边界这里以车位为例 parking_xmin min(x1, x2) safety_margin; parking_xmax max(x1, x2) - safety_margin; parking_ymin min(y1, y2) safety_margin; parking_ymax max(y1, y2) - safety_margin; % 判断所有角点是否都在边界内 x_ok all(corners_global(:,1) parking_xmin corners_global(:,1) parking_xmax); y_ok all(corners_global(:,2) parking_ymin corners_global(:,2) parking_ymax); is_collision_free x_ok y_ok;这里的safety_margin是一个小的安全裕度通常设为0.05到0.2米用来补偿模型简化带来的误差和提供安全缓冲。注意这种方法假设车位是轴对齐的矩形。如果车位方向与全局坐标系不一致你需要先对车辆角点坐标进行旋转或者更通用地将问题转化为判断两个凸多边形车辆轮廓和车位轮廓是否相交这时分离轴定理SAT就是更合适的工具。但对于Mathorcup这道题轴对齐的假设通常是成立的。4. 路径规划算法选型与实现在约束中寻找可行解有了运动学和碰撞模型接下来就是最核心的路径规划。对于泊车这种状态空间受限、需要前进倒退组合的问题常见的思路有几何法、基于最优控制的方法如庞特里亚金最小值原理和搜索法如状态格子搜索、混合A*。考虑到数学建模竞赛的时间限制和实现复杂度基于Reeds-Shepp曲线或Dubins曲线的几何组合方法结合多点打靶法进行局部优化是一个在效率和效果上取得很好平衡的选择。4.1 基于曲线组合的初始路径生成Dubins曲线描述了在最小转弯半径约束下从一点到另一点的最短路径但只允许前进。Reeds-Shepp曲线则扩展了Dubins允许车辆倒车这正好符合泊车需求。一条完整的Reeds-Shepp路径由若干段圆弧C和直线S组成例如 “CSC”圆弧-直线-圆弧或 “CCCC”三段圆弧组合。Matlab中并没有内置的Reeds-Shepp曲线生成器但网上有开源实现如reeds_shepp.m。我们可以利用它来快速生成一条连接起点和终点的、满足车辆最小转弯半径约束的几何路径。这条路径给出了一个粗略的、可能不精确满足碰撞约束的参考。% 假设有函数 reeds_shepp(curve, start_pose, end_pose, rho, step_size) % start_pose, end_pose 是 [x, y, theta] % rho 是最小转弯半径的倒数 (1/R_min) % curve 是路径类型字符串如 LfSfLf (左转前进-直行前进-左转前进) start [x0, y0, theta0]; goal [xg, yg, thetag]; % 目标位姿通常是车位内的一个姿态如车头朝里 rho 1 / vehicle.R_min; paths reeds_shepp(all, start, goal, rho, 0.1); % 计算所有可能类型的路径 % 选取长度最短的一条作为初始参考路径 [~, idx] min([paths.length]); ref_path paths(idx);ref_path对象通常包含一系列路径点位姿序列。然而这条路径可能穿过障碍物或者曲率不连续在段与段连接处。因此它只是一个“种子”。4.2 基于多点打靶法的轨迹优化为了得到一条真正可行、平滑且无碰撞的轨迹我们需要对初始参考路径进行优化。这里介绍一种在建模中非常实用的方法多点打靶法Multiple Shooting结合非线性规划NLP。其核心思想是将整个路径离散成N个阶段节点。每个节点上我们定义车辆的状态X_k [x_k, y_k, θ_k, φ_k]。相邻节点之间的控制输入U_k [v_k, δ_k]假设为常数。运动学方程作为等式约束将相邻状态联系起来X_{k1} f(X_k, U_k, dt) % f 是2.1节中的离散状态更新函数同时我们还有不等式约束控制量约束|v_k| v_max,|δ_k| δ_max,|φ_k| φ_max碰撞约束在每一个节点X_k车辆轮廓必须位于自由空间内通过3.2节的方法判断。边界约束起点X_0和终点X_N固定或在一定误差范围内。我们的优化目标是最小化路径总长度或总时间、控制能量等。例如总长度近似为∑ |v_k| * dt。这样我们就把路径规划问题转化为了一个标准的非线性优化问题。在Matlab中可以使用fmincon函数来求解。难点在于如何高效地处理碰撞约束。一个技巧是将碰撞约束转化为对车辆角点位置的边界约束。例如对于垂直停车位要求车辆四个角点的x坐标都在[x_min, x_max]之间y坐标都在[y_min, y_max]之间。但这会产生大量约束4个角点 * N个节点 * 2个方向 8N个约束。为了简化可以只在关键节点如路径中点、转弯处施加严格的碰撞约束在其他节点施加较宽松的约束或者将碰撞检测作为目标函数中的惩罚项障碍函数法。% 伪代码使用fmincon进行优化 % 1. 定义优化变量Z [X_0, U_0, X_1, U_1, ..., X_{N-1}, U_{N-1}, X_N] num_states 4; num_controls 2; Z0 ... % 初始猜测可以从Reeds-Shepp路径插值得到 % 2. 定义上下界 lb, ub lb ... % 对应状态和控制的物理下限 ub ... % 对应上限 % 3. 定义线性/非线性等式约束状态转移方程 Aeq []; beq []; % 通常状态转移是非线性的所以用非线性等式约束 nonlcon (Z) myNonlinearConstraints(Z, vehicle, dt, parking_bounds); % 4. 定义目标函数如路径长度 objective (Z) sum(abs(Z(控制速度索引))) * dt; % 5. 调用fmincon options optimoptions(fmincon, Display, iter, Algorithm, interior-point, MaxFunctionEvaluations, 1e5); [Z_opt, fval] fmincon(objective, Z0, [], [], Aeq, beq, lb, ub, nonlcon, options);在myNonlinearConstraints函数中你需要实现状态转移的等式约束和碰撞检测的不等式约束。这是一个计算量较大的过程但对于N不是特别大比如50-100个节点的情况在现代计算机上是可行的。4.3 一种更工程化的简化策略后向递推生成路径如果觉得上述优化方法过于复杂还有一种在自动泊车领域常用的启发式方法基于圆弧和直线组合的后向递推法。其原理是从目标泊车位姿车尾朝里开始向后即模拟倒车推演出能安全进入的“入口区域”然后再规划从起点到这个入口区域的路径。具体步骤以垂直停车为例确定目标姿态车辆完全停入车位时的后轴中心位置(x_g, y_g)和航向角θ_g通常为0即与车位平行。后向圆弧扩展从目标点开始以最小转弯半径R_min向左和向右分别画一段倒车圆弧对应于前轮打满方向得到两条圆弧轨迹。确定安全边界沿着这两条圆弧计算车辆轮廓是否会碰到车位边界或道路边界。找到车辆轮廓刚好与边界相切无碰撞的那个点作为“临界点”。连接起点从车辆起始点规划一条Reeds-Shepp曲线或Dubins曲线连接到上述临界点。这条连接路径需要是前进的。组合路径将“前进连接路径”与“从临界点到目标点的后向圆弧路径”拼接起来就得到了一条完整的“前进-倒车”组合路径。这种方法物理意义清晰计算速度快非常适合在竞赛中快速出方案。在Matlab中实现核心是几何计算和判断。5. Matlab代码实现要点与调试心得将上述模型和算法转化为可运行的Matlab代码是最后也是最关键的一步。这里分享一些核心代码模块和调试中容易踩的坑。5.1 主程序框架与模块化设计一个好的程序结构能极大提升开发和调试效率。建议按以下模块组织main.m: 主脚本定义问题参数车辆、车位、起终点调用规划函数并可视化结果。vehicle_model.m: 包含车辆参数结构体和状态更新函数state update_state(state, u, dt, vehicle)。collision_check.m: 碰撞检测函数is_collision check_collision(state, vehicle, parking_bounds)。reeds_shepp.m: 第三方或自己实现的Reeds-Shepp曲线生成函数。optimize_trajectory.m: 如果采用优化方法包含目标函数、约束函数的优化求解模块。plot_scenario.m: 专门用于绘图的函数绘制车位、车辆轮廓、轨迹等。5.2 可视化让问题与结果一目了然在调试阶段可视化至关重要。至少要实现以下绘图功能绘制停车场景画出车位矩形、道路边界。绘制车辆轮廓给定一个状态[x, y, θ]用patch或plot函数绘制出车辆的矩形轮廓并用不同颜色区分车头车尾。动画演示将规划好的路径离散成一系列状态用循环依次绘制车辆轮廓形成动画。这能最直观地检查碰撞和路径平滑性。figure; hold on; axis equal; rectangle(Position, [parking_xmin, parking_ymin, parking_width, parking_height], EdgeColor, b, LineWidth, 2); % ... 绘制其他边界 % 绘制路径上的车辆 for i 1:length(trajectory) state trajectory(i).state; corners get_vehicle_corners(state, vehicle); % 根据状态计算角点 patch(corners(:,1), corners(:,2), g, FaceAlpha, 0.3, EdgeColor, k); pause(0.05); % 控制动画速度 if i 1 delete(prev_patch); % 删除上一帧的车辆实现动画效果 end prev_patch patch(corners(:,1), corners(:,2), g, FaceAlpha, 0.3, EdgeColor, k); end5.3 常见问题与调试技巧路径不可行碰撞检查碰撞检测的参考点确保计算车辆角点时使用的参考点后轴中心或几何中心与运动学模型一致。安全裕度不足适当增加safety_margin。在转角处由于车辆是刚体旋转角点扫过的区域比静态轮廓大需要更大的裕度。离散精度不足在优化或路径采样时时间步长dt或距离步长过大可能会跳过中间的碰撞点。尝试减小步长或在两个节点之间进行更密集的碰撞检查。优化求解失败或速度慢提供好的初始猜测这是成功的关键。用Reeds-Shepp曲线生成的路径作为fmincon的初始值Z0远比随机初始化有效。缩放优化变量状态量x, y可能是几米到几十米而转向角φ是弧度-0.6~0.6控制量v和δ的量级也不同。这会导致Hessian矩阵条件数很差。尝试对变量进行缩放使它们处于相近的数量级如1~10。简化约束如果碰撞约束导致问题太复杂可以先尝试只优化一条满足运动学约束的平滑路径然后后验检查碰撞。如果不满足再调整起点、终点或中间点重新优化。路径曲率不连续在Reeds-Shepp曲线的段与段连接处转向角φ可能发生跳变。这在现实中是不可能的。解决方法是在优化问题中对控制输入的变化率即转向角加速度或加加速度施加约束或者在目标函数中加入对控制量变化的惩罚项∑ (δ_k - δ_{k-1})^2以平滑转向动作。倒车逻辑错误在运动学模型中倒车时v为负值。此时dθ/dt (v / L) * tan(φ)的符号会发生变化这意味着同样的方向盘转角在前进和倒车时车辆的瞬时转弯方向是相反的。这一点在拼接前进段和倒车段时务必注意。在动画中如果发现车辆倒车时转弯方向与预期相反请检查这里的符号处理。最后分享一个我个人在调试中的小技巧先验证单个模块再组合。比如先写一个脚本固定一个转向角让车辆匀速圆周运动看看轨迹是不是一个标准的圆半径是否符合R L / tan(φ)。再单独测试碰撞检测函数手动设置一些明显会碰撞和不会碰撞的状态看函数返回值是否正确。每个模块都确认无误后整个系统的调试就会顺利很多。构建一个完整的泊车模型从问题理解、数学抽象、算法选择到代码实现是一个系统工程。它考验的不仅是数学和编程能力更是将物理问题转化为可计算、可优化模型的思维能力。希望这篇基于Mathorcup赛题的详细拆解能为你解决类似的轨迹规划问题提供一个清晰的框架和实用的起点。在实际动手时不妨从最简单的几何法开始逐步增加优化和约束这样更容易把握问题的核心。