第二讲：数学基础回顾——向量与坐标系，曲线参数化表示，弧长与曲率计算

各位同学好，我是你们的老朋友。

做路径规划这些年，我最大的感触就是：数学基础不牢，后面代码写得再花哨也白搭。今天这一讲，咱们就把轨迹平滑最核心的数学工具过一遍。别怕，我不讲那些枯燥的定理证明，咱们直接聊“这东西在代码里怎么用”。

2.1 向量与坐标系——路径规划的“语言”

说白了，路径规划就是跟向量打交道。你规划出来的每一条轨迹，本质上都是一系列向量的集合。

2.1.1 向量的基本操作

我习惯把向量看作“带方向和大小的箭头”。在二维平面里，一个向量 v = (x, y) 就代表从原点指向 (x, y) 的箭头。

常用的操作就这几个：

• 加法：对应坐标相加，用于位置累加
• 点积：v1 · v2 = x1*x2 + y1*y2，用来判断方向是否一致
• 叉积：二维里叉积是个标量，v1 × v2 = x1*y2 - y1*x2，判断左右转向
• 模长：|v| = sqrt(x² + y²)，就是距离

2.1.2 坐标系的选择

嗯，这里要注意。路径规划里常见的坐标系有三种：

坐标系	特点	我常用的场景
全局坐标系（世界系）	固定不动，原点在起点或地图原点	全局路径规划，A*算法
局部坐标系（车体系）	随车辆移动，x轴朝前	局部避障，控制指令生成
Frenet坐标系	沿参考线建立，s为纵向，d为横向	轨迹平滑，换道决策

我曾经在项目里吃过亏——全局坐标系下做平滑，结果车辆在弯道处老是“切内线”。后来换成Frenet坐标系，问题一下子就解决了。你想想看，沿着道路方向做平滑，是不是比在直角坐标系里硬算要合理得多？

2.2 曲线参数化表示——给轨迹一个“数学身份证”

路径规划出来的是一堆离散点，但控制模块要的是连续曲线。怎么把离散点变成连续曲线？这就是参数化的活儿了。

2.2.1 为什么需要参数化？

直接连点成线？那叫折线，机器人走起来会一顿一顿的。我们需要一条光滑的曲线，既能通过所有关键点，又有一阶、二阶连续导数。

常用的参数化方法有三种：

• 多项式曲线：简单直接，但高阶多项式容易震荡
• 样条曲线（B样条、三次样条）：分段低阶多项式，兼顾光滑与稳定
• 贝塞尔曲线：通过控制点调整形状，直观好用

2.2.2 参数化表示示例

拿三次样条来说，每一段曲线可以写成：

# 三次样条的一段参数方程
x(s) = a0 + a1*s + a2*s² + a3*s³
y(s) = b0 + b1*s + b2*s² + b3*s³

这里的 s 是参数，通常取 [0, 1] 区间。通过调整系数 a0~a3, b0~b3，就能让相邻段在连接点处位置、速度、加速度都连续。

我记得第一次手写样条插值时，debug 了整整两天。后来发现是边界条件没处理好——端点处的二阶导数设成了0，结果曲线在起点处“翘”起来了。嗯，这种坑踩过一次就记住了。

2.3 弧长与曲率计算——轨迹平滑的“硬指标”

弧长和曲率，是评价轨迹质量的两个核心指标。说白了：弧长决定效率，曲率决定可行性。

2.3.1 弧长计算

对于参数曲线 (x(s), y(s))，弧长公式是：

L = ∫ sqrt( (dx/ds)² + (dy/ds)² ) ds

实际工程中，我们很少直接积分。我一般用数值积分，比如辛普森法则：

def arc_length(x_func, y_func, s_start, s_end, n=100):
    """数值计算弧长"""
    ds = (s_end - s_start) / n
    length = 0.0
    for i in range(n):
        s = s_start + i * ds
        dx = derivative(x_func, s)  # 数值求导
        dy = derivative(y_func, s)
        length += sqrt(dx**2 + dy**2) * ds
    return length

2.3.2 曲率计算——这才是关键

曲率 κ 描述曲线弯曲的程度。公式是：

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

其中 x', y' 是一阶导数，x'', y'' 是二阶导数。

为什么曲率这么重要？因为车辆有最小转弯半径限制。如果规划出来的轨迹曲率太大，车辆根本转不过去。

2.3.3 曲率计算代码示例

def curvature(x_func, y_func, s):
    """计算参数s处的曲率"""
    # 一阶导数
    dx = derivative(x_func, s)
    dy = derivative(y_func, s)
    # 二阶导数
    ddx = second_derivative(x_func, s)
    ddy = second_derivative(y_func, s)
    
    numerator = abs(dx * ddy - dy * ddx)
    denominator = (dx**2 + dy**2) ** 1.5
    
    if denominator < 1e-10:
        return 0.0  # 直线段曲率为0
    
    return numerator / denominator

2.4 本章知识体系总览

下面这张图，是我自己梳理的本章知识结构。你把它存下来，后面学完所有章节再回来看，会有更深的理解。

好了，这一讲的内容就到这里。数学基础这东西，光看不行，得动手算。我建议你把上面的代码敲一遍，跑几个测试用例试试。

可可精品资料库