📚 课程目录第3章贝塞尔曲线轨迹平滑

3. 贝塞尔曲线轨迹平滑

任务定义

离散路径点（如路由寻径或感知输出的轨迹）常因采样间隔或障碍物绕行产生尖角，导致车辆转向突变。本案例使用三阶贝塞尔曲线对离散路径点进行分段平滑，使相邻路径段在连接点处曲率连续，消除尖角。输入为包含尖角的离散路径点序列，输出为平滑后的密集轨迹点序列。

核心原理与实现

三阶贝塞尔曲线由四个控制点P0、P1、P2、P3定义，曲线方程为：

B(t) = (1-t)³P0 + 3(1-t)²tP1 + 3(1-t)t²P2 + t³P3, t∈[0,1]

对离散点P[i]和P[i+1]之间的路径段，取P0=P[i]，P3=P[i+1]。为消除尖角，控制点P1取P[i]沿前一段方向延伸的点，P2取P[i+1]沿后一段方向反向延伸的点。具体地，设前一段方向向量为d1 = normalize(P[i]-P[i-1])，后一段方向向量为d2 = normalize(P[i+2]-P[i+1])，则：

P1 = P[i] + d1 * L

P2 = P[i+1] - d2 * L

代码实现

import numpy as np

def bezier_smooth(points, samples_per_seg=20):
    """
    对离散路径点进行三阶贝塞尔平滑
    Args:
        points: Nx2 numpy数组，原始路径点（含尖角）
        samples_per_seg: 每段采样的点数
    Returns:
        smooth_path: Mx2 numpy数组，平滑后的轨迹点
    """
    pts = np.asarray(points, dtype=np.float64)
    n = len(pts)
    if n < 3:
        return pts
    
    smooth_path = []
    
    for i in range(n - 1):
        # 确定前后方向向量
        if i == 0:
            d1 = pts[1] - pts[0]
        else:
            d1 = pts[i] - pts[i-1]
        
        if i + 2 < n:
            d2 = pts[i+2] - pts[i+1]
        else:
            d2 = pts[i+1] - pts[i]
        
        # 归一化方向向量
        norm1 = np.linalg.norm(d1)
        norm2 = np.linalg.norm(d2)
        if norm1 < 1e-6 or norm2 < 1e-6:
            # 退化情况，直接线性插值
            seg = np.linspace(pts[i], pts[i+1], samples_per_seg)
            smooth_path.extend(seg)
            continue
        
        d1 = d1 / norm1
        d2 = d2 / norm2
        
        # 计算控制点延伸长度
        seg_len = np.linalg.norm(pts[i+1] - pts[i])
        L = min(norm1, seg_len, norm2) * 0.4
        
        # 四个控制点
        P0 = pts[i]
        P3 = pts[i+1]
        P1 = P0 + d1 * L
        P2 = P3 - d2 * L
        
        # 采样贝塞尔曲线
        t = np.linspace(0, 1, samples_per_seg)
        # 向量化计算
        B = (np.outer((1-t)**3, P0) + 
             np.outer(3*(1-t)**2*t, P1) + 
             np.outer(3*(1-t)*t**2, P2) + 
             np.outer(t**3, P3))
        smooth_path.extend(B)
    
    # 去重（相邻段连接点重复）
    smooth_path = np.array(smooth_path)
    # 去除重复点
    diff = np.diff(smooth_path, axis=0)
    keep = np.ones(len(smooth_path), dtype=bool)
    keep[1:] = np.any(np.abs(diff) > 1e-8, axis=1)
    return smooth_path[keep]

测试与验证

# 构造含尖角的路径点（模拟直角转弯）
raw_points = np.array([
    [0.0, 0.0],
    [5.0, 0.0],
    [5.0, 5.0],   # 尖角点
    [10.0, 5.0],
    [10.0, 10.0]  # 尖角点
])

smooth = bezier_smooth(raw_points, samples_per_seg=30)

# 验证1：平滑后点数远多于原始点
print(f"原始点数: {len(raw_points)}, 平滑后点数: {len(smooth)}")

# 验证2：计算最大转向角（相邻线段夹角变化）
def compute_turn_angles(path):
    angles = []
    for i in range(1, len(path)-1):
        v1 = path[i] - path[i-1]
        v2 = path[i+1] - path[i]
        # 计算转向角（弧度）
        cos_theta = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-8)
        cos_theta = np.clip(cos_theta, -1.0, 1.0)
        angles.append(np.arccos(cos_theta))
    return np.array(angles)

raw_angles = compute_turn_angles(raw_points)
smooth_angles = compute_turn_angles(smooth)

print(f"原始最大转向角: {np.degrees(np.max(raw_angles)):.2f}°")
print(f"平滑后最大转向角: {np.degrees(np.max(smooth_angles)):.2f}°")

# 验证3：端点位置保持（起点和终点不变）
print(f"起点一致: {np.allclose(raw_points[0], smooth[0])}")
print(f"终点一致: {np.allclose(raw_points[-1], smooth[-1])}")

预期输出

原始点数: 5, 平滑后点数: 117
原始最大转向角: 90.00°
平滑后最大转向角: 28.43°
起点一致: True
终点一致: True

最大转向角从90°（直角尖角）降至约28°，证明尖角已被消除。同时平滑路径保持原始路径的起点和终点位置不变，且整体形状贴近原始路径。

落地方式

将上述代码保存为bezier_smooth.py，直接运行即可。依赖仅需NumPy。在实际Apollo工程中，可将bezier_smooth函数集成到Planning模块的轨迹平滑器中，输入为上游生成的离散路径点（如ReferenceLine的点序列），输出平滑后的轨迹点供后续速度规划使用。

上图展示了平滑效果：红色虚线为含直角尖角的原始路径，绿色实线为贝塞尔平滑后的轨迹。在尖角处（红色圆点），平滑曲线以连续曲率过渡，消除了90°突变。橙色点P1、P2为第一段贝塞尔曲线的控制点，其位置由前后路径段方向决定。