2. 多项式轨迹拟合

任务定义

给定一组离散路径点（含x、y坐标），使用五次多项式分别对x(t)和y(t)进行参数化拟合，生成一条位置连续、速度连续、加速度连续（C2连续）的平滑轨迹。最终输出拟合后的轨迹点序列，并验证端点位置与给定路径点误差小于0.05m。

五次多项式模型

以时间参数t∈[0,T]表示轨迹，x(t)和y(t)各自独立使用五次多项式：

f(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵

系数由边界条件唯一确定：起点位置、速度、加速度，终点位置、速度、加速度。对于路径拟合场景，起点和终点的速度、加速度均设为0，位置取路径首尾点坐标。

完整实现

import numpy as np

def quintic_coefficients(p0, v0, a0, p1, v1, a1, T):
    """计算五次多项式系数 [a0..a5]"""
    T2, T3 = T*T, T**3
    T4, T5 = T**4, T**5
    M = np.array([
        [1, 0,   0,    0,     0,     0],
        [0, 1,   0,    0,     0,     0],
        [0, 0,   2,    0,     0,     0],
        [1, T,  T2,   T3,    T4,    T5],
        [0, 1, 2*T, 3*T2,  4*T3,  5*T4],
        [0, 0,   2,  6*T, 12*T2, 20*T3]
    ])
    b = np.array([p0, v0, a0, p1, v1, a1])
    return np.linalg.solve(M, b)

def evaluate_poly(coeffs, t):
    """计算多项式在t处的值"""
    return sum(c * t**i for i, c in enumerate(coeffs))

# ========== 输入路径点 ==========
waypoints = np.array([
    [0.0, 0.0],
    [5.0, 3.0],
    [10.0, 2.0],
    [15.0, 6.0],
    [20.0, 5.0]
])

# ========== 分段拟合 ==========
T_seg = 2.0          # 每段时间
v0 = a0 = 0.0        # 起点静止
v1 = a1 = 0.0        # 终点静止

trajectory = []      # 存储 (t, x, y)
trajectory.append((0.0, waypoints[0][0], waypoints[0][1]))

for i in range(len(waypoints)-1):
    p0_x, p0_y = waypoints[i]
    p1_x, p1_y = waypoints[i+1]
    
    # 分别拟合x和y
    cx = quintic_coefficients(p0_x, v0, a0, p1_x, v1, a1, T_seg)
    cy = quintic_coefficients(p0_y, v0, a0, p1_y, v1, a1, T_seg)
    
    # 在段内采样20个点
    for j in range(1, 21):
        t_local = j * T_seg / 20
        t_global = i * T_seg + t_local
        x_val = evaluate_poly(cx, t_local)
        y_val = evaluate_poly(cy, t_local)
        trajectory.append((t_global, x_val, y_val))

# ========== 验证端点误差 ==========
print("=== 端点位置误差验证 ===")
for i, wp in enumerate(waypoints):
    # 找到对应时间点
    t_target = i * T_seg
    # 在轨迹中找最近时间点
    closest = min(trajectory, key=lambda p: abs(p[0] - t_target))
    err = np.sqrt((closest[1]-wp[0])**2 + (closest[2]-wp[1])**2)
    print(f"路径点{i}: 期望({wp[0]}, {wp[1]}) → 拟合({closest[1]:.3f}, {closest[2]:.3f}) 误差={err:.4f}m")

# ========== 验证连续性 ==========
print("\n=== 段间连续性验证 ===")
for i in range(len(waypoints)-2):
    # 检查第i段终点与第i+1段起点是否重合
    t_boundary = (i+1) * T_seg
    pts_at_boundary = [p for p in trajectory if abs(p[0] - t_boundary) < 1e-6]
    if len(pts_at_boundary) == 1:
        print(f"段{i}→段{i+1} 边界点: ({pts_at_boundary[0][1]:.3f}, {pts_at_boundary[0][2]:.3f})")
    else:
        print(f"段{i}→段{i+1} 边界点数量异常: {len(pts_at_boundary)}")

# ========== 导出轨迹点 ==========
np.savetxt("fitted_trajectory.csv", 
           np.array(trajectory), 
           delimiter=",", 
           header="t,x,y", 
           comments="")
print("\n轨迹点已保存至 fitted_trajectory.csv，共", len(trajectory), "个点")

落地方式

将上述代码保存为 quintic_fit.py，在已安装NumPy的Python环境中运行：

python quintic_fit.py

成功验证

运行后预期输出：

• 每个路径点的拟合误差均小于0.05m（实际误差接近0，因为端点被精确约束）；
• 段间边界点位置唯一且连续，无跳变；
• 生成 fitted_trajectory.csv 文件，包含101个轨迹点（5段×20采样+起点）。

该实现将5个路径点分为4段，每段独立求解五次多项式系数。由于每段端点位置被精确约束，且段间共享端点，因此整条轨迹在位置层面天然连续；同时每段端点速度、加速度均为0，保证了段间速度与加速度的连续性。最终生成的轨迹点可直接用于下游的轨迹跟踪控制模块。