ARYA-mgc/Swayam_Fleet

# Swayam — MAVLink 群体通信与机队管理

Multi-rotor drones performing coordinated formation flight — real-world swarm deployment

## 生态系统 **Swayam** 是 **ins-drone-pixhawk** 生态系统中的一个模块 —— 这是一个用于构建无 GPS 自主无人机系统的代码库集合： | 代码库 | 职责 | |---|---| | [`ins-drone-pixhawk`](https://github.com/ARYA-mgc/ins-drone-pixhawk) | 核心 INS 导航、传感器融合与飞行控制 | | **`Swayam_Fleet`** *(本代码库)* | 多无人机群体协同、机队管理与 GCS 中继 | 所有模块共享通用的 MAVLink 传输层，并被设计为在 **Pixhawk Cube Orange** + **Raspberry Pi 4** 硬件上协同运行。 ## 功能特性 | 功能 | 描述 | |---|---| | **INS 导航** | 通过 IMU 积分进行航位推算 —— 机体坐标系到世界坐标系的旋转、重力补偿、速度与位置追踪。无需 GPS。 | | **A\* 路径规划** | 在 50x50 米的占用网格上进行 8 方向 A\* 搜索，采用欧几里得启发式算法和障碍物膨胀处理。 | | **MAVLink 集成** | `SET_POSITION_TARGET_LOCAL_NED` 设定点、ARM/DISARM、模式切换、`SCALED_IMU2` 遥测。支持 UDP、TCP 或串行连接。 | | **机队协同** | 在并行线程中管理 N 架无人机。支持广播任务或单独分配。支持一键紧急降落所有无人机。 | | **航天级控制** | 完整的级联 PID 位置控制环、显式加加速度限制、8Hz D 项低通滤波器 (LPF) 以及反演算 Anti-Windup。 | | **安全保证** | 控制障碍函数 (CBF) 提供可证明的 1.5m 最小间隔保证。运行时安全不变量监控器。Lyapunov 稳定性证书。NaN/Inf 输出防护。 | | **防碰撞** | 基于叉积判别式的速度障碍 (VO) 预测。具有聚合多威胁排斥和确定性对称破断的紧急包络。 | | **地理围栏** | 绝对地理围栏优先级（越界时执行强制 RTL，沿边界滑动）高于所有任务逻辑。 | | **SQLite 数据库** | 3 表架构：`flight_logs`、`ins_telemetry`、`missions`。支持线程安全及 JSON 导出。 | | **Mission Planner GCS** | 与 Mission Planner 原生集成 —— 作为 MAVLink 中继，允许 GCS 通过标准 UDP/TCP 端口发现并控制整个集群。 | | **硬件平台** | 针对 **Pixhawk Cube Orange** + **Raspberry Pi 4** ( Companion Computer ) 通过 Serial/MAVLink 进行了优化。支持 HITL。 | | **高级中继** | 高性能 MAVLink 多路复用器 (`swarm_gcs_relay.py`)，用于将集群流量聚合到单个 GCS 实例中。 | | **加密** | 具有 Anti-Replay 保护的 AES-GCM 加密无人机间通信 (`swarm_security.py`)。 | ## 架构 ``` swayam/ ├── swayam_core.py # Core library — 5 classes ├── swarm_autonomous_logic.py # Swarm logic — VO, flocking, PID, geofence ├── safety_guarantees.py # CBF, Lyapunov, SafetyInvariant, IEEE-754 guards ├── swarm_gcs_relay.py # MAVLink multiplexer / router ├── advanced_telem_bridge.py # INS-to-MAVLink telemetry mapper ├── mission_planner_config.py # Mission Planner connection helper ├── mavlink_bridge.py # Robust Pi-to-Cube MAVLink bridge ├── swarm_telemetry.py # Swarm-wide UDP telemetry broadcaster ├── pi4_swarm_node.py # Main RPi4 node controller ├── mission_manager.py # Multi-drone waypoint mission handler ├── system_health.py # RPi4 + Pixhawk resource monitoring ├── swarm_security.py # AES-GCM encryption & anti-replay ├── swarm_commands.py # Inter-drone command definitions ├── pi_hardware_config.py # RPi 4 + Cube Orange hardware config ├── swarm_sync.py # Multi-drone synchronization logic ├── tests/ # 57 pytest tests (safety, swarm, stress, comm) ├── requirements.txt └── README.md ``` ### 核心类 — `swayam_core.py` | 类 | 职责 | |---|---| | `INSState` | 航位推算导航 (捷联惯导系统) | | `GridMap` + `A*` | 占用网格与最优路径规划 | | `DroneAgent` | 单个无人机 —— MAVLink I/O、遥测、任务 | | `FlightDatabase` | SQLite 持久层 | | `SwayamFleet` | 机队协调器 —— 并行任务、日志记录 | ## 快速开始 ### 1. 克隆与安装 ``` git clone https://github.com/ARYA-mgc/Swayam_Fleet.git cd Swayam_Fleet pip install -r requirements.txt ``` ### 2. 运行仿真 ``` python scripts/run_simulation.py ``` ### 3. 连接 Mission Planner 1. 打开 **Mission Planner**。 2. 选择 **UDP** → 点击 **Connect**。 3. 输入端口 `14550`。 4. 无人机 **ALPHA**、**BETA** 和 **GAMMA** 将自动出现。 ### 4. 运行测试 ``` pytest tests/ -v ``` ## 硬件集成 ### ArduPilot / Pixhawk (UDP) ``` from src.swayam_core import SwayamFleet fleet = SwayamFleet() fleet.add_drone("ALPHA", system_id=1, connection_string="udp:192.168.1.10:14550", simulation=False) fleet.connect_all() fleet.execute_mission("ALPHA", goal_n=20.0, goal_e=15.0, altitude=10.0) ``` ### STM32 + CAN-MAVLink 桥接器 (串行) ``` fleet.add_drone("BETA", system_id=2, connection_string="/dev/ttyUSB0,115200", simulation=False) ``` ### SITL (软件在环) ``` # 启动 ArduPilot SITL sim_vehicle.py -v ArduCopter --out=udp:127.0.0.1:14550 # 连接 Swayam python -c " from src.swayam_core import SwayamFleet f = SwayamFleet() f.add_drone('SIM', connection_string='udp:127.0.0.1:14550', simulation=False) f.connect_all() f.execute_mission('SIM', 10, 10, 15) " ``` ## 可视化文档 ### Mission Planner GCS 界面  ### 飞行前状态与诊断  ### 集群编队飞行  ### 机队距离指标  ### 航点收敛  ## INS 导航详情 `INSState` 类实现了一个捷联惯性导航系统： | 步骤 | 操作 | 公式 | |---|---|---| | 1 | **姿态更新** — 陀螺仪积分 | `roll += ωx · dt`, `pitch += ωy · dt`, `yaw += ωz · dt` | | 2 | **机体 → 世界旋转** | 将完整的 ZYX 欧拉旋转矩阵应用于加速度计读数 | | 3 | **重力去除** | 从世界坐标系 Z 轴 (NED 向下) 减去 `g = 9.80665 m/s²` | | 4 | **速度积分** | `v += a_world · dt` | | 5 | **位置积分** | `p += v · dt` | INS 数据来源于 `SCALED_IMU2` MAVLink 消息 (单位：milli-g, milli-rad/s)。 ## 航天级控制架构 Swayam 实现了严格受限的控制栈，用于硬件在环 (HITL) 和真实世界部署： - **级联 PID 环：** 外环 (位置到速度) 驱动内环 (速度到加速度)。向 FCU 输出有界的速度向量。 - **连续增益调度：** PID 增益基于 ESKF 置信度分数进行线性插值 (LERP)，以便在传感器质量下降时动态抑制响应。 - **物理包络限制：** 输出信号通过加加速度限制 (`j_max = 20 m/s^3`)、D 项上的低通滤波器 (8 Hz) 以及正式的反演算 Anti-Windup 进行主动滤波。 - **绝对安全层级：** `Geofence > Collision > Stability > Formation > Mission`。如果突破水平/垂直边界，无人机将立即触发 RTL，放弃所有其他集群逻辑。 ## 安全保证 — `safety_guarantees.py` 系统通过三个形式化层提供数学上可证明的安全性： | 层级 | 机制 | 保证 | |---|---|---| | **控制障碍函数** | `h(x) = \|\|p_i - p_j\|\|^2 - d_safe^2` | 通过速度投影实现最小 1.5m 的成对间隔 | | **安全不变量监控器** | 对间隔、速度、地理围栏进行运行时断言 | 记录所有带有时间戳的违规行为 | | **Lyapunov 证书** | `V(e) = 0.5 * (w_p * \|\|e_p\|\|^2 + w_v * \|\|e_v\|\|^2)` | PID 控制器能量非递增 | **CBF 执行流程：** ``` Mission Velocity --> Flocking Blend --> Geofence Clamp --> CBF Projection --> PID --> Sanitize --> FCU | Enforces: dh/dt + alpha * h >= 0 Method: Analytical half-space projection ``` **数值鲁棒性：** - IEEE-754 安全除法 (`safe_div`) 和归一化 (`safe_normalize`) - 每个控制路径上的 NaN/Inf 输出防护 (`sanitize_output`) - VO 未命中距离使用精确的叉积公式：`d_miss = |dp x dv| / |dv|` ## A* 路径规划 | 参数 | 值 | |---|---| | 网格 | 50 × 50 单元格，1 m/格 | | 起点 | 无人机当前的 INS 位置 (映射至网格) | | 目标 | 目标北/东坐标 | | 移动 | 8 方向 (允许对角线) | | 启发式 | 欧几里得距离 (可采纳 → 最优) | | 障碍物 | 基于半径的膨胀 | ``` fleet.add_obstacle(x=10, y=15, radius=2) # 5×5 blocked area path = fleet.plan_path("ALPHA", goal_n=20, goal_e=18) ``` ## Mission Planner 集成 系统使用 `SwayamFleet` 作为 MAVLink 中继。每个 `DroneAgent` 连接到 GCS (Mission Planner) 并转发遥测数据。 ### 高级多路复用 — `swarm_gcs_relay.py` 对于大型集群，使用 `SwarmGCSRelay` 将所有无人机聚合到单个流中： ``` relay = SwarmGCSRelay("udpout:127.0.0.1:14550") relay.add_drone(1, "udpin:127.0.0.1:14551") # Drone 1 relay.add_drone(2, "udpin:127.0.0.1:14552") # Drone 2 relay.start() ``` ### INS → MAVLink 映射 — `advanced_telem_bridge.py` | MAVLink 消息 | 映射数据 | |---|---| | `LOCAL_POSITION_NED` | 距起点的相对坐标 | | `GLOBAL_POSITION_INT` | 在 Mission Planner 地图上进行类 GPS 可视化 | | `ATTITUDE` | 高频率的横滚、俯仰、偏航 | | `SYS_STATUS` | 电池与健康监控 | ## 数据库架构 ``` flight_logs (id, drone_id, timestamp, level, event, details) ins_telemetry (id, drone_id, timestamp, pos_n, pos_e, pos_d, vel_n, vel_e, vel_d, roll, pitch, yaw) missions (id, drone_id, start_time, end_time, status, path_json, notes) ``` ## CI/CD GitHub Actions 在每次推送时运行： | 步骤 | 详情 | |---|---| | **测试矩阵** | Python 3.9, 3.10, 3.11 | | **覆盖率** | 完整的覆盖率报告 | | **仿真** | 无头集群仿真 | | **产物** | 上传 `swayam_export.json` | ## 许可证 MIT — 见 [LICENSE](LICENSE)

ins-drone-pixhawk 生态系统的一部分
由 ARYA-mgc 构建

标签：A*算法, MAVLink, Pixhawk, Python, TCP, UDP, 串口通信, 传感器融合, 地面控制站, 实时通信, 开源, 指令传输, 无GPS导航, 无人机, 无人机管理, 无人机群, 无人机通信, 无后门, 机器人学, 机载计算机, 物联网, 编队飞行, 自主系统, 航位推算, 路径规划, 遥测, 集群控制, 雷达避障, 飞行控制