自动驾驶轨迹规划中的曲率限速原理与工程实践

1. 轨迹规划中的曲率限速基础概念

第一次接触自动驾驶轨迹规划时，我遇到一个典型场景：车辆在急转弯处要么减速过猛导致乘客不适，要么速度过快产生侧滑风险。这正是曲率限速设计要解决的核心问题——让车辆在不同曲率的路径上智能调整速度。

曲率（Curvature）在数学上定义为曲线在某一点弯曲程度的量度，计算公式为κ=1/R（R为曲率半径）。在车辆动力学中，曲率直接影响着车辆通过弯道时的向心加速度。根据牛顿第二定律，向心加速度a=v²/R=v²κ，这意味着速度v和曲率κ必须满足特定关系才能保证行车安全。

实际工程中，我们通常采用简化模型来计算曲率限速值。最常用的经验公式是：

code复制v_max = √(a_max / κ)

其中a_max是根据车辆性能和舒适度设定的最大允许横向加速度（一般乘用车取2-3m/s²）。这个公式背后是简单的物理原理：限制向心加速度不超过安全阈值。我曾测试过不同参数的影响——当a_max从2.5m/s²提升到3m/s²时，同一弯道的通过速度可提升约10%，但会明显增加乘客的"被甩出去"的不适感。

2. 曲率限速的工程实现细节

2.1 离散路径点的曲率计算

实际路径是由离散点组成的polyline，计算曲率需要特殊的数值处理方法。我比较过三种常用方案：

1. 三点圆曲率法：取连续三个路径点P1,P2,P3，计算通过这三点的圆的半径倒数。这种方法计算简单但噪声敏感，适合平滑路径。实现代码如下：

python复制def three_point_curvature(p1, p2, p3):
    # 计算三角形面积
    area = 0.5 * abs((p2.x-p1.x)*(p3.y-p1.y) - (p2.y-p1.y)*(p3.x-p1.x))
    # 避免除零错误
    if area < 1e-5:
        return 0
    # 计算曲率
    curvature = 4 * area / (np.linalg.norm(p1-p2)*np.linalg.norm(p2-p3)*np.linalg.norm(p3-p1))
    return curvature

2. 最小二乘拟合法：对每个点取前后n个邻域点，用二次曲线拟合后解析求导。这种方法抗噪性强但计算量大，适合嘈杂的感知数据。

3. 样条曲线法：先用样条函数拟合整个路径，再对参数方程求导。这是最精确的方法，但实时性较差。

经过实测，在城区场景下我最终选择方案2（n=5），因为感知噪声较大时仍能保持稳定的曲率估计，且计算耗时控制在0.5ms以内。

2.2 限速曲线的生成策略

获得路径曲率后，需要生成平滑的速度限制曲线。这里有几个关键经验：

1. 前瞻距离选择：一般取3秒的行驶距离。太短会导致减速不及时，太长会过度保守。我曾用真车测试发现，对于90°直角弯，2.5秒前瞻能让减速过程更自然。

2. 曲率滤波处理：原始曲率信号会有高频波动，需要用滑动平均或低通滤波处理。但滤波过强会导致急弯识别延迟。我的经验是采用窗口大小为5的Savitzky-Golay滤波器。

3. 速度过渡设计：相邻点的限速差值应小于0.2m/s²（约0.07g）以保证舒适性。这需要通过速度规划器进行二次优化，简单的实现方式是：

python复制def smooth_speed_limit(raw_speeds, max_accel):
    smoothed = [raw_speeds[0]]
    for i in range(1, len(raw_speeds)):
        delta = raw_speeds[i] - smoothed[i-1]
        delta = np.clip(delta, -max_accel, max_accel)
        smoothed.append(smoothed[i-1] + delta)
    return smoothed

3. 复杂场景下的特殊处理

3.1 复合弯道的速度规划

连续S形弯道会引发特殊的"曲率符号问题"。我的团队曾遇到一个典型案例：车辆在通过S弯时出现不必要的速度波动。原因是正负曲率交替导致限速计算出现跳变。

解决方案是采用绝对曲率值计算，并在速度规划时考虑整体曲率包络。具体步骤：

1. 计算原始曲率的绝对值
2. 使用高斯模糊平滑处理（σ=3m）
3. 对波峰区域进行标记和合并
4. 生成连续的限速区间

3.2 曲率与道路限速的综合处理

实际道路既有曲率限速，也有法定限速。两者的协调策略直接影响行车效率。我们的最佳实践是：

python复制def combined_speed_limit(curve_limit, road_limit):
    # 取两者较小值作为硬约束
    hard_limit = min(curve_limit, road_limit)
    # 在接近限速点时采用平滑过渡
    if abs(curve_limit - road_limit) < 5km/h:
        return 0.5*(curve_limit + road_limit)
    return hard_limit

这种处理方式在山区高速路上特别重要，可以避免频繁的速度调整。

4. 实际部署中的性能优化

4.1 计算效率提升技巧

在资源受限的嵌入式平台实现时，我们开发了几个优化手段：

1. 曲率预计算：对于固定路线（如园区物流车），可以离线计算曲率并存储
2. 稀疏采样：每5米计算一个曲率点，中间点线性插值
3. 近似计算：用查表法替代实时开方运算，误差控制在2%以内

这些优化能使计算耗时从15ms降至3ms，满足100Hz的实时性要求。

4.2 参数自适应调整

通过大量路测，我们建立了参数自适应规则：

• 干燥路面：a_max=2.8m/s²
• 湿滑路面：a_max=1.8m/s²
• 载重每增加100kg，a_max降低0.1m/s²
• 乘客舒适模式：a_max=2.0m/s²

这种动态调整使系统能适应不同工况，实测可减少30%的急减速情况。

5. 验证与调试方法论

5.1 仿真测试框架

我们搭建了分级测试体系：

1. 单元测试：验证曲率计算精度

python复制def test_curvature_calculation():
    # 测试半圆形路径
    points = [(x, math.sqrt(1-x*x)) for x in np.linspace(-1,1,21)]
    curvatures = [three_point_curvature(points[i-1], points[i], points[i+1]) 
                 for i in range(1,len(points)-1)]
    assert abs(np.mean(curvatures) - 1.0) < 0.05

2. 场景测试：典型弯道组合
3. 随机测试：自动生成复杂路径

5.2 实车调试技巧

路测时我们特别关注几个指标：

• 横向加速度标准差（反映平稳性）
• 速度命令变化率（反映舒适性）
• 实际轨迹与参考路径的偏差

使用CANoe工具采集数据后，用MATLAB进行后处理分析。一个实用的技巧是在急弯处贴反光标记，方便摄像头记录实际通过速度。

6. 前沿发展与工程权衡

最新的研究开始考虑轮胎力学模型，将侧偏角等因素纳入限速计算。但在工程实践中，我们发现简单模型配合充分的余量设计往往更可靠。一个典型的权衡点是：

• 学术方案：考虑负载转移、轮胎非线性等，精度高但参数敏感
• 工程方案：简化模型+安全系数，牺牲5-10%的理论最优性换取稳定性

在量产项目中，我们最终选择工程方案，但保留了升级接口。这种务实做法能有效控制开发风险。