自动驾驶路径规划：从数学建模到轨迹优化实践

1. 项目概述从一道赛题看自动驾驶的“最后一公里”如果你在2022年参加过MathorCup或者对自动驾驶、数学建模感兴趣那么对C题“自动泊车”一定不陌生。这道题在当时引起了不小的讨论因为它精准地戳中了自动驾驶商业化落地中最具挑战性、也最贴近日常的环节之一——泊车。题目给出的场景并不复杂在一个已知尺寸的矩形停车场内给定车辆的初始位置、目标车位以及障碍物其他已停放车辆的位置要求我们规划出一条从起点到车位、且能安全泊入的路径。这听起来像是我们考驾照时的“倒车入库”模拟但背后涉及的数学、控制和工程问题却是一个完整的、微缩版的自动驾驶决策规划模块。我之所以对这个题目印象深刻是因为它完美地将一个前沿的工业问题抽象成了一个可供高校学生研究和求解的数学模型。它没有要求你造一辆真车而是让你聚焦于“大脑”的部分如何用数学语言描述车辆的运动如何在复杂的约束不碰撞、符合车辆运动学下找到一条最优或可行的路径这正是自动驾驶规划算法的核心。通过解析这道题我们不仅能学到数学建模的技巧更能一窥像百度Apollo这类成熟自动驾驶系统在解决类似问题时工程师们究竟在思考什么。无论是为了备战未来的数学建模竞赛还是想了解自动驾驶规划算法的入门原理这次对2022年MathorCup C题的深度拆解都将是一次从理论到实践的干货之旅。2. 赛题核心与建模思路拆解2.1 问题本质约束优化与运动规划拿到题目第一步是剥离场景看清本质。自动泊车路径规划问题在学术和工业界通常被归类为“非完整约束下的运动规划”问题。这里有两个关键词“非完整约束”和“运动规划”。“运动规划”好理解就是找一条从A到B的路。但“非完整约束”是车辆的物理特性带来的限制。最典型的非完整约束就是小汽车不能像螃蟹一样横向移动。它的运动方向必须与车轮朝向更准确地说是后轴中点的速度方向基本一致。这个约束用数学表达就是车辆的运动学模型。题目中通常会暗示或要求使用简化的车辆模型比如“自行车模型”。在这个模型里我们把四个轮子简化成前轮和后轮两个轮子并且假设车辆只在平面上运动。其核心微分方程是dx/dt v * cos(θ) dy/dt v * sin(θ) dθ/dt v / L * tan(δ)其中(x, y)是车辆后轴中心点的坐标θ是车头朝向与x轴夹角v是车速标量L是轴距前后轮距离δ是前轮转角。这个模型清晰地体现了非完整约束车辆位置(x, y)的变化率速度依赖于朝向θ而θ的变化率又依赖于前轮转角δ。这意味着你不能独立地指定x和y方向的速度路径的弯曲程度受到限制。因此整个问题的建模思路就清晰了在满足车辆运动学方程等式约束、避免与静态障碍物碰撞不等式约束、以及可能的速度/转角限制不等式约束的前提下寻找控制量车速v和前轮转角δ随时间的变化使得车辆从初始状态x0, y0, θ0运动到目标状态xf, yf, θf并且可能优化某个指标如时间最短、路径最平滑、能耗最低等。这构成了一个典型的最优控制问题或轨迹优化问题。2.2 主流求解方法对比与选择面对这样一个优化问题数学建模中常用的思路有以下几种各有优劣2.2.1 几何分解法适合初学求可行解这是最直观的方法。将复杂的泊车路径分解为几段简单的几何曲线组合例如“直线-圆弧-直线”序列。比如先直线倒车然后打满方向盘沿圆弧转动当车身与车位对齐时再直线倒入。这种方法的核心是几何计算。你需要根据车辆的最小转弯半径由最大前轮转角决定、起点和终点的相对位置通过解几何方程来确定切换点如圆弧的圆心、切点。优点概念简单计算速度快易于实现结果一目了然。缺点通常只能找到一条可行路径不一定是最优的对于复杂障碍物环境几何关系会变得极其繁琐甚至无法分解难以处理动态约束如速度平滑变化。在赛题中的应用如果题目障碍物较少对最优性要求不高此法可以作为基础方案快速拿到分数。论文中需要清晰画出几何关系图并给出推导公式。2.2.2 基于随机采样的规划算法如RRT, RRT* 这是机器人学和自动驾驶中非常流行的一类方法特别适合高维空间和复杂约束下的路径搜索。其核心思想是在车辆的状态空间x, y, θ中随机采样并尝试用局部控制器如遵循运动学模型连接这些采样点从而像一棵生长的树一样探索空间直到找到连接起点和终点的路径。优点概率完备性只要解存在给定足够时间总能找到能有效处理复杂障碍物和狭窄通道概念相对直观。缺点生成的路径通常不够平滑需要后处理随机性导致每次结果可能不同且不一定最优参数如步长、采样范围调优需要经验。在赛题中的应用这是体现建模“高级感”的选择。你可以详细阐述RRT*算法如何逐步构建搜索树并引入针对车辆运动学的扩展策略如使用Dubins曲线或Reeds-Shepp曲线连接两点这能保证连接路径本身符合车辆运动学。这能显著提升论文的理论深度。2.2.3 最优控制/轨迹优化法如直接法iLQR这是最“正统”的求解思路直接将问题建模为一个数值优化问题。我们把连续时间的控制问题在时间轴上离散成N个时刻状态变量x, y, θ和控制变量v, δ在每个时刻都有一个值。这样微分方程约束变成了N组等式约束碰撞避免变成了N组不等式约束。然后使用非线性优化求解器如IPOPT、SNOPT或在MATLAB中用fmincon来求解所有变量使得目标函数如总时间、控制量变化率平方和最小。优点能直接处理各种复杂约束并能求出在某种指标下的最优解生成的轨迹在时间和控制上都是平滑的。缺点建模复杂计算量大对初始猜测非常敏感如果初始猜测离可行解太远优化器可能失败或陷入局部最优需要一定的优化理论背景。在赛题中的应用这是冲击高奖项的“大杀器”。你需要展示如何将连续问题离散化构建目标函数和约束矩阵并讨论优化求解器的选择与初始化策略。在论文中呈现清晰的优化问题数学表述和求解流程框图极具说服力。提示对于数学建模竞赛一个有效的策略是“分层递进”。可以先使用几何法或RRT快速生成一条粗糙的可行路径将这条路径作为轨迹优化方法的初始猜测。这样既保证了求解的鲁棒性又通过优化提升了路径质量。在论文中详细描述这种“粗规划精优化”的框架能很好地体现建模的系统性思维。3. 关键细节车辆模型与碰撞约束的精确刻画3.1 车辆轮廓与碰撞检测建模题目中车辆和障碍物通常被简化为矩形。但碰撞检测不能简单地将车辆视为一个点。常见的处理方法是用多个圆形包络Bounding Circles或一个矩形框来近似车辆轮廓。圆形包络法将车辆的矩形轮廓用2个或4个完全覆盖它的圆来表示。碰撞检测就变成了判断这些圆是否与障碍物矩形或其他车辆的包络圆相交。圆心通常取在车辆中轴线上半径要能覆盖车角。这种方法计算简单只需计算圆心到矩形边的最短距离且是保守的保证了安全在规划中广泛使用。实操要点圆的个数和位置需要仔细设计。两个圆分别覆盖前后半车身是常用折中方案。在论文中需要画出示意图并给出圆心位置相对于车辆后轴中心点(x, y)和朝向θ的计算公式。矩形框法直接使用车辆的实际矩形。碰撞检测需要判断两个旋转矩形是否相交这比圆-矩判断复杂通常需要用到分离轴定理SAT。虽然更精确但计算量稍大。实操要点在编程实现时可以先将障碍物顶点坐标变换到车辆坐标系下然后判断是否有顶点在车辆矩形内或者利用投影法。在数学建模论文中如果采用此法务必给出清晰的数学判定条件。安全裕度无论用哪种方法必须引入一个安全裕度Safety Margin。即在计算距离时将车辆轮廓稍微“膨胀”一点或者要求距离大于一个正数阈值如0.2米。这是工程实践中的必备操作用于补偿模型误差、控制误差和传感器噪声。3.2 运动学离散化与数值积分当我们采用轨迹优化或某些基于采样的方法时需要将连续时间的运动学方程进行离散化。最常用的是欧拉法x[k1] x[k] v[k] * cos(θ[k]) * dt y[k1] y[k] v[k] * sin(θ[k]) * dt θ[k1] θ[k] v[k] / L * tan(δ[k]) * dt这里k是时间步索引dt是离散时间间隔。这个方程就变成了优化问题中的等式约束。注意事项dt的选择至关重要。太大离散误差大轨迹不精确甚至可能“跳过”障碍物导致漏检碰撞太小优化问题的变量数N总时间/dt会剧增计算负担加重。通常需要根据车辆最大速度和环境尺度进行试凑例如保证v_max * dt小于车辆最小尺寸的一半。更精确的方法对于要求高的场景可以使用更高阶的积分方法如二阶龙格-库塔法。但在数学建模竞赛中欧拉法因其简单直观且易于融入优化框架通常是首选。3.3 控制量与状态量的约束设定合理的约束是保证解符合物理实际的关键。前轮转角δ约束|δ| ≤ δ_max。δ_max决定了车辆的最小转弯半径R_min L / tan(δ_max)。这是车辆固有的物理限制。车速v约束泊车过程速度较慢但一般也需要设定范围例如-v_max_reverse ≤ v ≤ v_max_forward。倒车时v为负值。控制变化率约束为了保证乘坐舒适性和执行器方向盘电机、刹车油门的可行性通常需要对控制量的变化率加以限制例如|(δ[k1] - δ[k]) / dt| ≤ ω_steer_max方向盘角速度限制|(v[k1] - v[k]) / dt| ≤ a_max加速度限制。这些约束能有效避免规划出的轨迹出现方向盘瞬间打满或急加速急刹车的情况。4. 基于轨迹优化方法的完整实现流程这里我们以非线性轨迹优化直接法为例展示一个相对完整、可复现的求解流程。我们假设使用MATLAB环境因为其优化工具箱对数学建模竞赛友好。4.1 问题数学表述定义总时间步数N时间间隔dt。令状态向量为s [x, y, θ]^T控制向量为u [v, δ]^T。 决策变量X [s1, u1, s2, u2, ..., sN, uN] 注意sN1通常也作为变量但由sN和uN通过动力学约束关联。 目标函数最小化总时间T N * dt或者最小化控制量的变化使轨迹平滑J Σ (δ[k1] - δ[k])^2 (v[k1] - v[k])^2。 约束条件动力学约束对于k1 to Ns[k1] f(s[k], u[k])欧拉离散化公式。初始状态约束s[1] [x0, y0, θ0]。终端状态约束s[N1] [xf, yf, θf]允许一个小的容忍误差。控制量边界约束v_min ≤ v[k] ≤ v_max,|δ[k]| ≤ δ_max。控制变化率约束|(u[k1] - u[k]) / dt| ≤ du_max。碰撞避免约束对于所有时间步k和所有障碍物obsdist(vehicle_shape(s[k]), obs) ≥ d_safe。4.2 MATLAB实现步骤与代码要点步骤1定义问题参数和初始猜测L 2.8; % 轴距 (米) dt 0.5; % 时间间隔 (秒) 可调整 N 20; % 时间步数 总时间 T N*dt % 边界值 v_max 2; v_min -1.5; % 最大前进/倒车速度 (m/s) delta_max deg2rad(35); % 最大前轮转角 (弧度) % 初始和终点状态 s0 [0, 10, pi/2]; % (x0, y0, theta0) sF [6, 2, 0]; % (xf, yf, thetaf) % 障碍物 [x_center, y_center, length, width] obstacles [3, 5, 4.8, 1.8; 7, 3, 4.8, 1.8];生成初始猜测至关重要。一个简单的方法是线性插值状态并假设控制量为零X0 []; for k 1:N1 alpha (k-1)/N; s_guess s0 alpha * (sF - s0); % 线性插值位置朝向需要特殊处理 s_guess(3) s0(3) alpha * (angdiff(s0(3), sF(3))); % 处理角度差值 X0 [X0; s_guess]; end for k 1:N u_guess [0; 0]; % 初始猜测速度和控制为0 X0 [X0; u_guess]; end更好的初始猜测可以用4.1节提到的几何法或RRT先生成一条粗略路径然后采样得到各点的状态和控制量。步骤2调用优化求解器fmincon% 定义变量上下界 lb []; ub []; for i 1:N1 lb [lb; -inf; -inf; -inf]; % 状态无硬性边界除初始和终点 ub [ub; inf; inf; inf]; end for i 1:N lb [lb; v_min; -delta_max]; ub [ub; v_max; delta_max]; end % 设置优化选项 options optimoptions(fmincon, Display, iter, Algorithm, interior-point, ... MaxFunctionEvaluations, 1e5, MaxIterations, 1000, ... SpecifyConstraintGradient, false); % 对于复杂约束梯度可设置为false % 非线性约束函数 nonlcon % 该函数需要返回约束违反值 [c, ceq]。 % ceq 包含动力学约束和终端状态约束。 % c 包含碰撞避免约束应 0。 function [c, ceq] parking_constraints(X) % 解析决策变量X 提取状态和控制序列 % 计算动力学约束 ceq_dyn % 计算终端约束 ceq_terminal % 计算所有时间步、所有障碍物的距离约束 c_collision ceq [ceq_dyn; ceq_terminal]; c c_collision; end % 目标函数 objective function J parking_objective(X) % 例如最小化控制变化率 % 解析X提取控制序列u % J sum(diff(v).^2) sum(diff(delta).^2); end % 调用求解器 [X_opt, fval, exitflag] fmincon(parking_objective, X0, [], [], [], [], lb, ub, parking_constraints, options);步骤3结果解析与可视化优化完成后从X_opt中解析出状态序列和控制序列。% 解析状态轨迹 traj_x X_opt(1:6:end); % 假设变量排列为 [x1,y1,θ1, v1,δ1, x2,y2,θ2, v2,δ2, ...] traj_y X_opt(2:6:end); traj_theta X_opt(3:6:end); % 解析控制序列 % ... % 绘制结果 figure; hold on; grid on; axis equal; rectangle(Position, [s0(1)-L/2, s0(2)-1, L, 2], EdgeColor, b, FaceColor, none); % 绘制初始车辆 rectangle(Position, [sF(1)-L/2, sF(2)-1, L, 2], EdgeColor, g, FaceColor, none); % 绘制目标车辆 % 绘制障碍物 for i 1:size(obstacles,1) rect obstacles(i,:); rectangle(Position, [rect(1)-rect(3)/2, rect(2)-rect(4)/2, rect(3), rect(4)], ... EdgeColor, r, FaceColor, [1, 0.8, 0.8]); end % 绘制优化轨迹 plot(traj_x, traj_y, k-o, LineWidth, 1.5, MarkerSize, 4); % 可以在轨迹上选几个点绘制车辆轮廓更直观 for k 1:5:N1 draw_car(traj_x(k), traj_y(k), traj_theta(k), L); end xlabel(X (m)); ylabel(Y (m)); title(优化后的自动泊车轨迹); legend(起点, 终点, 障碍物, 规划路径);注意fmincon求解此类问题的挑战碰撞约束是非凸的且数量众多时间步数×障碍物数。这会导致优化问题存在大量局部最优解且求解器可能收敛到不可行点。因此一个高质量的初始猜测和适当的约束松弛例如允许碰撞约束有微小的违反但加入惩罚项到目标函数是成功的关键。在竞赛中如果能处理好这些问题并在论文中深入讨论将是巨大的亮点。5. 竞赛实战技巧与常见问题排查5.1 模型简化与复杂度权衡数学建模竞赛时间有限必须做出合理的简化。车辆模型自行车模型是绝对的主流和首选。不要试图引入更复杂的动力学模型如考虑轮胎滑移这会使问题难度呈指数增长且对泊车低速场景收益甚微。障碍物严格按照题目说明通常视为静态矩形。不必考虑动态障碍物或不确定性。目标函数优先选择最简单的可行目标。例如“最小化路径长度”或“最小化控制量变化”比“最小化时间”更容易处理因为后者需要将时间也作为优化变量并可能涉及离散时间间隔的变化问题会更复杂。可以先实现一个基础目标如果时间充裕再尝试更复杂的目标进行对比。碰撞检测圆形包络法在竞赛中是最实用的。用2个或4个圆来近似车辆矩形能极大简化距离计算将问题转化为计算圆心到矩形边的最短距离这个计算有解析公式速度快。5.2 算法实现与调试心得“先验可行解”的重要性在尝试复杂的轨迹优化前务必先实现一个能生成可行路径的简单方法比如几何法或一个非常粗糙的RRT。这个路径有两个关键作用一是验证你整个流程模型、约束、可视化是否正确二是为后续优化提供至关重要的初始猜测。没有好的初始值非线性优化几乎注定失败。分阶段验证不要一次性写完所有代码然后调试。应该第一步先写一个无碰撞约束的版本只优化动力学和边界约束看车辆能否从起点运动到终点。这会帮你排除动力学模型和优化框架的基本错误。第二步引入单个障碍物的碰撞约束调试碰撞检测函数是否正确。第三步处理多个障碍物和所有约束。可视化是最高效的调试工具每一步都要画图。画出初始猜测路径、优化后的路径、车辆在每个时间步的轮廓、障碍物位置。图形能立刻告诉你问题出在哪里是碰撞了还是根本没动或者是路径极其怪异参数调优顺序先调dt和N它们决定了问题的粒度。可以先设一个较大的dt如1秒和较小的N让优化快速跑通。然后再减小dt、增加N以提高精度。再调优化器选项fmincon的MaxIterations和MaxFunctionEvaluations要设得足够大。Algorithm可以尝试‘interior-point’或‘sqp’。最后处理数值问题如果优化失败检查梯度是否异常启用CheckGradients选项或者考虑对目标函数和约束进行缩放归一化使所有变量量级相近。5.3 常见问题速查与解决方案问题现象可能原因排查与解决思路优化失败提示“无可行解”1. 初始猜测不可行如与障碍物相交。2. 约束过紧如安全距离太大。3. 终端状态约束太严格。1. 可视化初始猜测路径确保它不穿障。2. 暂时调小安全距离d_safe或将其从硬约束改为目标函数中的惩罚项软约束。3. 放宽终端状态约束允许一个小的误差范围。优化结果路径很奇怪比如原地打转1. 目标函数定义不合理。2. 控制量变化率约束太松。3. 陷入了局部最优。1. 检查目标函数例如最小化控制变化率可以避免不必要的抖动。2. 加入方向盘角速度限制和加速度限制。3. 尝试不同的初始猜测或使用全局优化算法如多起点优化但在竞赛中时间成本高。优化速度极慢1. 时间步数N过多。2. 碰撞约束计算复杂度高。3. 问题规模太大超出了求解器能力。1. 增加dt减少N。2. 简化碰撞检测模型改用圆形包络。3. 考虑使用更高效的求解器如IPOPT的MATLAB接口或在代码层面优化约束计算向量化操作。车辆模型“飘移”不遵循运动学动力学约束欧拉离散方程编写错误或约束力不够。仔细核对离散化公式代码。确保在约束函数nonlcon中动力学约束ceq_dyn的误差被正确计算并返回。可以用一个简单的无约束运动测试你的动力学函数。论文中算法对比不清晰只实现了一种方法或对比维度单一。至少实现两种不同思路的方法如几何法和优化法。对比维度应包括计算时间、路径长度、安全性最小障碍距离、平滑度控制量变化。用表格和对比图呈现结论更有力。5.4 论文写作点睛之笔问题重述不是照抄用自己的话精炼概括问题并立即给出你对问题的数学本质的理解“这是一个带非完整约束和避障约束的最优控制问题”这能瞬间提升论文档次。模型假设要明确且合理列出“自行车模型”、“障碍物静态且形状已知”、“车辆轮廓用圆形包络近似”等假设并简要说明其合理性。图表并茂一图胜千言除了最终的路径图还应包含车辆与圆形包络示意图、坐标系定义图、算法流程图尤其是你采用的“粗规划精优化”框架、不同方法或不同参数下的对比图。灵敏度分析这是拿高分的关键。讨论某个关键参数如安全距离d_safe、最大转角δ_max的变化如何影响规划结果成功率、路径长度、计算时间。这体现了你对模型鲁棒性的思考。模型评价与推广客观评价自己模型的优缺点。例如“本文模型基于精确的车辆运动学能生成平滑可行的轨迹但未考虑执行器延迟和定位误差。未来可考虑加入反馈控制层进行轨迹跟踪并引入不确定性分析。” 这展示了你的思维深度。回过头看2022年MathorCup C题就像是一个精心设计的引子它将我们引入了自动驾驶规划这个广阔而有趣的领域。通过这道题我们实践了如何将一个具体的工程问题转化为数学模型并尝试用不同的数学工具去求解。我个人最深的体会是在数学建模中“先求可行再求最优”是一条黄金法则。与其纠结于一个复杂模型而无法得到任何结果不如先搭建一个简单但能跑通的框架然后再逐步迭代、完善和优化。自动泊车问题中从几何法到轨迹优化的递进正是这一思维的体现。这道题也让我明白理论上的优美和工程上的可行常常需要权衡而一个好的模型恰恰是在这两者之间找到了最佳的平衡点。