导航向量场（GVF）算法¶

GVF（Guidance Vector Field）把“沿路径前进”和“向路径收敛”合成为一个连续向量场。单机示例将单位导航向量乘以巡航速度，再通过 /control_signal/vector 发送给机型控制器。

通用结构¶

对于由隐式函数 φ(p)=0 描述的路径，源码中的向量场可以概括为：

guide = tangent - K · error · gradient

增益越大，远离路径时收敛越强，但指令方向变化也可能更激烈。调参应同时观察横向误差、滚转角、速度变化和数值稳定性。

目标圆心为 c，半径为 r。gvf_circle.circle 使用有符号径向误差：

φ(p) = ||p-c|| - r
grad φ = (p-c) / ||p-c||
tau = R90 · grad φ
omega = tau - k φ grad φ

get_tau() 通过 90° 旋转矩阵选择顺时针或逆时针方向；get_omega() 将切向项与收敛项相加并归一化。圆心处梯度没有定义，源码返回零向量作为保护。

circle_track.py 的默认参数为：

高度方向没有使用圆 GVF，而是直接令 v_z = 50 - z。

gvf_inclined_circle.py 用圆柱面与倾斜平面的交线描述三维倾斜圆。

圆柱约束：

φ1(p) = ||p_circle - c_circle|| - r

倾斜平面约束：

φ2(p) = n · p + d
d = -n · point

两个曲面的梯度分别为 J1、J2。交线切向量由叉积得到：

v_t = J1 × J2
v_n = -k1 φ1 J1 - k2 φ2 J2
v_g = normalize(v_t + v_n)

inclinecircle_track.py 以 normal_vector=[1,0,10]、平面过点 [0,0,150]、圆柱半径 100 m 构造默认轨迹。

三维 Lissajous 轨迹通过辅助路径参数 w 表示：

p_i(w) = amplitude_i · cos(phase_i w + offset_i) + add_i
φ_i(p,w) = p_i - p_i(w)

算法在扩展空间 [x,y,z,w] 中构造误差雅可比。GetGuidVec() 使用：

converge = -φᵀ K Dφ
transport = -[Dφ 的 w 列, -1]
guide = converge + transport

空间分量按模长归一化，最后一个分量决定路径参数 w 的推进速度。lissajous_track.py 中的更新方式为：

w += cruise_speed · guide_w · dt
velocity_sp = cruise_speed · guide_xyz

这种扩展状态方法能处理同一空间位置可能对应多个路径参数的曲线。

多项式轨迹同样把路径参数 t 加入状态。目标流形由：

φ_i(p,t) = p_i - f_i(t)

定义。gvf_poly.GVF 分别计算：

核心组合为：

converge_perp = converge - projection(converge, transport)
vec = (-1)^ori transport - converge_perp

当 mc_flag=1 时，源码根据 Hessian 最小特征值增加对角修正，使矩阵保持最小正定裕量，再用于收敛方向计算。

spiraling_up.py 复用二维圆 GVF，水平速度沿圆向量场，垂向速度固定为 2 m/s。它不是单独的三维隐式曲线算法，而是“二维闭环 + 恒定爬升率”的组合示例。