分段多项式轨迹

多项式轨迹由 Polynomial.py 和 PolynomialTraj.py 两层组成：前者处理单个维度的分段三次多项式，后者把多个维度组合为参数轨迹，并为 GVF 提供误差、雅可比和 Hessian。

单段三次多项式

每段轨迹使用局部参数 τ=t-t_i：

x(τ) = a0 + a1 τ + a2 τ² + a3 τ³

四个系数由段首和段末的位置、速度约束确定：

x(0), x'(0), x(T), x'(T)

Polynomial3Order.MCal() 按每段时长 T 构造映射矩阵，PolynomialTemplate.ACal() 将端点条件转换为系数。GetPos()、GetD()、GetDD()、Get3D() 分别返回位置、一阶导、二阶导和三阶导。

多段拼接

PolynomialTemplate 保存：

  • key_points：每段端点的位置与速度；
  • t_list：各关键点对应的路径参数；
  • M：各段端点条件到系数的映射；
  • A_list：各段实际多项式系数。

FindIndex() 确定当前 t 落在哪一段，并缓存上一次结果。若 t 超出范围，底层类会抛出异常；上层 PolynomialTraj3Order.AdjustT() 负责先做循环或钳位。

自动关键点速度

AutoVel() 根据相邻线段方向构造关键点速度。中间点的方向近似为入段和出段方向的角平分方向，模长使用类内参考值 self.v=0.1。开放轨迹的首尾速度为零；闭合轨迹还会处理首尾衔接。

当相邻点重合或两段方向相反时，归一化可能失效，因此关键点应去重，并避免未处理的 180° 折返。

路径参数分配

AutoTlist() 先求总折线长度，再按每段长度分配参数区间。由于：

t_range = total_length / self.v
segment_dt = segment_length / self.v

较长线段会得到更长的参数区间。这里的 self.v 是轨迹参数化内部参考量，不是最终飞机巡航速度；真实速度由上层 cruise_speed 与 GVF 输出共同决定。

开放与闭合轨迹

loop_flag 超出末端后的处理
0 钳位到首端或末端
1 对总参数周期取模，回到闭合轨迹起点

闭合模式会在构造时自动把首点附加到末尾，并复制相应边界速度。

给 GVF 的误差量

对 D 维轨迹 f(t)，扩展状态为 [p,t]，误差为：

φ(p,t) = p - f(t)

源码提供：

  • PhiJacobianCal()：[I, -f'(t)]；
  • EJacobianCal()：二次误差能量 E=1/2 φᵀφ 的梯度；
  • EHessianCal()：用于混合收敛策略的 Hessian 近似；
  • TangentCal() / TangentVCal()：单位/未归一化切向量。

Update() 对相同位置和参数使用缓存，四个 Get* 接口都先调用它。

曲率、挠率与标架角

CurveCal() 与 CurveCal2() 使用一至三阶导数计算三维曲率和挠率。ThetaCal() 通过数值积分累积负挠率，用于更新随曲线移动的标架角。

曲率/挠率计算要求三维轨迹且一阶导数、叉积模长不能接近零。直线段或退化曲线处应显式处理返回值。

新建轨迹

points = np.array([
    [0.0,   0.0, 100.0],
    [200.0, 0.0, 100.0],
    [300.0, 150.0, 120.0],
])

traj = PolynomialTraj3Order(
    dimension=3,
    points=points,
    loop_flag=0,
)
gvf = GVF(traj, k=0.003, mc_flag=0, ori=1)

建议先用 GetSampleTraj() 绘制参考轨迹，再接入飞行控制。完整函数见Polynomial.py与PolynomialTraj.py。