091、编队飞行：虚拟结构法

飞控算法从入门到精通 · 091 编队飞行：虚拟结构法

一、一次编队试飞中的“幽灵漂移”

去年在松山湖做四机编队测试，三号机在直线编队时突然像被无形的手拽了一下，横向偏移了0.8米。地面站日志显示它的位置环输出正常，但编队控制器给出的期望位置却莫名其妙地偏了。排查了三个小时，最后发现是虚拟结构法里一个坐标变换的符号写反了——把“机体到虚拟结构”的旋转矩阵转置成了逆矩阵。这个bug在单机仿真里根本不会暴露，因为单机时虚拟结构退化为一个点，旋转矩阵退化为单位阵。但多机编队时，只要虚拟结构有旋转，这个错误就会让每架飞机的期望位置产生一个与虚拟结构角速度相关的漂移项。

那次之后我养成了一个习惯：所有涉及虚拟结构法的代码，第一件事就是打印虚拟结构坐标系到惯性系的旋转矩阵行列式，确保它严格等于1.0。

二、虚拟结构法的本质：把编队问题变成单机跟踪问题

虚拟结构法的核心思想很朴素：把整个编队想象成一个刚体，每架飞机是这个刚体上的一个固定点。编队飞行时，我们控制这个虚拟刚体的位置和姿态，每架飞机只需要跟踪自己在刚体上的那个固定点即可。

数学上，第i架飞机的期望位置可以写成：

p_i_des = p_v + R_v * r_i

其中p_v是虚拟结构中心的位置，R_v是虚拟结构坐标系到惯性系的旋转矩阵，r_i是第i架飞机在虚拟结构坐标系下的固定坐标。

这个公式看起来简单，但实际实现时有两个坑：

坑一：r_i的定义必须统一。我见过有人把r_i定义在机