4、曲率计算与离散化：连续曲率公式、离散点曲率近似（三点法、五点法）

各位同学，咱们今天聊点实在的。曲率这东西，说白了就是描述曲线“弯”的程度。你开车过弯，方向盘打得越狠，曲率就越大。但在控制算法里，我们面对的往往不是连续曲线，而是一堆离散的点——比如激光雷达扫出来的路径点，或者规划层给的一串轨迹点。怎么从这些离散点里算出曲率？这就是本章要啃的硬骨头。

4.1 连续曲率公式：先打好理论基础

先回顾一下数学定义。对于一条连续曲线 y = f(x)，曲率 κ 的公式是：

κ = |y''| / (1 + y'²)^(3/2)

嗯，看着有点吓人。但说白了，分子是二阶导（加速度），分母是一阶导（速度）的某种组合。你想想看，如果曲线很平缓，y' 很小，分母接近1，曲率主要由 y'' 决定。如果曲线很陡，y' 很大，分母变大，曲率就会被压下去——这很符合直觉：陡坡上的弯道，其实没那么“弯”。

我个人习惯把这个公式记成“弯度 = 加速度 / 速度的某种三次方”。虽然不严谨，但好记。

对于参数曲线 (x(t), y(t))，公式稍微变一下：

κ = |x' y'' - y' x''| / (x'² + y'²)^(3/2)

这个形式更通用。我在项目中遇到过用三次样条插值做路径平滑，然后直接用这个公式算曲率。效果不错，但计算量稍大。

4.2 离散点曲率近似：三点法

好，现在进入实战。假设你有一串离散点 P0, P1, P2, ..., Pn。怎么算 Pi 点的曲率？

最经典的方法就是三点法。取 Pi 的前一个点 Pi-1 和后一个点 Pi+1，用这三个点拟合一个圆，然后取这个圆的半径的倒数作为曲率。

公式如下：

设三点为 A(x1,y1), B(x2,y2), C(x3,y3)
先算边长：
a = dist(B, C)
b = dist(A, C)
c = dist(A, B)

然后算三角形面积：
s = |(x2-x1)*(y3-y1) - (x3-x1)*(y2-y1)| / 2

外接圆半径：
R = (a * b * c) / (4 * s)

曲率：
κ = 1 / R

注意，这里 s 是三角形面积。如果三点共线，s = 0，R 无穷大，曲率为 0。这很合理——直线嘛，不弯。

三点法的优点是简单、计算快。适合嵌入式系统，或者对实时性要求高的场景。但缺点也很明显：

• 对噪声敏感
• 只能反映局部弯曲程度
• 点间距不均匀时误差大

4.3 离散点曲率近似：五点法

为了克服三点法的缺点，我们可以用更多的点来做拟合。五点法就是取 Pi 前后各两个点，一共五个点，拟合一个二次曲线或三次曲线，然后求导得到曲率。

具体做法：

1. 取点 Pi-2, Pi-1, Pi, Pi+1, Pi+2
2. 用最小二乘法拟合一个二次多项式 y = a x² + b x + c
3. 然后直接用连续曲率公式：κ = |2a| / (1 + (2a x + b)²)^(3/2)

当然，更常见的是用参数曲线拟合。把五个点的 x 和 y 分别用弧长参数化，然后分别拟合 x(s) 和 y(s) 的二次或三次多项式，再代入参数形式的曲率公式。

// 伪代码示例：五点法曲率计算
function curvatureFivePoints(points, i):
    // points 是点数组，i 是目标点索引
    // 取前后各两个点
    p = points[i-2 : i+3]  // 共5个点
    
    // 用弧长参数化
    s = cumulative_distance(p)
    
    // 拟合 x(s) 和 y(s) 的二次多项式
    coeff_x = polyfit(s, x_coords, 2)
    coeff_y = polyfit(s, y_coords, 2)
    
    // 计算一阶导和二阶导
    x1 = 2*coeff_x[0]*s[2] + coeff_x[1]  // 在中间点求导
    y1 = 2*coeff_y[0]*s[2] + coeff_y[1]
    x2 = 2*coeff_x[0]
    y2 = 2*coeff_y[0]
    
    // 曲率公式
    k = abs(x1*y2 - y1*x2) / (x1^2 + y1^2)^(1.5)
    return k

4.4 三点法与五点法的对比

对比项	三点法	五点法
计算量	小（适合嵌入式）	较大（需要拟合）
噪声敏感度	高	低（有平滑效果）
点间距适应性	差	好
边界处理	简单（首尾点需特殊处理）	复杂（首尾各两个点需特殊处理）
适用场景	实时性高、数据干净	噪声大、精度要求高

4.5 知识体系图

下面这张图展示了本章的核心逻辑：从连续曲率公式出发，到离散化的两种近似方法，再到实际应用中的选择策略。

4.6 实际应用中的选择策略

说了这么多，到底用哪个？我个人的经验是：

• 如果你在做实时控制（比如电机伺服、无人机飞控），用三点法。计算快，够用。
• 如果你在做路径规划或离线分析，用五点法。精度高，曲线平滑。
• 如果数据噪声特别大，可以考虑用更多点（七点法、九点法），但边际效益递减。

嗯，曲率计算这块就聊到这儿。下一节咱们会讲曲率约束怎么加到控制律里，以及怎么防止控制发散。到时候你会看到，曲率算得准不准，直接决定了你的车能不能稳稳地过弯。

可可精品资料库