1. 从一道赛题看自动驾驶的“调头”难题如果你关注过国内的数学建模竞赛或者对自动驾驶技术有点兴趣那你大概率听说过“MathorCup”这个名字。2021年的A题直接把一个看似简单、实则暗藏玄机的场景抛了出来自动驾驶车辆的调头问题。题目描述很直接就是让一辆车在一条双向四车道的道路上完成一次安全的调头操作。听起来是不是像驾校科目二里的“掉头”但当你真正开始用数学模型和算法去描述它时就会发现这远不是打一把方向那么简单。这道题之所以经典是因为它精准地戳中了自动驾驶从“实验室Demo”走向“真实复杂路况”的一个关键痛点。在封闭测试场或者结构化的高速路上车辆大多沿着车道线行驶决策逻辑相对简单。但调头这个动作是一个典型的强交互、高不确定性、多约束耦合的复杂场景。它要求车辆不仅要规划出一条从A点到B点的几何路径更要在这条路径上动态地处理与对向来车、同向车辆、交通规则如虚实线以及自身动力学极限的实时博弈。很多刚接触这个领域的朋友可能会把轨迹规划和避障当成两个独立的问题先规划一条理想路径再往上面叠加一个避障模块。但MathorCup 2021 A题的现实意义就在于它告诉你在调头这种场景下路径、速度、安全、规则是必须被一体化考虑和优化的。你不能先画一条完美的弧线再发现对面来车根本不让也不能为了绝对安全把车停在路口一直等待那又失去了通行的效率。这道题考察的正是这种在多重约束下寻找“最优解”或“满意解”的系统性思维。从更广的视角看这道题所涉及的核心——轨迹规划与动态避障——正是当下自动驾驶、移动机器人乃至机械臂控制领域最活跃的研究方向之一。无论是七轴机械臂在复杂环境中的灵巧作业还是扫地机器人在桌椅腿间穿梭其底层逻辑都是相通的如何在充满不确定性的环境中生成一条安全、平滑、高效的执行轨迹。因此解这道题的过程实际上是一次对机器人决策规划核心思想的深度演练。接下来我们就抛开抽象的题目描述深入到技术细节中看看要解决这样一个问题我们需要搭建怎样的思维框架又会遇到哪些实实在在的“坑”。2. 问题拆解调头场景下的四大核心约束面对“自动驾驶车辆调头”这个问题第一步不是急着写公式或编代码而是要把这个开放性问题拆解成一系列可量化、可建模的具体约束。这就像医生看病得先弄清楚病灶在哪里。根据题目描述和实际交通场景我们可以梳理出四大类核心约束它们共同构成了一个复杂的优化空间。2.1 几何约束车辆不是质点道路也有边界首先是最基础的几何约束。我们的车辆不是一个可以任意旋转的点它是一个有长度、宽度的刚体。在调头过程中我们必须确保车辆的整个外廓通常用一个矩形包络来近似在任何时刻都不与道路边界如路缘石、隔离带发生碰撞。这就引出了车辆运动学模型的选择。对于低速调头场景通常车速低于30km/h我们一般可以忽略复杂的轮胎动力学采用阿克曼转向几何模型。这个模型假设车辆四个轮子的速度方向都指向同一个瞬时转动中心。它的状态通常用(x, y, θ, v, φ)来表示分别是车辆后轴中心点的坐标、航向角、速度和前轮转角。有了这个模型我们才能准确地计算出车辆在未来一段时间内每一个位姿下所占用的空间区域也就是所谓的占用区域。道路约束则相对直观。双向四车道意味着有明确的车道线。调头通常需要在路口或允许调头的虚线处进行。这里的一个关键细节是调头轨迹的起始点和终点必须位于合法的车道区域内。你不能从实线开始调头也不能调头后压着实线结束。此外调头过程往往需要跨越对向车道这就必须考虑对向车道的宽度确保车辆在跨越时其占用区域不会侵入对向车道正常行驶车辆的安全空间。将这些几何约束用数学不等式表达出来是构建规划模型的第一步。2.2 动力学约束方向盘不能打太快油门也不能踩太猛即使几何上存在一条通路我们的车也未必能开过去因为它受限于自身的物理特性。这就是动力学约束它决定了轨迹的“平滑性”和“可行性”。主要有两类动力学约束运动学约束主要由转向系统决定。前轮转角φ及其变化率φ即方向盘转动速度是有上限的。|φ| ≤ φ_max决定了车辆的最小转弯半径这直接影响了调头所需道路的宽度——转弯半径不够小在窄路上就转不过来。|φ| ≤ ω_max则限制了轨迹的曲率变化率如果方向盘打得忽快忽慢生成的轨迹就会很“生硬”乘客会感到不适控制模块也很难精准跟踪。动力学约束主要由动力和制动系统决定。车辆的纵向加速度a含加速和减速有上下限。a_min ≤ a ≤ a_max。这限制了车辆在调头过程中速度变化的剧烈程度。例如为了等待对向来车而急刹或者为了快速通过而急加速都必须在这个范围内。通常为了保证舒适性我们对加速度的变化率加加速度jerk也会有所限制。在数学建模时我们通常将车辆的运动状态位置、速度、加速度和控制输入前轮转角、加速度随时间的变化关系用微分方程来描述。规划出的轨迹必须是这个微分方程的一个可行解。2.3 交通规则与交互约束最重要的部分也是最难的部分这是调头问题中最具挑战性的一环也是MathorCup这道题的精髓所在。它不再是单车优化而是进入了多智能体博弈的范畴。规则约束最典型的就是车道线规则。调头起始必须在虚线处过程中车轮不能长时间压实线。在有些建模中我们可以将实线区域设置为“不可穿越”的硬约束或者通过一个很高的代价函数来惩罚压线行为。动态避障约束这是核心中的核心。对向车道有来车同向车道可能有前车或后车。我们不仅要避免静态碰撞还要预测其他交通参与者的未来轨迹并做出反应。这里常用的方法是引入时空联合规划的概念。我们不再只规划空间中的一条路径而是规划一条“轨迹”即路径随时间的变化s(t)。这样车辆在时空坐标系中就是一个不断延伸的“管道”。其他车辆也是如此。避障约束就转化为本车在时空中的管道不能与其他车辆的预测管道发生重叠。但问题来了其他车辆的未来轨迹是未知的我们只能预测。常见的简化假设有恒定速度假设假设对方保持当前车速和车道行驶。这是最常用的但显然过于理想尤其在对方可能因我车调头而减速时。交互式预测假设对方是一个理性的驾驶员也会根据我车的意图做出反应如减速让行。这需要引入博弈论模型如纳什均衡或社会价值取向模型复杂度急剧上升。最坏情况假设假设对方不采取任何避让措施。这样规划出的轨迹最保守但可能导致过度谨慎甚至永远无法找到调头时机。在竞赛的有限时间内采用“恒定速度假设安全时距”是一个务实的选择。即只要本车在穿越冲突点时与预测的对方车辆到达该点的时间差大于一个安全阈值如3秒就认为是安全的。2.4 舒适性与效率约束寻找最佳平衡点在满足了安全、合规、可行的硬约束之后我们还要追求“开得好”。这主要体现在舒适性和效率上它们通常作为优化目标而不是硬约束。舒适性主要惩罚控制量的剧烈变化。例如在目标函数中最小化加速度a的平方积分减小急加急刹、最小化前轮转角变化率φ的平方积分减小方向盘猛打、最小化轨迹的曲率κ及其变化率使路径平滑。J_comfort ∫(w1*a² w2*φ² w3*κ²) dt效率主要希望尽快完成调头动作。最简单的就是最小化总时间T。但单纯追求时间最短可能会产生不舒适甚至不安全的轨迹。因此更常见的做法是结合舒适性指标形成一个多目标优化问题J_total w_t * T J_comfort。通过调整权重系数w_t,w1,w2,w3我们可以在这两者之间进行权衡。比如在车流密集时可能更注重安全性和机会捕捉效率对舒适性要求放宽而在车流稀疏时则可以追求平顺舒适的乘坐体验。3. 方法选型从搜索到优化哪种路径规划算法更合适明确了约束和目标接下来就要选择“武器”——规划算法。没有一种算法是万能的不同的算法在求解质量、计算效率和实现难度上各有千秋。针对调头这个中等复杂度、需要精细控制的场景我们主要对比几类主流方法。3.1 基于图搜索的方法A* 与 Hybrid A*A算法* 是路径规划领域的经典算法。它的核心思想是在离散的状态空间如栅格地图中通过评估函数f(n) g(n) h(n)来寻找从起点到终点的最优路径。其中g(n)是从起点到节点n的实际代价h(n)是从节点n到终点的预估代价启发函数。在调头问题中我们可以将车辆的状态如(x, y, θ)离散化构建一个状态栅格。A* 可以很好地融入障碍物其他车辆信息只要将其所在栅格标记为不可通行即可。但是A有两个明显缺点*第一它搜索的是路径Path而不是带时间信息的轨迹Trajectory因此难以直接处理动态障碍物。第二由于状态离散化特别是加入了方向θ后搜索空间会急剧膨胀“维度灾难”计算量很大。Hybrid A* 是针对车辆运动学特性改进的 A* 算法。它不再使用固定的栅格而是使用与车辆运动学模型相匹配的“运动基元”来连接状态。这些运动基元是一小段预先计算好的、符合车辆运动学约束的轨迹片段如直行5米、左转3米等。Hybrid A* 在状态空间中搜索这些运动基元的组合从而直接得到一条运动学可行的路径。它比传统 A* 更适用于车辆规划生成的路径更平滑。然而它依然主要解决静态环境下的路径问题对于动态避障通常需要在其生成的路径基础上再进行速度规划属于“路径-速度解耦”的范式在动态密集场景下可能不是最优。3.2 基于采样的方法RRT 与它的家族快速探索随机树RRT及其变种如 RRT*是另一大类规划算法。它的思路很直观从起点开始随机在状态空间中采样一个点然后尝试用一条简单的路径如直线连接树上最近的节点到这个采样点如果这条路径是可行的无碰撞就把这个新点加入到树中。如此反复直到树扩展到目标点附近。RRT 的优点在于它能快速在高维空间中找到一条可行路径对动态障碍物也有一定的适应性可以通过在已规划路径上重规划来避障。但其缺点也很突出生成的路径通常是折线非常不光滑完全不符合车辆的运动学特性需要后处理进行平滑。虽然 RRT* 能渐进最优但收敛速度慢。对于调头这种对轨迹平滑度要求很高的场景原始的 RRT 并不直接适用需要结合曲线拟合和优化。3.3 基于优化的方法最优控制与模型预测控制MPC这是目前学术界和工业界在轨迹规划上更主流和前沿的方向。其核心思想是将规划问题直接建模为一个有约束的数值优化问题。我们定义一个目标函数如上一节提到的总代价J_total以及一系列约束运动学/动力学方程、碰撞避免、交通规则等。然后使用数值优化算法如序列二次规划 SQP、内点法 IPM来求解控制输入序列u(t)方向盘转角、加速度使得目标函数最小同时满足所有约束。模型预测控制MPC是这类方法的一个非常强大的框架。MPC 在每个控制周期如0.1秒都执行一次有限时域如3秒的优化基于当前状态和环境预测优化得到未来一段时域内的最优控制序列但只执行第一个控制指令。到下一个周期根据新的状态重新进行优化。这种“滚动优化”的方式让 MPC 具备了强大的反馈校正能力能够很好地处理模型误差和外部干扰如其他车辆未按预测行驶。对于调头问题MPC 框架具有天然优势一体化可以同时优化路径和速度处理时空联合约束。动态性在每个周期都重新考虑最新的环境信息实现真正的动态避障。约束处理能够以统一的形式处理各种硬约束和软约束。当然它的缺点是计算负担重。在线求解一个非线性、非凸的优化问题对计算平台要求很高。不过随着硬件的发展和优化算法的改进MPC 在自动驾驶领域的应用已越来越广泛。综合对比与选择建议对于 MathorCup 这类竞赛场景计算资源相对充足更追求方案的先进性和完整性。因此采用基于优化的方法特别是 MPC 框架是一个很有竞争力的选择。它能够最直接、最优雅地描述我们前面拆解的所有约束和目标。如果担心在线优化实时性可以采用“分层规划”的思路上层用一个轻量级的 Hybrid A* 或车道级路由生成一条粗略的参考路径下层用 MPC 进行局部精细轨迹优化和动态避障。这样既保证了全局可行性又获得了局部最优性。4. 建模实战如何构建一个可求解的 MPC 调头模型理论说了这么多我们动手把它变成一个可以写进论文和代码的数学模型。这里我们以非线性 MPC 为例展示一个相对完整的建模过程。请注意这是一个简化的示例旨在说明核心思想。4.1 车辆模型与状态空间定义我们采用自行车模型阿克曼转向简化版。定义在惯性坐标系下的状态向量x和控制向量u为x [X, Y, φ, v]^T。这里做了简化用横摆角φ代表航向v为车速。u [a, δ]^T。a为纵向加速度δ为前轮转角。其微分方程连续时间模型为Ẋ v * cos(φ) Ẏ v * sin(φ) φ̇ v / L * tan(δ) // L 为轴距 v̇ a为了用于离散时间的 MPC 求解我们需要对其进行离散化例如采用欧拉法采样时间为dtX_{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 v_{k1} v_k a_k * dt这个离散模型x_{k1} f(x_k, u_k)将作为 MPC 中的预测模型。4.2 约束的数学表达这是建模的关键需要将自然语言描述的约束转化为数学不等式或等式。控制量约束物理极限与舒适性a_min ≤ a_k ≤ a_max, k0,..., N-1 δ_min ≤ δ_k ≤ δ_max, k0,..., N-1 Δδ_min ≤ (δ_k - δ_{k-1})/dt ≤ Δδ_max, k1,..., N-1 // 转向速率限制状态量约束v_min ≤ v_k ≤ v_max (道路限速), k0,..., N 0 ≤ v_k (不倒车), k0,..., N // 假设调头过程不允许倒车道路边界约束矩形区域 假设调头区域是一个已知的矩形其边界为[X_min, X_max, Y_min, Y_max]。X_min ≤ X_k ≤ X_max, k0,..., N Y_min ≤ Y_k ≤ Y_max, k0,..., N动态避障约束最核心也最复杂 假设对向有一辆车B我们预测其未来轨迹为(X_B(t), Y_B(t))并假设其外形可用一个圆来保守近似半径为R_B。本车外形也近似为半径为R_ego的圆。那么在每一个预测时刻k两车圆心之间的距离必须大于安全半径之和(X_k - X_B(k*dt))^2 (Y_k - Y_B(k*dt))^2 ≥ (R_ego R_B d_safe)^2, k0,..., N其中d_safe是额外的安全裕度。这是一个非线性、非凸约束会给优化求解带来困难。在实际中可能会采用线性化、或者将其转化为控制障碍函数CBF等形式来处理。4.3 目标函数的设计目标函数引导优化器找到我们想要的轨迹。一个典型的设计如下J Σ_{k0}^{N-1} ( w_a * a_k^2 w_δ * δ_k^2 w_Δδ * (δ_k - δ_{k-1})^2/dt^2 ) * dt w_v * (v_N - v_ref)^2 w_pos * [(X_N - X_goal)^2 (Y_N - Y_goal)^2] w_φ * (φ_N - φ_goal)^2我们来拆解一下第一项求和项是运行代价旨在最小化控制努力和提高舒适性。惩罚大的加速度急刹急加、大的前轮转角转向过猛以及大的转向速率方向盘打得快。第二项终端速度代价希望规划结束时车速接近一个期望值v_ref如调头完成后的正常车速。第三、四项终端状态代价这是最重要的。它强制要求规划时域N步结束时的车辆位置(X_N, Y_N)和航向φ_N尽可能接近我们的调头目标点(X_goal, Y_goal, φ_goal)。没有这项优化器可能会生成一条原地不动最省力的“轨迹”。权重系数w_*需要仔细调节。例如在调头初期为了寻找插入车流的时机可能对终端位置权重w_pos设置得稍低而对舒适性权重w_a, w_δ设置得较高允许车辆更灵活地调整速度和方向。当确认安全窗口后则可以增大w_pos让车辆更果断地完成调头动作。4.4 求解与实现要点将上述模型预测模型、约束、目标函数整合起来就形成了一个标准的非线性规划NLP问题。我们可以使用现成的求解器来求解例如IPOPT开源、SNOPT商业或ACADO自动生成高效代码的工具包。在实现时有几个关键点需要注意初始猜测非线性优化求解器非常依赖于初始猜测。一个好的初始猜测能极大提高收敛速度和成功率。我们可以用一条简单的几何曲线如回旋线或多项式曲线连接起点和终点并分配一个粗略的速度曲线作为优化变量的初始值。实时性MPC 需要在几十到几百毫秒内完成求解。除了选择高效求解器控制时域N和采样时间dt的选择至关重要。N太大问题维度过高N太小预见性不足。通常需要折中例如N30,dt0.1s预测未来3秒。不确定性处理我们之前的模型假设了对向来车轨迹是确定已知的。实际上这是预测的。更鲁棒的做法是考虑预测的不确定性例如采用鲁棒 MPC或随机 MPC规划一条在最坏情况或概率意义下安全的轨迹。但这会进一步增加计算复杂度。通过这样的建模我们就把一个生动的驾驶场景转化为了一个严谨的数学优化问题。求解这个问题的输出就是车辆在未来一段时间内每一时刻应该执行的方向盘转角和油门/刹车指令。5. 从仿真到现实模型验证与典型“坑点”分析建好了模型写出了求解代码在仿真环境中看着小车完美地避开虚拟车辆完成调头这远不是终点。从仿真到现实中间隔着一道名为“模型失配”和“ corner case”的鸿沟。很多在仿真中运行良好的算法一上真车或者遇到复杂情况就“趴窝”。下面结合调头场景聊聊那些必须提前考虑的验证环节和常见“坑点”。5.1 仿真验证的层次莫在浮沙筑高台仿真不是一次性的而是一个多层次、逐步逼近真实的过程。算法逻辑验证无动力学、无噪声 这是最初级的阶段。在一个完全理想的二维平面上其他车辆按预设轨迹匀速运动本车模型完美传感器无延迟无噪声。这个阶段的目标是验证核心规划算法的逻辑正确性目标函数是否导向期望行为约束是否被正确施加和满足避障逻辑是否生效在这个阶段你可能会发现目标函数权重设置不合理导致车辆“犹豫不决”或者碰撞约束存在漏洞导致轨迹穿车而过。这个阶段要用大量极端场景如对向来车突然加速进行压力测试。控制闭环验证加入底层控制器 规划模块输出的是期望的轨迹一系列位姿和速度车辆能否准确跟踪这条轨迹取决于底层控制器如横向的LQR/Stanley控制器纵向的PID控制器的性能。在这个仿真中你需要接入一个车辆动力学模型如CarSim、VeDYNA模型或简单的二自由度动力学模型并用控制器去跟踪规划轨迹。这时你会发现如果规划出的轨迹曲率变化过快方向盘打得太急控制器可能无法跟上产生较大的跟踪误差这个误差反过来可能使实际轨迹侵入安全边界导致碰撞风险。因此在规划阶段就必须考虑轨迹的跟踪可行性这就是为什么要在目标函数中惩罚控制量的变化率。感知-预测-规划闭环验证加入不确定性 这是最高级的仿真阶段。你需要模拟一个完整的感知链路传感器如激光雷达、摄像头生成带有噪声和延迟的点云或检测框预测模块基于不完美的感知数据输出带有概率分布的未来轨迹规划模块再基于这个不确定的预测进行决策。在这个阶段你会遭遇真实世界最核心的挑战信息不完整和决策延迟。例如感知模块可能漏检一辆车或者预测错误其意图直行还是左转。规划算法必须具备一定的容错性和鲁棒性。常用的策略包括增大安全裕度d_safe采用概率性碰撞检查PCC或者规划多条备选轨迹最后时刻根据最新信息选择。5.2 调头场景下的典型“坑点”与对策即使通过了层层仿真一些场景特有的问题依然会在细节处埋雷。坑点一对向来车博弈与“冻结机器人”问题这是最经典的难题。你的车在路口等待调头对向车道一直有车流。如果你的避障约束是严格的“时空管道不重叠”且预测对方恒定速度那么算法可能永远找不到一个绝对安全的空隙车辆会一直等待成为“冻结机器人”。对策引入“礼貌性”或“交互性”假设。例如可以建模对向车辆在发现本车有调头意图时有一定概率会减速让行。这可以通过在预测模型中增加一个基于距离和时间的减速概率来实现。或者采用更激进的策略规划一条“迫使”对方减速的轨迹但需非常谨慎确保安全底线。另一种务实的方法是在目标函数中增加“等待时间”惩罚让车辆在安全的前提下愿意接受一个较小的、经过计算的风险窗口。坑点二轨迹末端抖动与控制不匹配在仿真中规划轨迹的终点完美地对准了目标车道中心。但实际控制中车辆在接近终点时由于跟踪误差和模型失配可能产生轻微的横向摆动导致车辆停稳后没有完全摆正或者压到了车道线。对策在规划时不要将终端状态约束设为一个精确的点而应设为一个区域例如目标车道中心线附近±0.3米的范围。同时在目标函数的终端代价中不仅考虑位置更要重视终态的速度和角速度是否为零即是否达到静止稳定状态。这能引导规划器生成一条在末端更平缓、更易于稳定停车的轨迹。坑点三复杂道路几何与曲率连续题目中是标准的十字路口。但现实中调头区域可能是一个狭窄的“灯泡型”匝道或者有绿化带导流岛。这时单纯用起点和终点状态进行规划可能无法生成贴合道路几何的轨迹甚至可能规划出穿过导流岛的非法路径。对策引入参考路径。先通过高精度地图或车道级导航获取一条描述调头区域中心线的参考路径s_ref。然后在 MPC 的优化问题中增加一个惩罚项使车辆的实际横向偏移y相对于这条参考路径尽可能小。同时参考路径的曲率信息也可以用来指导规划确保生成轨迹的曲率变化是连续的避免方向盘突变。坑点四计算实时性与异常处理在车流密集的路口优化问题可能因为初始猜测差、约束突然收紧近距离切入车辆而求解失败或超时。规划模块必须有降级策略。对策实现一个分层架构。上层 MPC 作为主规划器。一旦 MPC 求解失败或超时例如 100ms立即切换到一个计算速度极快的应急规划器例如一个基于规则的“紧急制动车道内居中”策略或者一个简化的基于二次规划的轨迹生成器。同时主规划器在后台继续尝试求解一旦成功再平滑地切换回来。这保证了系统的功能安全。6. 超越赛题相关技术在更广阔领域的映射解完一道题其价值不应止于答案本身。MathorCup 2021 A题所锤炼的这套“建模-约束-优化”的方法论其应用范围远不止于自动驾驶汽车的调头。它本质上是一套解决有约束运动规划问题的通用框架。让我们把视野放宽看看它在其他领域是如何映射和应用的。6.1 移动机器人领域的直接应用仓储AGV、酒店服务机器人、园区配送无人车这些移动机器人与自动驾驶汽车面临的核心规划问题是高度相似的。它们都需要在动态、不确定的环境中从A点移动到B点同时避开障碍物可能是人、其他机器人、货架。场景一个仓储AGV需要从一个货架取货后行驶到打包台。通道狭窄且有其他AGV和人员穿梭。映射车辆模型AGV可能是差速驱动或全向轮驱动其运动学模型与阿克曼模型不同但建模思路一致用状态方程描述其运动。约束几何约束货架和通道边界、动力学约束最大速度、加速度、动态避障约束其他AGV和行人。行人的预测可能比车辆更不可预测可能需要采用社会力模型或更复杂的交互预测算法。优化目标效率最短时间、能耗最小加速度变化、平稳性。对于仓储场景能耗可能是一个非常重要的指标。方法同样可以采用 MPC 框架。由于AGV速度较慢计算资源相对宽松有时甚至可以求解更复杂的优化问题。Hybrid A* 在仓储这种结构化程度高的环境中也非常有效。6.2 机械臂轨迹规划的深度关联七轴机械臂在杂乱的工作台上抓取零件这个场景和汽车调头有着惊人的数学同构性。场景机械臂末端执行器需要从当前位置运动到目标位置抓取物体期间要避开工作台上的其他物体、夹具甚至要避开自身的机械臂连杆自碰撞。映射状态空间从车辆的(x, y, θ)变成了机械臂的关节角(θ1, θ2, ..., θ7)维度更高非线性更强。运动学约束关节角位移、速度、加速度极限。这对应了车辆的方向盘转角和加速度限制。几何约束避障约束变得极其复杂。不仅末端要避开障碍物整个机械臂的连杆都要避开。这需要计算每个连杆与障碍物之间的最小距离并作为约束加入优化问题。这比车辆用一个包络圆要复杂得多。动力学约束关节力矩限制这对应了车辆的轮胎力极限。方法轨迹规划Motion Planning是机器人学的核心课题。基于采样的方法如 RRT*在机械臂规划中历史悠久因为它能有效处理高维空间和复杂几何。但近年来基于优化的方法同样大放异彩尤其是STOMP、CHOMP以及结合了机器学习的方法它们能生成更平滑、更符合动力学特性的轨迹。其核心思想同样是定义一个关于轨迹平滑度、避障距离的目标函数并施加动力学约束进行优化。6.3 从“规划”到“预测-规划-控制”闭环的思考无论是自动驾驶车、AGV还是机械臂我们讨论的“规划”都只是决策大脑的一部分。一个完整的自主系统必须形成“感知-预测-规划-控制”的闭环。感知提供环境的状态。对于车辆是车道线、车辆、行人对于机械臂是相机或激光雷达看到的物体三维点云。感知的精度、速度和可靠性直接决定了上游信息的质量。预测赋予系统“预判”能力。这是区分“自动化”和“智能化”的关键。预测模块需要基于感知的历史数据推断其他动态物体未来的意图和轨迹。深度学习特别是时空图神经网络STGNN和 Transformer 模型正在这一领域快速发展试图更好地建模复杂的交互行为。规划就是我们本文重点讨论的内容基于当前状态和对未来的预测做出自身的最优决策生成轨迹。控制精准地执行规划出的轨迹并克服模型误差和外部扰动。这个闭环中的每一个环节都面临挑战。感知有噪声和漏检预测充满不确定性规划要在极短时间内求解复杂问题控制要应对执行器的非线性和延迟。因此一个鲁棒的自主系统绝不是各个模块的简单堆砌而需要在设计之初就考虑模块间的协同与容错。例如规划模块需要知道感知的置信度在目标模糊时采取更保守的策略控制模块的跟踪误差需要反馈给规划模块作为下一次规划的初始状态修正。回过头看 MathorCup 的这道赛题它虽然简化了很多现实因素如完美的感知、简单的预测模型但它抓住了自主运动规划最核心的骨架在多约束下通过建模和优化寻求安全、高效、舒适的行动方案。掌握这套思想无论是面对自动驾驶的调头、机械臂的穿针引线还是无人机在丛林中的穿梭你都有了分析和解决问题的基本武器库。这或许就是这类竞赛超越分数之外带给参赛者最宝贵的财富。 自动驾驶轨迹规划与动态避障：从MathorCup赛题看MPC优化建模
1. 从一道赛题看自动驾驶的“调头”难题如果你关注过国内的数学建模竞赛或者对自动驾驶技术有点兴趣那你大概率听说过“MathorCup”这个名字。2021年的A题直接把一个看似简单、实则暗藏玄机的场景抛了出来自动驾驶车辆的调头问题。题目描述很直接就是让一辆车在一条双向四车道的道路上完成一次安全的调头操作。听起来是不是像驾校科目二里的“掉头”但当你真正开始用数学模型和算法去描述它时就会发现这远不是打一把方向那么简单。这道题之所以经典是因为它精准地戳中了自动驾驶从“实验室Demo”走向“真实复杂路况”的一个关键痛点。在封闭测试场或者结构化的高速路上车辆大多沿着车道线行驶决策逻辑相对简单。但调头这个动作是一个典型的强交互、高不确定性、多约束耦合的复杂场景。它要求车辆不仅要规划出一条从A点到B点的几何路径更要在这条路径上动态地处理与对向来车、同向车辆、交通规则如虚实线以及自身动力学极限的实时博弈。很多刚接触这个领域的朋友可能会把轨迹规划和避障当成两个独立的问题先规划一条理想路径再往上面叠加一个避障模块。但MathorCup 2021 A题的现实意义就在于它告诉你在调头这种场景下路径、速度、安全、规则是必须被一体化考虑和优化的。你不能先画一条完美的弧线再发现对面来车根本不让也不能为了绝对安全把车停在路口一直等待那又失去了通行的效率。这道题考察的正是这种在多重约束下寻找“最优解”或“满意解”的系统性思维。从更广的视角看这道题所涉及的核心——轨迹规划与动态避障——正是当下自动驾驶、移动机器人乃至机械臂控制领域最活跃的研究方向之一。无论是七轴机械臂在复杂环境中的灵巧作业还是扫地机器人在桌椅腿间穿梭其底层逻辑都是相通的如何在充满不确定性的环境中生成一条安全、平滑、高效的执行轨迹。因此解这道题的过程实际上是一次对机器人决策规划核心思想的深度演练。接下来我们就抛开抽象的题目描述深入到技术细节中看看要解决这样一个问题我们需要搭建怎样的思维框架又会遇到哪些实实在在的“坑”。2. 问题拆解调头场景下的四大核心约束面对“自动驾驶车辆调头”这个问题第一步不是急着写公式或编代码而是要把这个开放性问题拆解成一系列可量化、可建模的具体约束。这就像医生看病得先弄清楚病灶在哪里。根据题目描述和实际交通场景我们可以梳理出四大类核心约束它们共同构成了一个复杂的优化空间。2.1 几何约束车辆不是质点道路也有边界首先是最基础的几何约束。我们的车辆不是一个可以任意旋转的点它是一个有长度、宽度的刚体。在调头过程中我们必须确保车辆的整个外廓通常用一个矩形包络来近似在任何时刻都不与道路边界如路缘石、隔离带发生碰撞。这就引出了车辆运动学模型的选择。对于低速调头场景通常车速低于30km/h我们一般可以忽略复杂的轮胎动力学采用阿克曼转向几何模型。这个模型假设车辆四个轮子的速度方向都指向同一个瞬时转动中心。它的状态通常用(x, y, θ, v, φ)来表示分别是车辆后轴中心点的坐标、航向角、速度和前轮转角。有了这个模型我们才能准确地计算出车辆在未来一段时间内每一个位姿下所占用的空间区域也就是所谓的占用区域。道路约束则相对直观。双向四车道意味着有明确的车道线。调头通常需要在路口或允许调头的虚线处进行。这里的一个关键细节是调头轨迹的起始点和终点必须位于合法的车道区域内。你不能从实线开始调头也不能调头后压着实线结束。此外调头过程往往需要跨越对向车道这就必须考虑对向车道的宽度确保车辆在跨越时其占用区域不会侵入对向车道正常行驶车辆的安全空间。将这些几何约束用数学不等式表达出来是构建规划模型的第一步。2.2 动力学约束方向盘不能打太快油门也不能踩太猛即使几何上存在一条通路我们的车也未必能开过去因为它受限于自身的物理特性。这就是动力学约束它决定了轨迹的“平滑性”和“可行性”。主要有两类动力学约束运动学约束主要由转向系统决定。前轮转角φ及其变化率φ即方向盘转动速度是有上限的。|φ| ≤ φ_max决定了车辆的最小转弯半径这直接影响了调头所需道路的宽度——转弯半径不够小在窄路上就转不过来。|φ| ≤ ω_max则限制了轨迹的曲率变化率如果方向盘打得忽快忽慢生成的轨迹就会很“生硬”乘客会感到不适控制模块也很难精准跟踪。动力学约束主要由动力和制动系统决定。车辆的纵向加速度a含加速和减速有上下限。a_min ≤ a ≤ a_max。这限制了车辆在调头过程中速度变化的剧烈程度。例如为了等待对向来车而急刹或者为了快速通过而急加速都必须在这个范围内。通常为了保证舒适性我们对加速度的变化率加加速度jerk也会有所限制。在数学建模时我们通常将车辆的运动状态位置、速度、加速度和控制输入前轮转角、加速度随时间的变化关系用微分方程来描述。规划出的轨迹必须是这个微分方程的一个可行解。2.3 交通规则与交互约束最重要的部分也是最难的部分这是调头问题中最具挑战性的一环也是MathorCup这道题的精髓所在。它不再是单车优化而是进入了多智能体博弈的范畴。规则约束最典型的就是车道线规则。调头起始必须在虚线处过程中车轮不能长时间压实线。在有些建模中我们可以将实线区域设置为“不可穿越”的硬约束或者通过一个很高的代价函数来惩罚压线行为。动态避障约束这是核心中的核心。对向车道有来车同向车道可能有前车或后车。我们不仅要避免静态碰撞还要预测其他交通参与者的未来轨迹并做出反应。这里常用的方法是引入时空联合规划的概念。我们不再只规划空间中的一条路径而是规划一条“轨迹”即路径随时间的变化s(t)。这样车辆在时空坐标系中就是一个不断延伸的“管道”。其他车辆也是如此。避障约束就转化为本车在时空中的管道不能与其他车辆的预测管道发生重叠。但问题来了其他车辆的未来轨迹是未知的我们只能预测。常见的简化假设有恒定速度假设假设对方保持当前车速和车道行驶。这是最常用的但显然过于理想尤其在对方可能因我车调头而减速时。交互式预测假设对方是一个理性的驾驶员也会根据我车的意图做出反应如减速让行。这需要引入博弈论模型如纳什均衡或社会价值取向模型复杂度急剧上升。最坏情况假设假设对方不采取任何避让措施。这样规划出的轨迹最保守但可能导致过度谨慎甚至永远无法找到调头时机。在竞赛的有限时间内采用“恒定速度假设安全时距”是一个务实的选择。即只要本车在穿越冲突点时与预测的对方车辆到达该点的时间差大于一个安全阈值如3秒就认为是安全的。2.4 舒适性与效率约束寻找最佳平衡点在满足了安全、合规、可行的硬约束之后我们还要追求“开得好”。这主要体现在舒适性和效率上它们通常作为优化目标而不是硬约束。舒适性主要惩罚控制量的剧烈变化。例如在目标函数中最小化加速度a的平方积分减小急加急刹、最小化前轮转角变化率φ的平方积分减小方向盘猛打、最小化轨迹的曲率κ及其变化率使路径平滑。J_comfort ∫(w1*a² w2*φ² w3*κ²) dt效率主要希望尽快完成调头动作。最简单的就是最小化总时间T。但单纯追求时间最短可能会产生不舒适甚至不安全的轨迹。因此更常见的做法是结合舒适性指标形成一个多目标优化问题J_total w_t * T J_comfort。通过调整权重系数w_t,w1,w2,w3我们可以在这两者之间进行权衡。比如在车流密集时可能更注重安全性和机会捕捉效率对舒适性要求放宽而在车流稀疏时则可以追求平顺舒适的乘坐体验。3. 方法选型从搜索到优化哪种路径规划算法更合适明确了约束和目标接下来就要选择“武器”——规划算法。没有一种算法是万能的不同的算法在求解质量、计算效率和实现难度上各有千秋。针对调头这个中等复杂度、需要精细控制的场景我们主要对比几类主流方法。3.1 基于图搜索的方法A* 与 Hybrid A*A算法* 是路径规划领域的经典算法。它的核心思想是在离散的状态空间如栅格地图中通过评估函数f(n) g(n) h(n)来寻找从起点到终点的最优路径。其中g(n)是从起点到节点n的实际代价h(n)是从节点n到终点的预估代价启发函数。在调头问题中我们可以将车辆的状态如(x, y, θ)离散化构建一个状态栅格。A* 可以很好地融入障碍物其他车辆信息只要将其所在栅格标记为不可通行即可。但是A有两个明显缺点*第一它搜索的是路径Path而不是带时间信息的轨迹Trajectory因此难以直接处理动态障碍物。第二由于状态离散化特别是加入了方向θ后搜索空间会急剧膨胀“维度灾难”计算量很大。Hybrid A* 是针对车辆运动学特性改进的 A* 算法。它不再使用固定的栅格而是使用与车辆运动学模型相匹配的“运动基元”来连接状态。这些运动基元是一小段预先计算好的、符合车辆运动学约束的轨迹片段如直行5米、左转3米等。Hybrid A* 在状态空间中搜索这些运动基元的组合从而直接得到一条运动学可行的路径。它比传统 A* 更适用于车辆规划生成的路径更平滑。然而它依然主要解决静态环境下的路径问题对于动态避障通常需要在其生成的路径基础上再进行速度规划属于“路径-速度解耦”的范式在动态密集场景下可能不是最优。3.2 基于采样的方法RRT 与它的家族快速探索随机树RRT及其变种如 RRT*是另一大类规划算法。它的思路很直观从起点开始随机在状态空间中采样一个点然后尝试用一条简单的路径如直线连接树上最近的节点到这个采样点如果这条路径是可行的无碰撞就把这个新点加入到树中。如此反复直到树扩展到目标点附近。RRT 的优点在于它能快速在高维空间中找到一条可行路径对动态障碍物也有一定的适应性可以通过在已规划路径上重规划来避障。但其缺点也很突出生成的路径通常是折线非常不光滑完全不符合车辆的运动学特性需要后处理进行平滑。虽然 RRT* 能渐进最优但收敛速度慢。对于调头这种对轨迹平滑度要求很高的场景原始的 RRT 并不直接适用需要结合曲线拟合和优化。3.3 基于优化的方法最优控制与模型预测控制MPC这是目前学术界和工业界在轨迹规划上更主流和前沿的方向。其核心思想是将规划问题直接建模为一个有约束的数值优化问题。我们定义一个目标函数如上一节提到的总代价J_total以及一系列约束运动学/动力学方程、碰撞避免、交通规则等。然后使用数值优化算法如序列二次规划 SQP、内点法 IPM来求解控制输入序列u(t)方向盘转角、加速度使得目标函数最小同时满足所有约束。模型预测控制MPC是这类方法的一个非常强大的框架。MPC 在每个控制周期如0.1秒都执行一次有限时域如3秒的优化基于当前状态和环境预测优化得到未来一段时域内的最优控制序列但只执行第一个控制指令。到下一个周期根据新的状态重新进行优化。这种“滚动优化”的方式让 MPC 具备了强大的反馈校正能力能够很好地处理模型误差和外部干扰如其他车辆未按预测行驶。对于调头问题MPC 框架具有天然优势一体化可以同时优化路径和速度处理时空联合约束。动态性在每个周期都重新考虑最新的环境信息实现真正的动态避障。约束处理能够以统一的形式处理各种硬约束和软约束。当然它的缺点是计算负担重。在线求解一个非线性、非凸的优化问题对计算平台要求很高。不过随着硬件的发展和优化算法的改进MPC 在自动驾驶领域的应用已越来越广泛。综合对比与选择建议对于 MathorCup 这类竞赛场景计算资源相对充足更追求方案的先进性和完整性。因此采用基于优化的方法特别是 MPC 框架是一个很有竞争力的选择。它能够最直接、最优雅地描述我们前面拆解的所有约束和目标。如果担心在线优化实时性可以采用“分层规划”的思路上层用一个轻量级的 Hybrid A* 或车道级路由生成一条粗略的参考路径下层用 MPC 进行局部精细轨迹优化和动态避障。这样既保证了全局可行性又获得了局部最优性。4. 建模实战如何构建一个可求解的 MPC 调头模型理论说了这么多我们动手把它变成一个可以写进论文和代码的数学模型。这里我们以非线性 MPC 为例展示一个相对完整的建模过程。请注意这是一个简化的示例旨在说明核心思想。4.1 车辆模型与状态空间定义我们采用自行车模型阿克曼转向简化版。定义在惯性坐标系下的状态向量x和控制向量u为x [X, Y, φ, v]^T。这里做了简化用横摆角φ代表航向v为车速。u [a, δ]^T。a为纵向加速度δ为前轮转角。其微分方程连续时间模型为Ẋ v * cos(φ) Ẏ v * sin(φ) φ̇ v / L * tan(δ) // L 为轴距 v̇ a为了用于离散时间的 MPC 求解我们需要对其进行离散化例如采用欧拉法采样时间为dtX_{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 v_{k1} v_k a_k * dt这个离散模型x_{k1} f(x_k, u_k)将作为 MPC 中的预测模型。4.2 约束的数学表达这是建模的关键需要将自然语言描述的约束转化为数学不等式或等式。控制量约束物理极限与舒适性a_min ≤ a_k ≤ a_max, k0,..., N-1 δ_min ≤ δ_k ≤ δ_max, k0,..., N-1 Δδ_min ≤ (δ_k - δ_{k-1})/dt ≤ Δδ_max, k1,..., N-1 // 转向速率限制状态量约束v_min ≤ v_k ≤ v_max (道路限速), k0,..., N 0 ≤ v_k (不倒车), k0,..., N // 假设调头过程不允许倒车道路边界约束矩形区域 假设调头区域是一个已知的矩形其边界为[X_min, X_max, Y_min, Y_max]。X_min ≤ X_k ≤ X_max, k0,..., N Y_min ≤ Y_k ≤ Y_max, k0,..., N动态避障约束最核心也最复杂 假设对向有一辆车B我们预测其未来轨迹为(X_B(t), Y_B(t))并假设其外形可用一个圆来保守近似半径为R_B。本车外形也近似为半径为R_ego的圆。那么在每一个预测时刻k两车圆心之间的距离必须大于安全半径之和(X_k - X_B(k*dt))^2 (Y_k - Y_B(k*dt))^2 ≥ (R_ego R_B d_safe)^2, k0,..., N其中d_safe是额外的安全裕度。这是一个非线性、非凸约束会给优化求解带来困难。在实际中可能会采用线性化、或者将其转化为控制障碍函数CBF等形式来处理。4.3 目标函数的设计目标函数引导优化器找到我们想要的轨迹。一个典型的设计如下J Σ_{k0}^{N-1} ( w_a * a_k^2 w_δ * δ_k^2 w_Δδ * (δ_k - δ_{k-1})^2/dt^2 ) * dt w_v * (v_N - v_ref)^2 w_pos * [(X_N - X_goal)^2 (Y_N - Y_goal)^2] w_φ * (φ_N - φ_goal)^2我们来拆解一下第一项求和项是运行代价旨在最小化控制努力和提高舒适性。惩罚大的加速度急刹急加、大的前轮转角转向过猛以及大的转向速率方向盘打得快。第二项终端速度代价希望规划结束时车速接近一个期望值v_ref如调头完成后的正常车速。第三、四项终端状态代价这是最重要的。它强制要求规划时域N步结束时的车辆位置(X_N, Y_N)和航向φ_N尽可能接近我们的调头目标点(X_goal, Y_goal, φ_goal)。没有这项优化器可能会生成一条原地不动最省力的“轨迹”。权重系数w_*需要仔细调节。例如在调头初期为了寻找插入车流的时机可能对终端位置权重w_pos设置得稍低而对舒适性权重w_a, w_δ设置得较高允许车辆更灵活地调整速度和方向。当确认安全窗口后则可以增大w_pos让车辆更果断地完成调头动作。4.4 求解与实现要点将上述模型预测模型、约束、目标函数整合起来就形成了一个标准的非线性规划NLP问题。我们可以使用现成的求解器来求解例如IPOPT开源、SNOPT商业或ACADO自动生成高效代码的工具包。在实现时有几个关键点需要注意初始猜测非线性优化求解器非常依赖于初始猜测。一个好的初始猜测能极大提高收敛速度和成功率。我们可以用一条简单的几何曲线如回旋线或多项式曲线连接起点和终点并分配一个粗略的速度曲线作为优化变量的初始值。实时性MPC 需要在几十到几百毫秒内完成求解。除了选择高效求解器控制时域N和采样时间dt的选择至关重要。N太大问题维度过高N太小预见性不足。通常需要折中例如N30,dt0.1s预测未来3秒。不确定性处理我们之前的模型假设了对向来车轨迹是确定已知的。实际上这是预测的。更鲁棒的做法是考虑预测的不确定性例如采用鲁棒 MPC或随机 MPC规划一条在最坏情况或概率意义下安全的轨迹。但这会进一步增加计算复杂度。通过这样的建模我们就把一个生动的驾驶场景转化为了一个严谨的数学优化问题。求解这个问题的输出就是车辆在未来一段时间内每一时刻应该执行的方向盘转角和油门/刹车指令。5. 从仿真到现实模型验证与典型“坑点”分析建好了模型写出了求解代码在仿真环境中看着小车完美地避开虚拟车辆完成调头这远不是终点。从仿真到现实中间隔着一道名为“模型失配”和“ corner case”的鸿沟。很多在仿真中运行良好的算法一上真车或者遇到复杂情况就“趴窝”。下面结合调头场景聊聊那些必须提前考虑的验证环节和常见“坑点”。5.1 仿真验证的层次莫在浮沙筑高台仿真不是一次性的而是一个多层次、逐步逼近真实的过程。算法逻辑验证无动力学、无噪声 这是最初级的阶段。在一个完全理想的二维平面上其他车辆按预设轨迹匀速运动本车模型完美传感器无延迟无噪声。这个阶段的目标是验证核心规划算法的逻辑正确性目标函数是否导向期望行为约束是否被正确施加和满足避障逻辑是否生效在这个阶段你可能会发现目标函数权重设置不合理导致车辆“犹豫不决”或者碰撞约束存在漏洞导致轨迹穿车而过。这个阶段要用大量极端场景如对向来车突然加速进行压力测试。控制闭环验证加入底层控制器 规划模块输出的是期望的轨迹一系列位姿和速度车辆能否准确跟踪这条轨迹取决于底层控制器如横向的LQR/Stanley控制器纵向的PID控制器的性能。在这个仿真中你需要接入一个车辆动力学模型如CarSim、VeDYNA模型或简单的二自由度动力学模型并用控制器去跟踪规划轨迹。这时你会发现如果规划出的轨迹曲率变化过快方向盘打得太急控制器可能无法跟上产生较大的跟踪误差这个误差反过来可能使实际轨迹侵入安全边界导致碰撞风险。因此在规划阶段就必须考虑轨迹的跟踪可行性这就是为什么要在目标函数中惩罚控制量的变化率。感知-预测-规划闭环验证加入不确定性 这是最高级的仿真阶段。你需要模拟一个完整的感知链路传感器如激光雷达、摄像头生成带有噪声和延迟的点云或检测框预测模块基于不完美的感知数据输出带有概率分布的未来轨迹规划模块再基于这个不确定的预测进行决策。在这个阶段你会遭遇真实世界最核心的挑战信息不完整和决策延迟。例如感知模块可能漏检一辆车或者预测错误其意图直行还是左转。规划算法必须具备一定的容错性和鲁棒性。常用的策略包括增大安全裕度d_safe采用概率性碰撞检查PCC或者规划多条备选轨迹最后时刻根据最新信息选择。5.2 调头场景下的典型“坑点”与对策即使通过了层层仿真一些场景特有的问题依然会在细节处埋雷。坑点一对向来车博弈与“冻结机器人”问题这是最经典的难题。你的车在路口等待调头对向车道一直有车流。如果你的避障约束是严格的“时空管道不重叠”且预测对方恒定速度那么算法可能永远找不到一个绝对安全的空隙车辆会一直等待成为“冻结机器人”。对策引入“礼貌性”或“交互性”假设。例如可以建模对向车辆在发现本车有调头意图时有一定概率会减速让行。这可以通过在预测模型中增加一个基于距离和时间的减速概率来实现。或者采用更激进的策略规划一条“迫使”对方减速的轨迹但需非常谨慎确保安全底线。另一种务实的方法是在目标函数中增加“等待时间”惩罚让车辆在安全的前提下愿意接受一个较小的、经过计算的风险窗口。坑点二轨迹末端抖动与控制不匹配在仿真中规划轨迹的终点完美地对准了目标车道中心。但实际控制中车辆在接近终点时由于跟踪误差和模型失配可能产生轻微的横向摆动导致车辆停稳后没有完全摆正或者压到了车道线。对策在规划时不要将终端状态约束设为一个精确的点而应设为一个区域例如目标车道中心线附近±0.3米的范围。同时在目标函数的终端代价中不仅考虑位置更要重视终态的速度和角速度是否为零即是否达到静止稳定状态。这能引导规划器生成一条在末端更平缓、更易于稳定停车的轨迹。坑点三复杂道路几何与曲率连续题目中是标准的十字路口。但现实中调头区域可能是一个狭窄的“灯泡型”匝道或者有绿化带导流岛。这时单纯用起点和终点状态进行规划可能无法生成贴合道路几何的轨迹甚至可能规划出穿过导流岛的非法路径。对策引入参考路径。先通过高精度地图或车道级导航获取一条描述调头区域中心线的参考路径s_ref。然后在 MPC 的优化问题中增加一个惩罚项使车辆的实际横向偏移y相对于这条参考路径尽可能小。同时参考路径的曲率信息也可以用来指导规划确保生成轨迹的曲率变化是连续的避免方向盘突变。坑点四计算实时性与异常处理在车流密集的路口优化问题可能因为初始猜测差、约束突然收紧近距离切入车辆而求解失败或超时。规划模块必须有降级策略。对策实现一个分层架构。上层 MPC 作为主规划器。一旦 MPC 求解失败或超时例如 100ms立即切换到一个计算速度极快的应急规划器例如一个基于规则的“紧急制动车道内居中”策略或者一个简化的基于二次规划的轨迹生成器。同时主规划器在后台继续尝试求解一旦成功再平滑地切换回来。这保证了系统的功能安全。6. 超越赛题相关技术在更广阔领域的映射解完一道题其价值不应止于答案本身。MathorCup 2021 A题所锤炼的这套“建模-约束-优化”的方法论其应用范围远不止于自动驾驶汽车的调头。它本质上是一套解决有约束运动规划问题的通用框架。让我们把视野放宽看看它在其他领域是如何映射和应用的。6.1 移动机器人领域的直接应用仓储AGV、酒店服务机器人、园区配送无人车这些移动机器人与自动驾驶汽车面临的核心规划问题是高度相似的。它们都需要在动态、不确定的环境中从A点移动到B点同时避开障碍物可能是人、其他机器人、货架。场景一个仓储AGV需要从一个货架取货后行驶到打包台。通道狭窄且有其他AGV和人员穿梭。映射车辆模型AGV可能是差速驱动或全向轮驱动其运动学模型与阿克曼模型不同但建模思路一致用状态方程描述其运动。约束几何约束货架和通道边界、动力学约束最大速度、加速度、动态避障约束其他AGV和行人。行人的预测可能比车辆更不可预测可能需要采用社会力模型或更复杂的交互预测算法。优化目标效率最短时间、能耗最小加速度变化、平稳性。对于仓储场景能耗可能是一个非常重要的指标。方法同样可以采用 MPC 框架。由于AGV速度较慢计算资源相对宽松有时甚至可以求解更复杂的优化问题。Hybrid A* 在仓储这种结构化程度高的环境中也非常有效。6.2 机械臂轨迹规划的深度关联七轴机械臂在杂乱的工作台上抓取零件这个场景和汽车调头有着惊人的数学同构性。场景机械臂末端执行器需要从当前位置运动到目标位置抓取物体期间要避开工作台上的其他物体、夹具甚至要避开自身的机械臂连杆自碰撞。映射状态空间从车辆的(x, y, θ)变成了机械臂的关节角(θ1, θ2, ..., θ7)维度更高非线性更强。运动学约束关节角位移、速度、加速度极限。这对应了车辆的方向盘转角和加速度限制。几何约束避障约束变得极其复杂。不仅末端要避开障碍物整个机械臂的连杆都要避开。这需要计算每个连杆与障碍物之间的最小距离并作为约束加入优化问题。这比车辆用一个包络圆要复杂得多。动力学约束关节力矩限制这对应了车辆的轮胎力极限。方法轨迹规划Motion Planning是机器人学的核心课题。基于采样的方法如 RRT*在机械臂规划中历史悠久因为它能有效处理高维空间和复杂几何。但近年来基于优化的方法同样大放异彩尤其是STOMP、CHOMP以及结合了机器学习的方法它们能生成更平滑、更符合动力学特性的轨迹。其核心思想同样是定义一个关于轨迹平滑度、避障距离的目标函数并施加动力学约束进行优化。6.3 从“规划”到“预测-规划-控制”闭环的思考无论是自动驾驶车、AGV还是机械臂我们讨论的“规划”都只是决策大脑的一部分。一个完整的自主系统必须形成“感知-预测-规划-控制”的闭环。感知提供环境的状态。对于车辆是车道线、车辆、行人对于机械臂是相机或激光雷达看到的物体三维点云。感知的精度、速度和可靠性直接决定了上游信息的质量。预测赋予系统“预判”能力。这是区分“自动化”和“智能化”的关键。预测模块需要基于感知的历史数据推断其他动态物体未来的意图和轨迹。深度学习特别是时空图神经网络STGNN和 Transformer 模型正在这一领域快速发展试图更好地建模复杂的交互行为。规划就是我们本文重点讨论的内容基于当前状态和对未来的预测做出自身的最优决策生成轨迹。控制精准地执行规划出的轨迹并克服模型误差和外部扰动。这个闭环中的每一个环节都面临挑战。感知有噪声和漏检预测充满不确定性规划要在极短时间内求解复杂问题控制要应对执行器的非线性和延迟。因此一个鲁棒的自主系统绝不是各个模块的简单堆砌而需要在设计之初就考虑模块间的协同与容错。例如规划模块需要知道感知的置信度在目标模糊时采取更保守的策略控制模块的跟踪误差需要反馈给规划模块作为下一次规划的初始状态修正。回过头看 MathorCup 的这道赛题它虽然简化了很多现实因素如完美的感知、简单的预测模型但它抓住了自主运动规划最核心的骨架在多约束下通过建模和优化寻求安全、高效、舒适的行动方案。掌握这套思想无论是面对自动驾驶的调头、机械臂的穿针引线还是无人机在丛林中的穿梭你都有了分析和解决问题的基本武器库。这或许就是这类竞赛超越分数之外带给参赛者最宝贵的财富。