多机路径跟踪与编队算法

multi_demo 的算法以二维双积分质点模型为基础，先计算期望水平加速度，再根据当前速度分解为轴向加速度和滚转角，最后由每架飞机的控制节点执行。

共同状态模型

状态和输入定义为：

x = [p_x, p_y, v_x, v_y]ᵀ
u = [a_x, a_y]ᵀ
x_dot = A x + B u

其中：

A = [[0,0,1,0],
     [0,0,0,1],
     [0,0,0,0],
     [0,0,0,0]]

B = [[0,0],
     [0,0],
     [1,0],
     [0,1]]

PathFollow、Follower 和 ConsensusFollow 都用 scipy.linalg.solve_continuous_are() 求连续代数 Riccati 方程，并构造：

K = -R⁻¹ Bᵀ P
u = C K (x - x_desire)

折线路径 DesirePath

DesirePath 接收二维路径点和期望速度。初始化时先按经验二次多项式计算“提前距离”，并要求任意相邻路径点距离大于该值。

NearestP() 的处理顺序：

  1. 计算当前位置到每条有限线段的最近点；
  2. 选出距离最小的线段；
  3. 若最近点接近线段末端，则跨到下一段计算提前点；
  4. 用当前目标线段单位方向乘 desire_v 得到期望速度。

提前点让飞机在折线转角前开始改变方向，减少到达顶点后才急转的情况。

领航者路径跟踪 PathFollow

Acal() 将路径投影点与路径切向速度组合为期望状态：

converge_v = (nearest_point - current_position) / converge_k
desire_v = normalize(path_v + converge_v) · path_speed
desire_state = [nearest_point, desire_v]

LQR 输出随后限制到 10 m/s²，并在速度小于 10 m/s 时移除继续减速的切向加速度，在速度大于 50 m/s 时移除继续加速的切向分量。

leader_uav.py 把算法类接入 ROS，并广播：

  • /leader/desire_p：实际领航位置；
  • /leader/desire_v：实际领航速度；
  • /leader/desire_roll：当前期望滚转角。

领航—跟随 Follower

跟随者偏移 offset 定义在领航者航向坐标系。DesireStatusCal() 先根据领航速度构造二维旋转矩阵：

R(psi) = [[cos psi, -sin psi],
          [sin psi,  cos psi]]

p_desire = p_leader + R(psi) offset

直线飞行时跟随者期望速度等于领航速度。转弯时根据领航者转弯半径估计角速度，并加入 omega × offset，使外侧跟随者速度更高、内侧更低。

follower_uav.py 从领航滚转角估计转弯半径：

r = ||v_leader||² / (g tan(roll_leader))

随后用 LQR 跟踪编队目标。

分布式一致性 ConsensusFollow

一致性方案不依赖单独的领航广播。每架无人机根据自身编队偏移计算一个参考点，并把它发布到 /uavN/consensus_data。只订阅图拉普拉斯矩阵中与自己相邻的节点。

一致性速度项为：

v_consensus = -k_c · L_i · P_all
  • L_i：图拉普拉斯矩阵第 i 行；
  • P_all：各成员参考点组成的矩阵；
  • k_c：consensus_k；
  • 当前实现把一致性速度限制在 4 m/s。

最终期望速度由路径速度、路径收敛速度和一致性速度相加，再由 LQR 计算加速度。加速度限制为 5 m/s²。

默认 CircleCommunicationMGenerate() 生成环形拓扑，每个节点只连接前后两个邻居。矩阵对角线为 2，相邻项为 -1。

加速度到飞行指令 AngleCal()

将二维加速度 a_vec 沿当前速度方向分解：

a_longitudinal = a_vec · v / ||v||
a_normal = a_vec - projection(a_vec, v)
roll = atan(||a_normal|| / g)

源码再通过将速度向量旋转 90°，比较法向加速度方向，确定滚转角符号。偏航角取路径或领航速度方向，高度由独立的爬升率/俯仰角逻辑控制。

队形函数

  • CircleForm(n, distance)：以给定成员数和相邻弧长计算圆形偏移；
  • RectangleCompact(n, distance)：按近似方形行列生成紧凑矩形偏移，跳过原点位置。

扩展到 N 架飞机

需要同步处理：

  1. PX4 实例数量、实例 ID 与模型名称；
  2. 每个 MAVROS 实例的 UDP 端口；
  3. /uav0 至 /uavN-1 命名空间；
  4. 算法 launch 中的 uav_id、uav_amount；
  5. 队形偏移数量；
  6. 图拉普拉斯矩阵维度与连通性；
  7. 计算机 CPU、内存和实时因子。

完整函数参考从multi_demo API进入。