4. 连续性概念：C0、C1、C2连续性，几何连续性G0、G1、G2，连续性对轨迹的影响

好，咱们今天聊一个在轨迹规划里特别基础、但又特别容易被忽视的话题——连续性。

我记得刚入行那会儿，做AGV的路径规划，车跑起来总是一顿一顿的，尤其是在拐弯的地方。领导问我怎么回事，我查了半天电机参数、PID参数，最后发现——问题出在轨迹本身不够“光滑”。说白了，就是连续性没处理好。

那什么是连续性？咱们从数学定义和工程实践两个角度来拆解。

4.1 参数连续性：C0、C1、C2

参数连续性，也叫数学连续性。它描述的是轨迹上相邻两段曲线在连接点处的“贴合程度”。

类型	数学含义	通俗理解
C0	位置连续：P₁(t_end) = P₂(t_start)	两段轨迹在连接点处“接上了”，没有断点
C1	速度连续：P'(t) 连续	不仅位置接上了，速度（一阶导数）也一致，没有突变
C2	加速度连续：P''(t) 连续	速度变化率也平滑，没有冲击感

我在项目中遇到过这样一个坑：用三次样条插值生成路径，默认就是C2连续的，看起来很美。但实际跑起来，机器人还是抖。后来一查，问题出在时间参数化上——样条曲线在几何上是C2的，但时间分配不均匀，导致实际运动速度有跳变。嗯，这里要注意，C2连续是必要条件，但不是充分条件。

4.2 几何连续性：G0、G1、G2

几何连续性，是参数连续性的“放松版本”。它不关心参数化方式，只关心曲线的几何形状。

类型	几何含义	与C连续的关系
G0	位置连续（同C0）	完全相同
G1	切线方向连续	C1 ⇒ G1，但G1 ⇏ C1
G2	曲率连续	C2 ⇒ G2，但G2 ⇏ C2

你想想看，G1和C1的区别在哪？C1要求速度向量完全相等，包括大小和方向。而G1只要求方向一致，大小可以不同。说白了，G1允许你“慢一点进弯、快一点出弯”，只要方向对就行。

4.3 连续性对轨迹的影响

咱们用一张SVG图来梳理一下知识体系：

4.4 实际工程中的选择策略

那在实际项目中，到底该选哪种连续性？我给大家一个参考：

• 仓储AGV（低速、重载）：G1连续就够了。因为速度慢，加速度突变影响不大。我曾经做过一个项目，为了追求G2连续，路径计算时间从2ms涨到15ms，得不偿失。
• 自动驾驶（中高速）：至少G2连续。曲率突变会导致方向盘急转，乘客会晕车。我记得有一次测试，就因为路径上有一个G1连接点，车在60km/h过弯时明显“甩了一下”。
• 工业机械臂（高精度）：C2连续是底线。机械臂的关节电机对加速度突变特别敏感，C1连续会导致电机电流尖峰，严重时可能触发过流保护。

4.5 代码示例：检查轨迹连续性

最后，给一段我常用的Python代码，用来检查轨迹的连续性。你可以在自己的项目中直接拿来用。

import numpy as np

def check_continuity(traj_points, dt=0.01):
    """
    检查轨迹的C0、C1、C2连续性
    traj_points: (N, 2) 或 (N, 3) 的轨迹点
    dt: 时间步长
    """
    # 计算一阶导数（速度）
    vel = np.diff(traj_points, axis=0) / dt
    # 计算二阶导数（加速度）
    acc = np.diff(vel, axis=0) / dt
    
    # C0检查：位置是否连续（相邻点距离是否过大）
    pos_diff = np.linalg.norm(np.diff(traj_points, axis=0), axis=1)
    c0_ok = np.all(pos_diff < 0.1)  # 阈值根据实际场景调整
    
    # C1检查：速度是否连续（速度变化率是否过大）
    vel_diff = np.linalg.norm(np.diff(vel, axis=0), axis=1)
    c1_ok = np.all(vel_diff < 1.0)
    
    # C2检查：加速度是否连续（加加速度是否过大）
    acc_diff = np.linalg.norm(np.diff(acc, axis=0), axis=1)
    c2_ok = np.all(acc_diff < 10.0)
    
    print(f"C0连续: {c0_ok}")
    print(f"C1连续: {c1_ok}")
    print(f"C2连续: {c2_ok}")
    
    return c0_ok, c1_ok, c2_ok

这段代码虽然简单，但很实用。我每次生成新轨迹后，都会先跑一遍这个检查，心里才有底。

好了，关于连续性的内容就聊到这儿。记住一句话：连续性不是越高越好，够用就行。关键是要理解你的应用场景对平滑度的真实需求。