第19章 多无人机协同控制

多无人机协同系统是无人机技术的重要发展方向。通过多架无人机的协同工作，可以完成单机无法胜任的复杂任务，如大面积搜索、协同运输、编队表演等。本章将介绍多机通信、编队控制、任务分配和多机仿真等核心技术。

本章学习目标：掌握多机通信架构设计方法；理解Leader-Follower和人工势场编队控制算法；掌握基于拍卖的任务分配策略；能够搭建多机Gazebo仿真环境；完成协同搜索与区域覆盖案例。

19.1 多机通信架构设计

多无人机协同的基础是可靠的通信架构。通信系统负责在无人机之间、无人机与地面站之间传输控制指令、状态信息和任务数据。

19.1.1 通信拓扑结构

多机通信网络的拓扑结构决定了信息传输的方式和效率。常见的拓扑结构包括：

• 1. 星型拓扑：所有无人机通过中心节点（地面站）通信

• 2. 网状拓扑：每架无人机可以与相邻无人机直接通信

• 3. 树型拓扑：分层结构，Leader负责协调下层无人机

• 4. 混合拓扑：结合多种拓扑的优势

表19-1 多机通信拓扑对比

拓扑类型	优点	缺点	适用场景
星型	简单可靠、易管理	单点故障、带宽瓶颈	小规模编队（<5架）
网状	冠余度高、鲁棒性强	复杂度高、带宽消耗大	大规模无人机群
树型	分层管理、可扩展	上层故障影响大	分层指挥系统
混合	综合优势	设计复杂	大型协同任务

对于小规模编队（3-10架），星型拓扑是最简单可靠的选择。地面站作为中心节点负责任务分配和状态监控。

19.1.2 通信协议与中间件

多机通信的协议选择直接影响系统的性能和可靠性。主流方案包括：

• 1. MAVLink：无人机领域的标准协议，通过MAVROS2与ROS2集成

• 2. ROS2 DDS：分布式发布-订阅模型，支持自动发现和多机通信

• 3. 自定义UDP/TCP：灵活但需要自己处理可靠性和序列化

• 4. ZeroMQ/MQTT：轻量级消息队列，适合弱网络环境

ROS2的DDS（Data Distribution Service）是多机通信的理想选择。通过配置DDS的发现机制，多架无人机可以自动发现彼此并建立通信。关键配置包括：

• 域名（Domain ID）：确保所有无人机在同一域

• QoS策略：根据数据类型选择可靠或最佳努力传输

• 命名空间：为每架无人机分配独立的命名空间（如/uav1/、/uav2/）

多机 ROS2 通信配置

设置域名（每架无人机相同）

export ROS_DOMAIN_ID=42

为每架无人机设置命名空间

uav1:

export ROS_NAMESPACE=/uav1

ros2 launch px4_ros_com offboard_control.launch.py

uav2:

export ROS_NAMESPACE=/uav2

ros2 launch px4_ros_com offboard_control.launch.py

地面站可以订阅所有无人机的主题

ros2 topic list | grep -E "/uav0-9+"

对于每架无人机，需要配置独立的MAVLink实例和端口。典型配置为：uav1使用UDP端口14540，uav2使用14541，依次类推。

提示

多机通信中的带宽管理非常重要。每架无人机的状态发布频率应控制在10Hz以下，避免网络拥塞。可以使用ROS2的QoS策略来优先保证关键数据的传输。

19.1.3 状态同步与共享

多机系统需要在无人机之间共享状态信息，包括位置、速度、电池电量、任务状态等。状态共享的设计原则：

• 最小化传输：只发送必要的状态信息

• 异步更新：不等待所有无人机的响应

• 时间戳管理：使用NTP同步时间，处理过时数据

• 冲突解决：多机同时抢占同一任务时的处理策略

19.1.4 多机PX4配置

在多机系统中，每架无人机的PX4需要配置独立的系统ID和MAVLink实例：

表19-2 多机PX4配置参数

参数	uav1	uav2	uav3
MAV_SYS_ID	1	2	3
MAV_TYPE	2 (四旋翼)	2 (四旋翼)	2 (四旋翼)
MAV_COMP_ID	1	1	1
UDP端口	14540	14541	14542
ROS命名空间	/uav1	/uav2	/uav3
GZ_MODEL_NAME	uav1	uav2	uav3

每架无人机的PX4固件可以相同，但必须通过参数区分不同的系统ID。在Gazebo仿真中，每架无人机还需要独立的模型实例和初始位置。

19.2 编队控制算法（Leader-Follower/人工势场）

编队控制是多无人机协同的核心技术，目标是让多架无人机保持特定的空间结构。常见的编队控制算法包括Leader-Follower、人工势场、基于行为的方法和虚拟结构法。

19.2.1 Leader-Follower编队控制

Leader-Follower是最直观的编队控制方法。一架无人机作为领导者（Leader），其他无人机（Follower）根据与Leader的相对位置偏差来调整自己的飞行路径。

Leader-Follower的核心原理：

• 1. Leader按照预定路径飞行，不关心Follower的状态

• 2. 每个Follower有一个相对Leader的期望偏移量（dx, dy, dz）

• 3. Follower的目标位置 = Leader当前位置 + 偏移量

• 4. Follower通过PID控制器跟踪目标位置

常见的编队队形包括：

• 一字队形：Follower在Leader后方等距排列

• V字队形：Follower在Leader后方两侧对称排列

• 三角队形：Follower组成三角形结构

• 矩形队形：Follower在Leader周围组成矩形

leader_follower.py - Leader-Follower编队控制

import numpy as np

from geometry_msgs.msg import PoseStamped

class FollowerController:

def init(self, formation_offset):

"""formation_offset: dx, dy, dz 相对Leader的偏移"""

self.offset = np.array(formation_offset)

self.kp = np.array(1.5, 1.5, 2.0) # 位置增益

self.kd = np.array(0.5, 0.5, 0.8) # 速度增益

self.max_vel = 3.0 # 最大速度 m/s

def compute_setpoint(self, leader_pose, follower_pose,

leader_vel, follower_vel):

计算目标位置

leader_pos = np.array([

leader_pose.position.x,

leader_pose.position.y,

leader_pose.position.z])

target_pos = leader_pos + self.offset

位置误差

follower_pos = np.array([

follower_pose.position.x,

follower_pose.position.y,

follower_pose.position.z])

pos_error = target_pos - follower_pos

速度误差

vel_error = np.array(leader_vel) - np.array(follower_vel)

PD控制

vel_cmd = self.kp * pos_error + self.kd * vel_error

速度限制

speed = np.linalg.norm(vel_cmd)

if speed > self.max_vel:

vel_cmd = vel_cmd / speed * self.max_vel

return vel_cmd

Leader-Follower方法的优点是简单直观、易于实现。缺点是Leader为单点故障，如果Leader失效，整个编队将失去引导。可以通过备任Leader机制来解决这个问题。

提示

Leader-Follower编队中，Follower的控制增益需要仔细调整。如果增益过高，Follower会出现振荡；如果增益过低，编队保持能力差。建议先在仿真中调参，确认稳定后再实飞。

19.2.2 人工势场编队控制

人工势场（Artificial Potential Field, APF）方法将编队中的每架无人机视为在势场中运动的粒子。通过设计合适的引力和斥力函数，可以实现编队保持和碰撞避免。

势场函数设计：

• 引力势场：目标位置产生的吸引力，使无人机向目标移动

• 斥力势场：其他无人机和障碀物产生的排斥力，避免碰撞

• 编队保持势场：维持期望的编队间距的弹簧力

合力计算：

• F_total = F_goal + F_repulsion + F_formation

• F_goal = -k_g * (p - p_goal)，引向目标

• F_repulsion = k_r * (1/d - 1/d0) * (1/d^2)，当d<d0时生效

• F_formation = k_f * (d - d_desired)，维持编队间距

人工势场方法的优点是去中心化，每架无人机独立计算自己的控制指令。缺点是可能出现局部最小值问题（死锁）。可以通过加入随机扰动或虚拟障碀物来解决。

提示

人工势场的参数调优是关键。引力增益k_g和斥力增益k_r的比例决定了编队保持与碰撞避免之间的平衡。建议k_r >> k_g，确保安全优先。

19.2.3 编队变换与重构

在实际任务中，编队可能需要动态变换队形以适应不同的场景。例如，通过狭窄通道时从三角队形变为一字队形。编队变换的实现方法：

• 1. 预定义队形库：预先定义多种队形及其偏移量

• 2. 平滑过渡：使用插值算法平滑切换偏移量

• 3. 分配算法：使用匈牙利算法最优分配新位置，最小化总移动距离

• 4. 冲突检测：确保变换过程中不会发生碰撞

19.2.4 编队控制的ROS2实现

在ROS2中实现编队控制的关键是正确的主题命名和数据流设计。每架无人机订阅其他无人机的状态，计算自己的控制指令。

• 地面站节点：发布编队目标和队形参数

• 每架无人机节点：订阅队友状态，计算并发布控制指令

• 监控节点：收集所有无人机状态，可视化编队状态

19.3 任务分配与调度策略

多机任务分配是将多个任务合理分配给多架无人机的过程。好的任务分配算法应该考虑无人机的能力、位置、电量等因素，最大化整体任务效率。

19.3.1 基于拍卖的任务分配

拍卖算法是多机任务分配的经典方法。每架无人机根据自己的状态对任务进行评估出价，然后由拍卖器将任务分配给出价最优的无人机。

拍卖算法的基本流程：

• 1. 任务发布：拍卖器广播待分配的任务列表

• 2. 出价计算：每架无人机计算执行每个任务的代价（距离、时间、能量）

• 3. 胜出确定：拍卖器收集所有出价，确定每个任务的胜出者

• 4. 任务确认：胜出的无人机确认接受任务

出价函数设计：

• Cost = w1distance + w2 time + w3energy + w4priority

• 距离代价：无人机到任务点的欧氏距离

• 时间代价：预计完成任务所需时间

• 能量代价：预计消耗的电池能量

• 优先级代价：任务的紧迫程度

auction_allocator.py - 拍卖任务分配

import numpy as np

from scipy.optimize import linear_sum_assignment

class AuctionAllocator:

def init(self, weights=None):

self.w = weights or {

'distance': 0.4,

'energy': 0.3,

'priority': 0.3

}

def compute_cost_matrix(self, uav_states, tasks):

n_uavs = len(uav_states)

n_tasks = len(tasks)

cost = np.zeros((n_uavs, n_tasks))

for i, uav in enumerate(uav_states):

for j, task in enumerate(tasks):

dist = np.linalg.norm(

np.array(uav'pos') - np.array(task'pos'))

energy_cost = dist / uav'battery'

priority = 1.0 / task.get('priority', 1.0)

costij = (self.w'distance' * dist +

self.w'energy' * energy_cost +

self.w'priority' * priority)

return cost

def allocate(self, uav_states, tasks):

cost_matrix = self.compute_cost_matrix(

uav_states, tasks)

row_ind, col_ind = linear_sum_assignment(cost_matrix)

assignments = {}

for r, c in zip(row_ind, col_ind):

assignmentsuav_states\[r'id'] = tasksc

return assignments

该实现使用匈牙利算法解决任务分配问题，确保总代价最小。当无人机数量和任务数量不等时，可以通过添加虚拟任务或虚拟无人机来平衡矩阵。

19.3.2 动态任务调度

在实际应用中，任务可能动态出现或变化。动态任务调度需要处理以下情况：

• 新任务出现：实时将新任务分配给空闲的无人机

• 无人机故障：重新分配故障无人机的任务

• 任务取消：释放已分配但未完成的任务

• 优先级变化：根据紧急程度调整任务执行顺序

动态调度的实现策略：

• 定期重分配：每隔N秒重新运行分配算法

• 事件触发：当发生重大变化时触发重分配

• 增量式分配：只调整受影响的任务，不全局重分配

提示

任务分配算法应该具备弹性。当无人机电量低于阈值时，应自动返航并将剩余任务重新分配。这是多机系统可靠性的关键。

19.3.3 任务状态管理

每个任务都有明确的生命周期：

• PENDING：待分配状态

• ASSIGNED：已分配给某架无人机

• IN_PROGRESS：正在执行中

• COMPLETED：已完成

• FAILED：执行失败，需要重新分配

• CANCELLED：已取消

19.4 多机仿真环境搭建（Gazebo + ROS2）

多机仿真是开发和验证协同算法的必要手段。Gazebo支持多无人机同时仿真，配合ROS2可以构建完整的多机控制系统。

19.4.1 多机Gazebo仿真配置

在Gazebo中启动多架无人机需要配置多个模型实例，每个实例有独立的名称和初始位置：

多机 Gazebo 启动脚本

#!/bin/bash

multi_uav_launch.sh

启动 Gazebo

gz sim -r empty.world &

启动多架无人机

for i in 1 2 3; do

计算初始位置 (V字队形)

x=(echo"scale=1;((echo "scale=1; ((echo"scale=1;(i - 2) * 2.0" | bc)

y=(echo"scale=1;abs((echo "scale=1; abs((echo"scale=1;abs(i - 2) * 1.5" | bc)

启动 PX4 SITL 实例

PX4_SYS_AUTOSTART=4001 </font>

PX4_GZ_MODEL_NAME=x500 </font>

PX4_GZ_MODEL_POSE="x,{x},x,{y},0,0,0,0" </font>

PX4_GZ_NAME="uav${i}" </font>

./build/px4_sitl_default/bin/px4 </font>

-i $((i - 1)) &

启动对应的 ROS2 bridge

ROS_NAMESPACE="/uav${i}" </font>

./build/px4_sitl_default/bin/px4-ros2-bridge &

done

每个PX4 SITL实例需要独立的实例ID（-i参数）和系统ID。初始位置通过PX4_GZ_MODEL_POSE参数设置。

19.4.2 多机通信配置

多机仿真的通信配置需要确保每架无人机的消息不会冲突。关键配置包括：

• 命名空间隔离：每架无人机使用独立的ROS2命名空间

• MAVLink端口分配：每架无人机使用不同的UDP端口

• PX4实例隔离：每个实例有独立的进程和数据目录

表19-3 多机仿真端口分配

无人机	PX4实例ID	MAVLink****端口	ROS2****命名空间	初始位置
uav1	0	14540	/uav1	0, 0, 0
uav2	1	14541	/uav2	-2, 1.5, 0
uav3	2	14542	/uav3	2, 1.5, 0
uav4	3	14543	/uav4	-4, 3, 0
uav5	4	14544	/uav5	4, 3, 0

19.4.3 多机控制节点

多机控制节点负责协调所有无人机的行为。它订阅每架无人机的状态，计算编队控制指令，并发送给每架无人机。

• 状态订阅：订阅/uavN/fmu/out/vehicle_status和/odometry

• 控制发布：发布到/uavN/fmu/in/trajectory_setpoint

• 编队计算：根据算法计算每架无人机的目标

19.4.4 仿真环境配置优化

多机仿真对计算资源的要求很高。优化策略包括：

• 降低Gazebo实时因子（RTF）以减少计算负担

• 禁用不必要的传感器仿真（如摄像头）

• 使用headless模式运行Gazebo（无GUI）

• 分布式仿真：将不同无人机分布到不同计算机

提示

多机仿真需要强大的硬件。5架以上的仿真建议使用16核以上的CPU和32GB以上的内存。可以先用2-3架进行开发测试，确认算法正确后再增加数量。

19.5 协同搜索与区域覆盖案例

协同搜索与区域覆盖是多机协同的典型应用。目标是让多架无人机协同工作，在最短时间内覆盖指定区域的所有位置。

19.5.1 覆盖路径规划

区域覆盖的核心是覆盖路径规划（Coverage Path Planning, CPP）。常用的覆盖模式包括：

• 往复式（Boustrophedon）：类似割草机的往复路径，最简单有效

• 蜂巢式（Hexagonal）：六角形覆盖，适合圆形传感器视场

• 螺旋式（Spiral）：从外向内螺旋，适合特定场景

多机覆盖的关键是区域分割。将大区域划分为多个子区域，每架无人机负责一个子区域。分割策略包括：

• 等面积分割：每个子区域面积相等

• Voronoi分割：根据无人机初始位置进行Voronoi分割

• 能力自适应分割：根据无人机的电量和速度调整子区域大小

表19-4 覆盖路径规划参数

参数	说明	典型值	影响
搜索宽度	传感器视场宽度	5-20m	覆盖效率
重叠率	相邻航线重叠比例	20-30%	覆盖完整性
飞行高度	搜索高度	10-50m	分辨率和视场
飞行速度	搜索速度	3-8m/s	搜索时间
起点位置	每架无人机的起始位置	均匀分布	初始分配效率

19.5.2 协同搜索算法实现

协同搜索的完整流程包括以下步骤：

• 1. 区域分割：将目标区域分割为多个子区域

• 2. 任务分配：使用拍卖算法分配子区域给无人机

• 3. 路径规划：为每架无人机生成往复式覆盖路径

• 4. 执行监控：监控搜索进度，处理异常情况

• 5. 完成汇报：汇总搜索结果，生成覆盖地图

coverage_planner.py - 多机覆盖路径规划

import numpy as np

from shapely.geometry import Polygon, box

from shapely.ops import unary_union

class CoveragePlanner:

def init(self, search_width=10.0, overlap=0.2):

self.width = search_width

self.overlap = overlap

self.effective_width = search_width * (1 - overlap)

def partition_region(self, region_polygon, n_uavs):

"""Voronoi分割区域"""

bounds = region_polygon.bounds

centroids = self._generate_centroids(

bounds, n_uavs)

regions = \[\]

for i, c in enumerate(centroids):

简化的等面积分割

region = self._split_region(

region_polygon, i, n_uavs)

regions.append(region)

return regions

def generate_boustrophedon(self, region, altitude):

"""生成往复式覆盖路径"""

bounds = region.bounds

x_min, y_min, x_max, y_max = bounds

waypoints = \[\]

y = y_min

direction = True

while y <= y_max:

if direction:

waypoints.append(x_min, y, altitude)

waypoints.append(x_max, y, altitude)

else:

waypoints.append(x_max, y, altitude)

waypoints.append(x_min, y, altitude)

y += self.effective_width

direction = not direction

return waypoints

该实现包含区域分割和往复式覆盖路径生成。在实际应用中，还需要处理不规则区域、障碀物避免和动态重规划。

19.5.3 完整的协同搜索案例

一个完整的多机协同搜索系统包含以下节点：

• 1. 任务规划节点：接收搜索区域，生成分割和路径

• 2. 任务分配节点：使用拍卖算法分配子区域

• 3. 每架无人机的飞行控制节点：跟踪覆盖路径

• 4. 监控节点：收集搜索进度，可视化覆盖地图

• 5. 异常处理节点：处理无人机故障、电量不足等异常

系统的典型性能指标：

• 3架无人机覆盖1平方公里区域：级10-15分钟

• 覆盖完整率：>98%

• 重复覆盖率：<25%

提示

多机协同搜索的效率并不总是线性提升。由于区域分割的不均匀性和路径规划的开销，实际效率提升通常为0.7-0.9倍。建议根据任务需求选择合适的无人机数量。

19.6 本章小结

本章系统介绍了多无人机协同控制的核心技术。主要内容包括：

• 1. 多机通信架构：介绍了星型、网状、树型等通信拓扑，以及ROS2 DDS通信配置

• 2. 编队控制算法：详细介绍Leader-Follower和人工势场两种编队控制方法

• 3. 任务分配与调度：介绍基于拍卖的任务分配算法和动态调度策略

• 4. 多机仿真环境：介绍Sazebo多机仿真的配置和通信设置

• 5. 协同搜索案例：实现了完整的多机区域覆盖系统

下一章将综合应用前面所学的技术，设计并实现一个完整的自主巡检系统项目。