1. 项目概述从一道赛题看自动驾驶中的经典“调头”难题如果你关注过国内的数学建模竞赛或者对自动驾驶技术稍有了解那么“MathorCup”和“车辆调头”这两个词组合在一起大概率会让你想起2021年那道颇具挑战性的A题。这道题没有停留在理论层面而是直指自动驾驶落地中的一个具体且棘手的场景在结构化道路比如城市双向四车道上一辆自动驾驶汽车如何安全、高效、舒适地完成一次调头操作。这听起来简单不就是打个方向盘转个弯吗但让机器来执行里面全是学问。它不像在空旷场地可以随意画个圆。它受到路沿、对向车道、交通规则如双黄线不可逾越的严格约束。目标是在一个狭长的“走廊”里规划出一条从起点到终点的平滑轨迹同时要满足车辆自身的运动学限制比如前轮转角不能太大否则转不过去或侧翻还要考虑乘坐的舒适性加速度、加加速度不能突变。这道赛题的精妙之处在于它把一个复杂的工程问题抽象成了一个清晰的数学优化模型求解问题涵盖了轨迹规划、运动学约束、最优控制等多个自动驾驶核心模块。所以我们今天不聊高深的感知算法和复杂的决策逻辑就聚焦在这个“调头”问题上。我将以2021年MathorCup A题为蓝本拆解如何将一次真实的车辆调头需求转化为可建模、可求解的数学问题并分享在求解过程中的核心思路、工具选型以及那些容易踩坑的细节。无论你是数学建模爱好者还是对自动驾驶轨迹规划感兴趣的工程师相信这篇从实战角度出发的总结都能给你带来直接的参考价值。2. 问题拆解把“调头”翻译成数学模型面对“车辆调头轨迹规划”这个问题第一步也是最关键的一步是问题定义与抽象。我们不能一上来就想着用什么算法而是要先弄清楚我们要让计算机解决一个什么样的问题。2.1 场景与约束的数学化描述题目通常会给定一个典型的道路场景图一条双向四车道中间有双黄线路宽、车道宽、调头区域长度等参数明确。车辆初始位置和调头后的目标位置也已给定。我们需要将这些物理约束转化为数学约束边界约束道路约束车辆在整个运动过程中其外轮廓通常用一个矩形包络来近似不能超出可行驶区域的边界。这包括不能撞上路沿也不能越过双黄线进入对向车道除非在允许调头的缺口处。在数学模型里这通常表示为车辆矩形四个角点的坐标 $(x, y)$ 必须始终处于一个由不等式定义的区域内。运动学约束车辆约束车辆不是质点它的运动必须符合“自行车模型”等运动学规律。核心约束包括前轮转角约束$\phi_{min} \leq \phi \leq \phi_{max}$。这直接决定了车辆的最小转弯半径 $R_{min} L / \tan(\phi_{max})$其中 $L$ 是轴距。这是硬约束规划出的轨迹曲率在任何一点都不能超过 $1/R_{min}$。转角变化率约束$|\dot{\phi}| \leq \dot{\phi}_{max}$。这限制了方向盘转动的快慢影响了轨迹的平滑性和舒适性。速度与加速度约束虽然赛题有时更关注几何路径但完整的轨迹包含时间维度。速度 $v$、加速度 $a$、加加速度 $jerk$ 都需要被限制在合理范围内以保证舒适性和安全性。起终点约束车辆在起始时刻 $t_0$ 和终止时刻 $t_f$需要有确定的位置 $(x, y)$、航向角 $\theta$有时还包括速度 $v$。通常起点和终点的速度要求为零静止或近乎静止航向角相差大约180度。连续性约束规划出的轨迹必须是光滑的至少要求位置连续C0、速度连续C1为了舒适性最好加速度也连续C2。这意味着我们不能规划出一条由直线和圆弧生硬拼接的、有尖角的路径。2.2 优化目标的确定在满足了上述所有“必须做到”的约束硬约束后我们还要追求“做得更好”。这就需要定义优化目标成本函数。常见的优化目标有时间最优最小化总行驶时间 $t_f - t_0$。这通常会让车辆以最大允许加速度/减速度行驶并在几何路径上寻求最短距离。舒适性最优最小化整个行程中的加加速度Jerk的积分或者加速度的平方的积分。这会使速度变化非常平滑。路径长度最优在固定速度曲线的情况下最小化几何路径的总长度。能量最优最小化控制输入如加速度、前轮转角的平方和积分。在MathorCup这类赛题中为了简化并突出几何路径规划常常将路径长度最短或路径平滑性最好作为首要优化目标。例如最小化路径的曲率平方积分可以有效避免急弯得到一条非常平滑的行驶轨迹。注意在实际建模时往往需要对问题进行简化。例如可能先忽略时间维度只做几何路径规划生成一条从起点到终点的空间曲线。然后再进行速度规划为这条路径分配一个合理的时间戳和速度曲线。这种“路径-速度解耦”的方法是工程上的常见做法可以降低问题复杂度。3. 核心方法选型从曲线族到最优解明确了问题和目标后接下来要选择用什么方法来生成这条轨迹。核心思路是用一组参数来“描述”一条可能的轨迹然后将约束和目标函数用这些参数来表达最后通过优化算法寻找最优的那组参数。3.1 参数化曲线表示我们不可能去优化轨迹上每一个点那样变量太多。我们需要用一个由少量参数控制的曲线族来拟合轨迹。常用的曲线有多项式曲线这是最直观的选择例如使用五次多项式$x(s) a_0 a_1s a_2s^2 a_3s^3 a_4s^4 a_5s^5$ $y(s)$ 同理。其中 $s$ 是归一化的路径参数如从0到1。多项式的系数就是我们的优化变量。它的优点是计算简单平滑性好高阶可导并且容易施加起终点的位置、速度、加速度约束这些约束可以直接转化为系数上的线性方程。缺点是全局多项式在拟合复杂形状时可能产生不必要的振荡且曲率约束车辆转角约束是非线性的处理起来较复杂。样条曲线特别是B样条B-Spline在轨迹规划中应用极广。它将整个路径分成多段每段用一个低阶多项式如三次表示在连接处满足一定的连续性条件。B样条由控制点定义局部修改一个控制点只影响曲线的一部分这提供了很大的灵活性。优化变量就是这些控制点的坐标。B样条天然具有凸包性质即曲线一定位于控制点构成的凸包内这个特性可以巧妙地用于碰撞避免只要确保控制点的凸包在可行区域内曲线大体上就在里面这为处理复杂边界约束提供了便利。回旋曲线Clothoid/Euler Spiral这是一种曲率随弧长线性变化的曲线。在高速道路和铁路设计中广泛应用因为车辆以恒定角速度转动方向盘时其行驶轨迹就是回旋曲线。用它来连接直线和圆弧段可以保证曲率连续从而实现平滑的转向。在调头场景中可以用“直线-回旋线-圆弧-回旋线-直线”的组合来构建路径。优化变量可能是各段曲线的长度、圆弧半径等。选型心得对于MathorCup A题这种边界规则矩形区域的场景三次或五次B样条是一个非常好的选择。理由如下首先它可以通过较少控制点描述复杂形状优化变量维度适中其次利用其凸包性质可以将车辆轮廓的边界约束转化为对控制点的约束大大简化了问题最后B样条曲线本身是光滑的容易计算曲率便于施加运动学约束。3.2 优化求解框架一旦我们用一组参数 $\mathbf{p}$ 例如B样条的控制点坐标表示了轨迹我们的问题就变成了一个非线性规划问题$$ \begin{aligned} \min_{\mathbf{p}} \quad J(\mathbf{p}) \quad \text{(例如路径长度或曲率平方和)} \ \text{s.t.} \quad g(\mathbf{p}) \leq 0 \quad \text{(不等式约束如边界约束、曲率约束)} \ h(\mathbf{p}) 0 \quad \text{(等式约束如起终点约束)} \end{aligned} $$接下来就是选择求解器。对于中小规模问题有以下选择fmincon (MATLAB)对于参加数学建模竞赛的团队来说这是最触手可及的工具。MATLAB的fmincon函数功能强大可以处理线性和非线性约束。你需要做的是写好目标函数J(p)和约束函数[c, ceq] constraints(p)。它的优点是集成度高调试方便缺点是对于超大规模或非凸问题可能陷入局部最优或速度较慢。CasADi IPOPT这是当前学术界和工业界解决最优控制问题的“黄金组合”。CasADi是一个用于自动微分和数值优化的框架它可以高效地计算目标函数和约束的梯度雅可比矩阵、海森矩阵这对于基于梯度的优化器至关重要。IPOPT是一个强大的内点法求解器专门处理大规模非线性规划问题。用Python或MATLAB调用CasADi定义变量、目标、约束然后交给IPOPT求解通常能获得非常可靠和快速的结果。这是解决此类问题最推荐的专业工具链。其他优化库如 SciPy 中的minimize NLopt 等也可以根据情况选用。实操要点初始值很重要非线性优化求解器严重依赖初始猜测。一个糟糕的初始值比如一条穿过障碍物的直线可能导致求解失败。一个实用的技巧是先用简单的几何方法比如Dubins Path或Reeds-Shepp Path虽然它们不考虑边界但能给出一个大致形状生成一条粗略的可行路径然后用这条路径来拟合B样条得到控制点的初始位置。约束的离散化边界约束和曲率约束需要在整条路径上处处满足。我们无法在无穷多个点上检查标准的做法是离散化。沿着规划出的路径等间距或自适应地取一系列“检查点”比如50-100个在每个检查点上计算车辆轮廓是否越界、曲率是否超限并将这些点的约束全部添加到优化问题中。只要检查点足够密就能近似保证全程满足约束。数值稳定性计算曲率 $ \kappa \frac{\dot{x}\ddot{y} - \ddot{x}\dot{y}}{(\dot{x}^2 \dot{y}^2)^{3/2}} $ 时当速度 $\dot{x}, \dot{y}$ 很小时分母接近零会导致数值问题。在实际编码中需要加入一个极小值 $\epsilon$ 进行保护或者确保在施加曲率约束时路径参数化与速度规划是合理的。4. 建模与求解全流程实操下面我以一个基于B样条和IPOPT求解器的简化流程为例拆解具体的实现步骤。假设我们使用Python语言和CasADi库。4.1 环境与工具准备首先确保你的环境已安装必要的库。使用conda或pip安装pip install casadi numpy matplotlib scipyCasADi是核心NumPy用于数值计算Matplotlib用于可视化结果SciPy可能用于一些辅助计算。4.2 定义问题参数根据赛题图示和描述定义常量import numpy as np import casadi as ca # 道路参数 road_width 3.5 # 单车道宽度米 num_lanes 2 # 单向车道数 double_yellow_width 0.15 # 双黄线宽度米 usable_width road_width * num_lanes - double_yellow_width / 2 # 车辆可用宽度假设中心线对齐一侧车道线 # 车辆参数 vehicle_length 4.8 # 车长米 vehicle_width 1.8 # 车宽米 wheelbase 2.8 # 轴距米 max_steering_angle np.deg2rad(35) # 最大前轮转角弧度 min_turning_radius wheelbase / np.tan(max_steering_angle) # 最小转弯半径 # 起终点状态 (示例值) start_state np.array([0.0, usable_width/2, np.deg2rad(0), 0.0]) # [x, y, theta, v] end_state np.array([20.0, -usable_width/2, np.deg2rad(180), 0.0]) # 调头后位置x坐标有偏移y坐标反向航向角转180度这里的关键是理解usable_width。车辆调头时其左侧不能越过道路中心双黄线右侧不能撞上路沿。因此车辆中心点的可行区域在横向y方向上是一个宽度略小于单车道宽度的带状区域。4.3 B样条轨迹参数化我们使用三次B样条它需要n_ctrl_pts个控制点轨迹由这些控制点完全定义。# 定义B样条相关函数 def bspline_series(ctrl_pts, knots, t): 计算B样条在参数t处的值。 使用CasADi实现De Boor算法以支持自动微分。 ctrl_pts: 控制点矩阵形状为 (n_dim, n_ctrl_pts) knots: 节点向量 t: 标量参数 # 此处省略具体的De Boor算法实现细节CasADi有内置函数或可手动实现。 # 实际中可以使用第三方库如scipy.interpolate.BSpline先进行原型验证。 pass # 选择控制点数量例如7个点足以描述一个调头曲线 n_ctrl_pts 7 n_dim 2 # x, y # 定义优化变量控制点的x和y坐标 ctrl_pts_var ca.MX.sym(ctrl_pts, n_dim, n_ctrl_pts) # 这是一个矩阵变量 # 定义节点向量均匀节点注意端点重复度 knots np.linspace(0, 1, n_ctrl_pts 4) # 对于三次样条节点向量长度 控制点数 阶数(3) 1 knots (knots - knots[3]) / (knots[-4] - knots[3]) # 调整使有效区间为[0,1]在实际操作中为了简化我们可能直接优化控制点的坐标。轨迹上任意一点(x(s), y(s))都可以通过ctrl_pts_var和s计算出来。4.4 构建优化问题这是最核心的一步我们将约束和目标用CasADi的符号变量表示出来。# 1. 定义优化变量向量将控制点矩阵拉直 opt_vars ctrl_pts_var.reshape((-1, 1)) # 形状 (n_dim * n_ctrl_pts, 1) # 2. 初始化目标函数 J 0 # 3. 定义约束列表 g [] # 不等式约束 lbg [] # 不等式约束下界 ubg [] # 不等式约束上界 # 4. 起终点约束 # 起点s0时轨迹位置应等于start_state[0:2] pos_start bspline_eval(ctrl_pts_var, knots, 0.0) g.append(pos_start - start_state[0:2]) lbg.append([0, 0]) # 等式约束上下界都设为0 ubg.append([0, 0]) # 终点s1时轨迹位置应等于end_state[0:2] pos_end bspline_eval(ctrl_pts_var, knots, 1.0) g.append(pos_end - end_state[0:2]) lbg.append([0, 0]) ubg.append([0, 0]) # 可选起终点航向角约束需要计算轨迹的一阶导数 # ... # 5. 路径离散化施加边界约束和曲率约束 n_check 50 # 检查点数量 for i in range(n_check): s i / (n_check - 1) # 计算该点的位置、一阶导、二阶导 pos, vel, acc bspline_eval_with_derivatives(ctrl_pts_var, knots, s) # 边界约束车辆矩形四个角点不越界 # 假设车辆中心在pos航向角为theta由vel方向近似 theta ca.atan2(vel[1], vel[0]) # 计算四个角点的坐标 # 注意这里简化处理更严谨的做法是考虑车辆轮廓的旋转 half_len vehicle_length / 2 half_wid vehicle_width / 2 corners_local ca.vertcat( ca.horzcat(half_len, half_wid), ca.horzcat(half_len, -half_wid), ca.horzcat(-half_len, -half_wid), ca.horzcat(-half_len, half_wid) ).T # 形状 (2, 4) # 旋转矩阵 R ca.vertcat( ca.horzcat(ca.cos(theta), -ca.sin(theta)), ca.horzcat(ca.sin(theta), ca.cos(theta)) ) corners_global pos ca.mtimes(R, corners_local) # 对每个角点施加y方向的边界约束假设道路沿x方向 for j in range(4): y_corner corners_global[1, j] # 车辆应在道路范围内例如y方向约束在 [-usable_width/2, usable_width/2] 内 g.append(y_corner) lbg.append(-usable_width/2) ubg.append(usable_width/2) # x方向也可能有约束比如调头区域长度限制 x_corner corners_global[0, j] g.append(x_corner) lbg.append(0) ubg.append(25) # 假设调头区域长度25米 # 曲率约束 # 曲率 kappa (x*y - y*x) / ( (x^2 y^2)^(3/2) ) # 注意这里s是路径参数不是时间。我们假设速度不为零或者直接对几何路径的曲率进行约束。 # 几何曲率: kappa_geo (vel[0]*acc[1] - vel[1]*acc[0]) / (vel[0]**2 vel[1]**2)**(1.5) # 为了避免除零加一个小量 eps 1e-6 vel_sq vel[0]**2 vel[1]**2 eps kappa_geo (vel[0]*acc[1] - vel[1]*acc[0]) / (vel_sq**1.5) # 约束几何曲率的绝对值小于等于 1/min_turning_radius max_kappa 1.0 / min_turning_radius g.append(kappa_geo) lbg.append(-max_kappa) ubg.append(max_kappa) # 6. 定义目标函数路径长度近似通过累加弦长 for i in range(n_check-1): s1 i / (n_check - 1) s2 (i1) / (n_check - 1) pos1 bspline_eval(ctrl_pts_var, knots, s1) pos2 bspline_eval(ctrl_pts_var, knots, s2) J ca.sqrt(ca.sumsqr(pos2 - pos1)) # 累加相邻检查点间的欧氏距离作为路径长度近似 # 7. 构建非线性规划问题 nlp { x: opt_vars, f: J, g: ca.vertcat(*g) }这段代码构建了一个完整的非线性规划问题。目标是最小化近似路径长度J约束g包含了起终点等式约束、数十个边界不等式约束和曲率不等式约束。4.5 求解与结果分析使用IPOPT求解器# 设置求解器选项 opts { ipopt.print_level: 5, # 输出详细程度 ipopt.max_iter: 1000, ipopt.tol: 1e-6, print_time: 0 } # 创建求解器 solver ca.nlpsol(solver, ipopt, nlp, opts) # 设置初始猜测简单线性插值作为控制点初始值 ctrl_pts_init np.zeros((n_dim, n_ctrl_pts)) for i in range(n_ctrl_pts): alpha i / (n_ctrl_pts - 1) ctrl_pts_init[0, i] (1-alpha)*start_state[0] alpha*end_state[0] ctrl_pts_init[1, i] (1-alpha)*start_state[1] alpha*end_state[1] # 初始直线 # 设置变量边界控制点位置可以自由移动这里设一个较大的范围 lbx [-ca.inf] * (n_dim * n_ctrl_pts) ubx [ca.inf] * (n_dim * n_ctrl_pts) # 求解 sol solver(x0ctrl_pts_init.reshape(-1,1), lbxlbx, ubxubx, lbglbg, ubgubg) # 提取结果 ctrl_pts_opt np.array(sol[x]).reshape(n_dim, n_ctrl_pts) # 利用优化后的控制点可以高精度地采样生成最终轨迹 s_vals np.linspace(0, 1, 200) traj_opt np.array([bspline_eval(ctrl_pts_opt, knots, s) for s in s_vals])求解完成后traj_opt就是优化得到的车辆中心轨迹。你需要用Matplotlib将其绘制出来并叠加道路边界、车辆轮廓在轨迹上采样几个点画出矩形进行可视化验证确保轨迹是光滑的且全程满足所有约束。5. 常见问题、调试技巧与心得在实际操作中几乎不可能一次成功。以下是我在解决这类问题时积累的一些常见问题排查方法和心得。5.1 求解失败或结果不可行问题IPOPT报告“不可行”或“达到最大迭代次数”或者求解出的轨迹明显违反约束。排查检查初始猜测这是最常见的原因。一条从起点直线连接到终点的初始路径几乎必然穿过不可行区域如双黄线。优化器很难从这种糟糕的初始点跳出来。解决方案手动或通过简单算法如Dubins Path生成一条粗略的、满足大部分约束的可行路径然后用它来初始化控制点。哪怕这条路径很丑但只要“可行”就能极大提高求解成功率。检查约束的上下界lbg,ubg这是最容易出错的地方。仔细核对每个约束的上下界数值和符号。例如边界约束是y bound还是-bound y bound确保lbg和ubg的长度与约束向量g的长度完全一致。简化问题如果问题太复杂导致失败可以先求解一个简化版。例如先去掉曲率约束只优化路径长度和边界约束看能否得到一条几何路径。然后再把曲率约束加回去并用上一步的结果作为热启动warm-start。或者减少检查点n_check的数量先快速得到一个大致结果。缩放变量如果优化变量的量级差异很大比如x坐标是几十米y坐标是几米可能会引起数值问题。考虑对优化变量进行缩放使其量级接近1。5.2 轨迹不光滑或有尖角问题规划出的轨迹在曲率图上出现突变或者肉眼可见有折角。原因与解决B样条控制点过少控制点太少曲线缺乏自由度去拟合一个光滑的转弯。尝试增加控制点数量例如从7个增加到10个。目标函数缺乏平滑项如果只优化路径长度算法会倾向于走直线在转弯处可能产生“抄近道”的尖角。在目标函数中加入一项对曲率平方的积分或对控制点二阶差分的惩罚可以强制轨迹变得平滑。例如J weight_smooth * sum((ctrl_pts[:, i] - 2*ctrl_pts[:, i-1] ctrl_pts[:, i-2])**2)这惩罚了控制点加速度能使曲线更光顺。离散化不够密检查点n_check太少可能导致在检查点之间约束被违反或者优化器“钻空子”产生振荡。增加检查点数量。5.3 计算效率低下问题优化求解时间过长特别是当检查点很多时。优化策略减少变量在满足精度的前提下使用最少的控制点。可以通过实验确定一个合适的数量。稀疏导数CasADi在计算梯度时如果问题规模大自动微分可能较慢。利用问题的稀疏性每个约束只涉及少数变量来加速。CasADi的高级功能支持生成更高效的C代码。热启动在交互式应用或MPC模型预测控制中可以用上一时刻的优化结果作为当前时刻的初始值通常能大幅减少迭代次数。问题分解采用“路径-速度解耦”策略。先解决几何路径规划本赛题核心得到一条空间路径。然后再单独进行速度规划这是一个相对简单的一维优化问题。5.4 对实际应用的思考MathorCup的这道题是一个完美的教学案例但它和真实的自动驾驶调头还有距离。真实场景更复杂动态障碍物对向可能有来车。这就需要引入时间维度进行时空联合规划ST Graph或者使用基于搜索如Hybrid A*结合优化如OBCA的方法。不确定性感知有误差定位有漂移。规划出的轨迹需要具有一定的鲁棒性或者规划模块需要能够根据实时信息快速重规划。车辆模型自行车模型是简化模型。对于低速大转角场景需要考虑转向机构的阿克曼几何甚至轮胎侧偏特性。更精确的模型会带来更复杂的约束。舒适性标准不仅仅是加加速度还包括侧向加速度等。这些都需要在目标函数中体现。尽管如此通过这道题掌握的将物理问题转化为优化问题的建模思想以及使用B样条参数化、处理多种约束、调用非线性求解器的技术流程是通往更高级轨迹规划算法的坚实基础。它训练的正是一种解决复杂工程问题的核心能力定义、抽象、建模、求解、验证。 自动驾驶车辆调头轨迹规划：从数学建模到B样条优化求解实战
1. 项目概述从一道赛题看自动驾驶中的经典“调头”难题如果你关注过国内的数学建模竞赛或者对自动驾驶技术稍有了解那么“MathorCup”和“车辆调头”这两个词组合在一起大概率会让你想起2021年那道颇具挑战性的A题。这道题没有停留在理论层面而是直指自动驾驶落地中的一个具体且棘手的场景在结构化道路比如城市双向四车道上一辆自动驾驶汽车如何安全、高效、舒适地完成一次调头操作。这听起来简单不就是打个方向盘转个弯吗但让机器来执行里面全是学问。它不像在空旷场地可以随意画个圆。它受到路沿、对向车道、交通规则如双黄线不可逾越的严格约束。目标是在一个狭长的“走廊”里规划出一条从起点到终点的平滑轨迹同时要满足车辆自身的运动学限制比如前轮转角不能太大否则转不过去或侧翻还要考虑乘坐的舒适性加速度、加加速度不能突变。这道赛题的精妙之处在于它把一个复杂的工程问题抽象成了一个清晰的数学优化模型求解问题涵盖了轨迹规划、运动学约束、最优控制等多个自动驾驶核心模块。所以我们今天不聊高深的感知算法和复杂的决策逻辑就聚焦在这个“调头”问题上。我将以2021年MathorCup A题为蓝本拆解如何将一次真实的车辆调头需求转化为可建模、可求解的数学问题并分享在求解过程中的核心思路、工具选型以及那些容易踩坑的细节。无论你是数学建模爱好者还是对自动驾驶轨迹规划感兴趣的工程师相信这篇从实战角度出发的总结都能给你带来直接的参考价值。2. 问题拆解把“调头”翻译成数学模型面对“车辆调头轨迹规划”这个问题第一步也是最关键的一步是问题定义与抽象。我们不能一上来就想着用什么算法而是要先弄清楚我们要让计算机解决一个什么样的问题。2.1 场景与约束的数学化描述题目通常会给定一个典型的道路场景图一条双向四车道中间有双黄线路宽、车道宽、调头区域长度等参数明确。车辆初始位置和调头后的目标位置也已给定。我们需要将这些物理约束转化为数学约束边界约束道路约束车辆在整个运动过程中其外轮廓通常用一个矩形包络来近似不能超出可行驶区域的边界。这包括不能撞上路沿也不能越过双黄线进入对向车道除非在允许调头的缺口处。在数学模型里这通常表示为车辆矩形四个角点的坐标 $(x, y)$ 必须始终处于一个由不等式定义的区域内。运动学约束车辆约束车辆不是质点它的运动必须符合“自行车模型”等运动学规律。核心约束包括前轮转角约束$\phi_{min} \leq \phi \leq \phi_{max}$。这直接决定了车辆的最小转弯半径 $R_{min} L / \tan(\phi_{max})$其中 $L$ 是轴距。这是硬约束规划出的轨迹曲率在任何一点都不能超过 $1/R_{min}$。转角变化率约束$|\dot{\phi}| \leq \dot{\phi}_{max}$。这限制了方向盘转动的快慢影响了轨迹的平滑性和舒适性。速度与加速度约束虽然赛题有时更关注几何路径但完整的轨迹包含时间维度。速度 $v$、加速度 $a$、加加速度 $jerk$ 都需要被限制在合理范围内以保证舒适性和安全性。起终点约束车辆在起始时刻 $t_0$ 和终止时刻 $t_f$需要有确定的位置 $(x, y)$、航向角 $\theta$有时还包括速度 $v$。通常起点和终点的速度要求为零静止或近乎静止航向角相差大约180度。连续性约束规划出的轨迹必须是光滑的至少要求位置连续C0、速度连续C1为了舒适性最好加速度也连续C2。这意味着我们不能规划出一条由直线和圆弧生硬拼接的、有尖角的路径。2.2 优化目标的确定在满足了上述所有“必须做到”的约束硬约束后我们还要追求“做得更好”。这就需要定义优化目标成本函数。常见的优化目标有时间最优最小化总行驶时间 $t_f - t_0$。这通常会让车辆以最大允许加速度/减速度行驶并在几何路径上寻求最短距离。舒适性最优最小化整个行程中的加加速度Jerk的积分或者加速度的平方的积分。这会使速度变化非常平滑。路径长度最优在固定速度曲线的情况下最小化几何路径的总长度。能量最优最小化控制输入如加速度、前轮转角的平方和积分。在MathorCup这类赛题中为了简化并突出几何路径规划常常将路径长度最短或路径平滑性最好作为首要优化目标。例如最小化路径的曲率平方积分可以有效避免急弯得到一条非常平滑的行驶轨迹。注意在实际建模时往往需要对问题进行简化。例如可能先忽略时间维度只做几何路径规划生成一条从起点到终点的空间曲线。然后再进行速度规划为这条路径分配一个合理的时间戳和速度曲线。这种“路径-速度解耦”的方法是工程上的常见做法可以降低问题复杂度。3. 核心方法选型从曲线族到最优解明确了问题和目标后接下来要选择用什么方法来生成这条轨迹。核心思路是用一组参数来“描述”一条可能的轨迹然后将约束和目标函数用这些参数来表达最后通过优化算法寻找最优的那组参数。3.1 参数化曲线表示我们不可能去优化轨迹上每一个点那样变量太多。我们需要用一个由少量参数控制的曲线族来拟合轨迹。常用的曲线有多项式曲线这是最直观的选择例如使用五次多项式$x(s) a_0 a_1s a_2s^2 a_3s^3 a_4s^4 a_5s^5$ $y(s)$ 同理。其中 $s$ 是归一化的路径参数如从0到1。多项式的系数就是我们的优化变量。它的优点是计算简单平滑性好高阶可导并且容易施加起终点的位置、速度、加速度约束这些约束可以直接转化为系数上的线性方程。缺点是全局多项式在拟合复杂形状时可能产生不必要的振荡且曲率约束车辆转角约束是非线性的处理起来较复杂。样条曲线特别是B样条B-Spline在轨迹规划中应用极广。它将整个路径分成多段每段用一个低阶多项式如三次表示在连接处满足一定的连续性条件。B样条由控制点定义局部修改一个控制点只影响曲线的一部分这提供了很大的灵活性。优化变量就是这些控制点的坐标。B样条天然具有凸包性质即曲线一定位于控制点构成的凸包内这个特性可以巧妙地用于碰撞避免只要确保控制点的凸包在可行区域内曲线大体上就在里面这为处理复杂边界约束提供了便利。回旋曲线Clothoid/Euler Spiral这是一种曲率随弧长线性变化的曲线。在高速道路和铁路设计中广泛应用因为车辆以恒定角速度转动方向盘时其行驶轨迹就是回旋曲线。用它来连接直线和圆弧段可以保证曲率连续从而实现平滑的转向。在调头场景中可以用“直线-回旋线-圆弧-回旋线-直线”的组合来构建路径。优化变量可能是各段曲线的长度、圆弧半径等。选型心得对于MathorCup A题这种边界规则矩形区域的场景三次或五次B样条是一个非常好的选择。理由如下首先它可以通过较少控制点描述复杂形状优化变量维度适中其次利用其凸包性质可以将车辆轮廓的边界约束转化为对控制点的约束大大简化了问题最后B样条曲线本身是光滑的容易计算曲率便于施加运动学约束。3.2 优化求解框架一旦我们用一组参数 $\mathbf{p}$ 例如B样条的控制点坐标表示了轨迹我们的问题就变成了一个非线性规划问题$$ \begin{aligned} \min_{\mathbf{p}} \quad J(\mathbf{p}) \quad \text{(例如路径长度或曲率平方和)} \ \text{s.t.} \quad g(\mathbf{p}) \leq 0 \quad \text{(不等式约束如边界约束、曲率约束)} \ h(\mathbf{p}) 0 \quad \text{(等式约束如起终点约束)} \end{aligned} $$接下来就是选择求解器。对于中小规模问题有以下选择fmincon (MATLAB)对于参加数学建模竞赛的团队来说这是最触手可及的工具。MATLAB的fmincon函数功能强大可以处理线性和非线性约束。你需要做的是写好目标函数J(p)和约束函数[c, ceq] constraints(p)。它的优点是集成度高调试方便缺点是对于超大规模或非凸问题可能陷入局部最优或速度较慢。CasADi IPOPT这是当前学术界和工业界解决最优控制问题的“黄金组合”。CasADi是一个用于自动微分和数值优化的框架它可以高效地计算目标函数和约束的梯度雅可比矩阵、海森矩阵这对于基于梯度的优化器至关重要。IPOPT是一个强大的内点法求解器专门处理大规模非线性规划问题。用Python或MATLAB调用CasADi定义变量、目标、约束然后交给IPOPT求解通常能获得非常可靠和快速的结果。这是解决此类问题最推荐的专业工具链。其他优化库如 SciPy 中的minimize NLopt 等也可以根据情况选用。实操要点初始值很重要非线性优化求解器严重依赖初始猜测。一个糟糕的初始值比如一条穿过障碍物的直线可能导致求解失败。一个实用的技巧是先用简单的几何方法比如Dubins Path或Reeds-Shepp Path虽然它们不考虑边界但能给出一个大致形状生成一条粗略的可行路径然后用这条路径来拟合B样条得到控制点的初始位置。约束的离散化边界约束和曲率约束需要在整条路径上处处满足。我们无法在无穷多个点上检查标准的做法是离散化。沿着规划出的路径等间距或自适应地取一系列“检查点”比如50-100个在每个检查点上计算车辆轮廓是否越界、曲率是否超限并将这些点的约束全部添加到优化问题中。只要检查点足够密就能近似保证全程满足约束。数值稳定性计算曲率 $ \kappa \frac{\dot{x}\ddot{y} - \ddot{x}\dot{y}}{(\dot{x}^2 \dot{y}^2)^{3/2}} $ 时当速度 $\dot{x}, \dot{y}$ 很小时分母接近零会导致数值问题。在实际编码中需要加入一个极小值 $\epsilon$ 进行保护或者确保在施加曲率约束时路径参数化与速度规划是合理的。4. 建模与求解全流程实操下面我以一个基于B样条和IPOPT求解器的简化流程为例拆解具体的实现步骤。假设我们使用Python语言和CasADi库。4.1 环境与工具准备首先确保你的环境已安装必要的库。使用conda或pip安装pip install casadi numpy matplotlib scipyCasADi是核心NumPy用于数值计算Matplotlib用于可视化结果SciPy可能用于一些辅助计算。4.2 定义问题参数根据赛题图示和描述定义常量import numpy as np import casadi as ca # 道路参数 road_width 3.5 # 单车道宽度米 num_lanes 2 # 单向车道数 double_yellow_width 0.15 # 双黄线宽度米 usable_width road_width * num_lanes - double_yellow_width / 2 # 车辆可用宽度假设中心线对齐一侧车道线 # 车辆参数 vehicle_length 4.8 # 车长米 vehicle_width 1.8 # 车宽米 wheelbase 2.8 # 轴距米 max_steering_angle np.deg2rad(35) # 最大前轮转角弧度 min_turning_radius wheelbase / np.tan(max_steering_angle) # 最小转弯半径 # 起终点状态 (示例值) start_state np.array([0.0, usable_width/2, np.deg2rad(0), 0.0]) # [x, y, theta, v] end_state np.array([20.0, -usable_width/2, np.deg2rad(180), 0.0]) # 调头后位置x坐标有偏移y坐标反向航向角转180度这里的关键是理解usable_width。车辆调头时其左侧不能越过道路中心双黄线右侧不能撞上路沿。因此车辆中心点的可行区域在横向y方向上是一个宽度略小于单车道宽度的带状区域。4.3 B样条轨迹参数化我们使用三次B样条它需要n_ctrl_pts个控制点轨迹由这些控制点完全定义。# 定义B样条相关函数 def bspline_series(ctrl_pts, knots, t): 计算B样条在参数t处的值。 使用CasADi实现De Boor算法以支持自动微分。 ctrl_pts: 控制点矩阵形状为 (n_dim, n_ctrl_pts) knots: 节点向量 t: 标量参数 # 此处省略具体的De Boor算法实现细节CasADi有内置函数或可手动实现。 # 实际中可以使用第三方库如scipy.interpolate.BSpline先进行原型验证。 pass # 选择控制点数量例如7个点足以描述一个调头曲线 n_ctrl_pts 7 n_dim 2 # x, y # 定义优化变量控制点的x和y坐标 ctrl_pts_var ca.MX.sym(ctrl_pts, n_dim, n_ctrl_pts) # 这是一个矩阵变量 # 定义节点向量均匀节点注意端点重复度 knots np.linspace(0, 1, n_ctrl_pts 4) # 对于三次样条节点向量长度 控制点数 阶数(3) 1 knots (knots - knots[3]) / (knots[-4] - knots[3]) # 调整使有效区间为[0,1]在实际操作中为了简化我们可能直接优化控制点的坐标。轨迹上任意一点(x(s), y(s))都可以通过ctrl_pts_var和s计算出来。4.4 构建优化问题这是最核心的一步我们将约束和目标用CasADi的符号变量表示出来。# 1. 定义优化变量向量将控制点矩阵拉直 opt_vars ctrl_pts_var.reshape((-1, 1)) # 形状 (n_dim * n_ctrl_pts, 1) # 2. 初始化目标函数 J 0 # 3. 定义约束列表 g [] # 不等式约束 lbg [] # 不等式约束下界 ubg [] # 不等式约束上界 # 4. 起终点约束 # 起点s0时轨迹位置应等于start_state[0:2] pos_start bspline_eval(ctrl_pts_var, knots, 0.0) g.append(pos_start - start_state[0:2]) lbg.append([0, 0]) # 等式约束上下界都设为0 ubg.append([0, 0]) # 终点s1时轨迹位置应等于end_state[0:2] pos_end bspline_eval(ctrl_pts_var, knots, 1.0) g.append(pos_end - end_state[0:2]) lbg.append([0, 0]) ubg.append([0, 0]) # 可选起终点航向角约束需要计算轨迹的一阶导数 # ... # 5. 路径离散化施加边界约束和曲率约束 n_check 50 # 检查点数量 for i in range(n_check): s i / (n_check - 1) # 计算该点的位置、一阶导、二阶导 pos, vel, acc bspline_eval_with_derivatives(ctrl_pts_var, knots, s) # 边界约束车辆矩形四个角点不越界 # 假设车辆中心在pos航向角为theta由vel方向近似 theta ca.atan2(vel[1], vel[0]) # 计算四个角点的坐标 # 注意这里简化处理更严谨的做法是考虑车辆轮廓的旋转 half_len vehicle_length / 2 half_wid vehicle_width / 2 corners_local ca.vertcat( ca.horzcat(half_len, half_wid), ca.horzcat(half_len, -half_wid), ca.horzcat(-half_len, -half_wid), ca.horzcat(-half_len, half_wid) ).T # 形状 (2, 4) # 旋转矩阵 R ca.vertcat( ca.horzcat(ca.cos(theta), -ca.sin(theta)), ca.horzcat(ca.sin(theta), ca.cos(theta)) ) corners_global pos ca.mtimes(R, corners_local) # 对每个角点施加y方向的边界约束假设道路沿x方向 for j in range(4): y_corner corners_global[1, j] # 车辆应在道路范围内例如y方向约束在 [-usable_width/2, usable_width/2] 内 g.append(y_corner) lbg.append(-usable_width/2) ubg.append(usable_width/2) # x方向也可能有约束比如调头区域长度限制 x_corner corners_global[0, j] g.append(x_corner) lbg.append(0) ubg.append(25) # 假设调头区域长度25米 # 曲率约束 # 曲率 kappa (x*y - y*x) / ( (x^2 y^2)^(3/2) ) # 注意这里s是路径参数不是时间。我们假设速度不为零或者直接对几何路径的曲率进行约束。 # 几何曲率: kappa_geo (vel[0]*acc[1] - vel[1]*acc[0]) / (vel[0]**2 vel[1]**2)**(1.5) # 为了避免除零加一个小量 eps 1e-6 vel_sq vel[0]**2 vel[1]**2 eps kappa_geo (vel[0]*acc[1] - vel[1]*acc[0]) / (vel_sq**1.5) # 约束几何曲率的绝对值小于等于 1/min_turning_radius max_kappa 1.0 / min_turning_radius g.append(kappa_geo) lbg.append(-max_kappa) ubg.append(max_kappa) # 6. 定义目标函数路径长度近似通过累加弦长 for i in range(n_check-1): s1 i / (n_check - 1) s2 (i1) / (n_check - 1) pos1 bspline_eval(ctrl_pts_var, knots, s1) pos2 bspline_eval(ctrl_pts_var, knots, s2) J ca.sqrt(ca.sumsqr(pos2 - pos1)) # 累加相邻检查点间的欧氏距离作为路径长度近似 # 7. 构建非线性规划问题 nlp { x: opt_vars, f: J, g: ca.vertcat(*g) }这段代码构建了一个完整的非线性规划问题。目标是最小化近似路径长度J约束g包含了起终点等式约束、数十个边界不等式约束和曲率不等式约束。4.5 求解与结果分析使用IPOPT求解器# 设置求解器选项 opts { ipopt.print_level: 5, # 输出详细程度 ipopt.max_iter: 1000, ipopt.tol: 1e-6, print_time: 0 } # 创建求解器 solver ca.nlpsol(solver, ipopt, nlp, opts) # 设置初始猜测简单线性插值作为控制点初始值 ctrl_pts_init np.zeros((n_dim, n_ctrl_pts)) for i in range(n_ctrl_pts): alpha i / (n_ctrl_pts - 1) ctrl_pts_init[0, i] (1-alpha)*start_state[0] alpha*end_state[0] ctrl_pts_init[1, i] (1-alpha)*start_state[1] alpha*end_state[1] # 初始直线 # 设置变量边界控制点位置可以自由移动这里设一个较大的范围 lbx [-ca.inf] * (n_dim * n_ctrl_pts) ubx [ca.inf] * (n_dim * n_ctrl_pts) # 求解 sol solver(x0ctrl_pts_init.reshape(-1,1), lbxlbx, ubxubx, lbglbg, ubgubg) # 提取结果 ctrl_pts_opt np.array(sol[x]).reshape(n_dim, n_ctrl_pts) # 利用优化后的控制点可以高精度地采样生成最终轨迹 s_vals np.linspace(0, 1, 200) traj_opt np.array([bspline_eval(ctrl_pts_opt, knots, s) for s in s_vals])求解完成后traj_opt就是优化得到的车辆中心轨迹。你需要用Matplotlib将其绘制出来并叠加道路边界、车辆轮廓在轨迹上采样几个点画出矩形进行可视化验证确保轨迹是光滑的且全程满足所有约束。5. 常见问题、调试技巧与心得在实际操作中几乎不可能一次成功。以下是我在解决这类问题时积累的一些常见问题排查方法和心得。5.1 求解失败或结果不可行问题IPOPT报告“不可行”或“达到最大迭代次数”或者求解出的轨迹明显违反约束。排查检查初始猜测这是最常见的原因。一条从起点直线连接到终点的初始路径几乎必然穿过不可行区域如双黄线。优化器很难从这种糟糕的初始点跳出来。解决方案手动或通过简单算法如Dubins Path生成一条粗略的、满足大部分约束的可行路径然后用它来初始化控制点。哪怕这条路径很丑但只要“可行”就能极大提高求解成功率。检查约束的上下界lbg,ubg这是最容易出错的地方。仔细核对每个约束的上下界数值和符号。例如边界约束是y bound还是-bound y bound确保lbg和ubg的长度与约束向量g的长度完全一致。简化问题如果问题太复杂导致失败可以先求解一个简化版。例如先去掉曲率约束只优化路径长度和边界约束看能否得到一条几何路径。然后再把曲率约束加回去并用上一步的结果作为热启动warm-start。或者减少检查点n_check的数量先快速得到一个大致结果。缩放变量如果优化变量的量级差异很大比如x坐标是几十米y坐标是几米可能会引起数值问题。考虑对优化变量进行缩放使其量级接近1。5.2 轨迹不光滑或有尖角问题规划出的轨迹在曲率图上出现突变或者肉眼可见有折角。原因与解决B样条控制点过少控制点太少曲线缺乏自由度去拟合一个光滑的转弯。尝试增加控制点数量例如从7个增加到10个。目标函数缺乏平滑项如果只优化路径长度算法会倾向于走直线在转弯处可能产生“抄近道”的尖角。在目标函数中加入一项对曲率平方的积分或对控制点二阶差分的惩罚可以强制轨迹变得平滑。例如J weight_smooth * sum((ctrl_pts[:, i] - 2*ctrl_pts[:, i-1] ctrl_pts[:, i-2])**2)这惩罚了控制点加速度能使曲线更光顺。离散化不够密检查点n_check太少可能导致在检查点之间约束被违反或者优化器“钻空子”产生振荡。增加检查点数量。5.3 计算效率低下问题优化求解时间过长特别是当检查点很多时。优化策略减少变量在满足精度的前提下使用最少的控制点。可以通过实验确定一个合适的数量。稀疏导数CasADi在计算梯度时如果问题规模大自动微分可能较慢。利用问题的稀疏性每个约束只涉及少数变量来加速。CasADi的高级功能支持生成更高效的C代码。热启动在交互式应用或MPC模型预测控制中可以用上一时刻的优化结果作为当前时刻的初始值通常能大幅减少迭代次数。问题分解采用“路径-速度解耦”策略。先解决几何路径规划本赛题核心得到一条空间路径。然后再单独进行速度规划这是一个相对简单的一维优化问题。5.4 对实际应用的思考MathorCup的这道题是一个完美的教学案例但它和真实的自动驾驶调头还有距离。真实场景更复杂动态障碍物对向可能有来车。这就需要引入时间维度进行时空联合规划ST Graph或者使用基于搜索如Hybrid A*结合优化如OBCA的方法。不确定性感知有误差定位有漂移。规划出的轨迹需要具有一定的鲁棒性或者规划模块需要能够根据实时信息快速重规划。车辆模型自行车模型是简化模型。对于低速大转角场景需要考虑转向机构的阿克曼几何甚至轮胎侧偏特性。更精确的模型会带来更复杂的约束。舒适性标准不仅仅是加加速度还包括侧向加速度等。这些都需要在目标函数中体现。尽管如此通过这道题掌握的将物理问题转化为优化问题的建模思想以及使用B样条参数化、处理多种约束、调用非线性求解器的技术流程是通往更高级轨迹规划算法的坚实基础。它训练的正是一种解决复杂工程问题的核心能力定义、抽象、建模、求解、验证。