用Python玩转PX4 Offboard模式：5行代码实现Gazebo无人机编队飞行

如果你正在研究无人机集群算法，或者想快速验证多机协同飞行的想法，那么PX4的Offboard模式配合Gazebo仿真绝对是你绕不开的利器。但很多朋友第一次接触时，往往被复杂的ROS节点、MAVROS消息和坐标系转换搞得晕头转向，写了几百行代码，无人机还在原地打转。其实，用Python控制多架仿真无人机，实现基础的编队飞行，核心逻辑可以简化到令人惊讶的程度——只需要理解几个关键概念，你就能用短短几行代码，让虚拟世界中的无人机群优雅地飞起来。

这篇文章就是为你准备的实战指南。我们不谈复杂的理论推导，直接从代码入手，手把手带你搭建一个可运行的多机仿真环境，并用Python脚本实现一个简单的三角形编队。你会发现，所谓的“高级控制”，其底层通信和坐标转换，完全可以封装成简洁的接口。无论是学术研究中的算法原型验证，还是工程项目中的快速演示，这套方法都能帮你节省大量时间，把精力集中在更有创造性的算法设计上。

1. 环境搭建：从零启动你的第一个仿真世界

在开始写控制代码之前，一个稳定、可重复的仿真环境是基础。这里我们选择 PX4 + Gazebo + ROS Melodic 这套经典组合。别被这些名词吓到，它们的分工非常明确：PX4是飞控软件，负责模拟真实无人机的“大脑”；Gazebo是物理仿真引擎，提供逼真的三维环境和动力学模型；ROS则是连接它们的“神经系统”，让我们的Python脚本能够向飞控发送指令。

1.1 一键部署仿真环境

最省心的方式是使用PX4官方提供的Docker镜像或一键安装脚本。但对于需要深度定制和调试的研究者，我推荐从源码编译，这样你对整个工具链会有更强的掌控力。假设你已经在Ubuntu 18.04或20.04系统上，打开终端，跟着下面的步骤走。

首先，创建工作空间并克隆必要的代码库：

# 创建并进入工作目录
mkdir -p ~/px4_ws/src
cd ~/px4_ws/src

# 克隆PX4固件（这里使用稳定版本）
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
cd PX4-Autopilot
git checkout v1.13.2  # 选择一个稳定版本
git submodule update --init --recursive

# 返回src目录，克隆MAVROS（ROS与PX4的通信桥梁）
cd ~/px4_ws/src
git clone https://github.com/mavlink/mavros.git
cd mavros
git checkout melodic-devel  # 对应ROS Melodic

接下来，安装系统依赖和编译。PX4提供了一个非常方便的脚本：

cd ~/px4_ws/src/PX4-Autopilot
bash ./Tools/setup/ubuntu.sh  # 这会安装Gazebo、ROS等大量依赖，需要一些时间

注意：安装过程会下载约2GB的数据，请确保网络通畅。如果遇到权限问题，可能需要使用sudo运行部分命令。

依赖安装完成后，开始编译PX4固件和MAVROS：

# 编译PX4固件（针对SITL仿真）
cd ~/px4_ws/src/PX4-Autopilot
make px4_sitl_default gazebo

# 打开新的终端，编译MAVROS及你的工作空间
cd ~/px4_ws
catkin build  # 或者使用 catkin_make
source devel/setup.bash

如果一切顺利，你现在应该可以通过一条命令启动一个带有一架无人机的Gazebo仿真世界：

cd ~/px4_ws/src/PX4-Autopilot
source ~/px4_ws/devel/setup.bash
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$(pwd)/Tools/sitl_gazebo/models

# 启动基础仿真
roslaunch px4 mavros_posix_sitl.launch

你会看到Gazebo界面弹出，一架 Iris 无人机模型出现在空旷的世界中。同时，终端里会滚动大量ROS和PX4的启动信息。这意味着你的仿真环境已经成功运行，PX4飞控和MAVROS桥接服务都已就绪。

1.2 理解仿真中的关键ROS话题

控制无人机的本质，就是向正确的ROS话题发布正确的消息。在单机仿真启动后，打开另一个终端，输入 rostopic list，你会看到一长串话题列表。对于基础的Offboard位置控制，我们只需要关注其中几个：

• /mavros/state： 无人机的状态信息，包括连接状态、当前飞行模式、是否上锁等。在发送任何控制指令前，必须先确认connected为True。
• /mavros/setpoint_position/local： 这是我们发送本地位置设定点的主要话题。消息类型是geometry_msgs/PoseStamped，包含位置(x, y, z)和姿态（四元数）。
• /mavros/setpoint_raw/local： 一个更底层的控制接口，消息类型是mavros_msgs/PositionTarget。它允许你指定坐标系、控制哪些量（例如忽略速度只控制位置），功能更强大，也是我们后续编队控制将使用的接口。
• /mavros/cmd/arming： 服务（Service），用于给无人机上锁（Arm）或解锁（Disarm）。
• /mavros/set_mode： 服务，用于设置飞行模式，我们要用的就是OFFBOARD模式。

为了验证通信是否正常，我们可以用一个简单的Python脚本来“听一听”无人机的状态。创建一个名为check_connection.py的文件：

#!/usr/bin/env python
import rospy
from mavros_msgs.msg import State

def state_callback(msg):
    print(f"Connected: {msg.connected}, Armed: {msg.armed}, Mode: {msg.mode}")

if __name__ == '__main__':
    rospy.init_node('listener_node')
    rospy.Subscriber("/mavros/state", State, state_callback)
    rospy.spin()  # 保持节点运行，等待消息

运行这个脚本，如果看到Connected: True，恭喜你，Python到PX4的通信链路已经打通。

2. Offboard模式精讲：从单机悬停到理解控制流

很多教程会直接给你一大段“万能”起飞代码，但如果不理解背后的逻辑，一旦出现问题就会无从下手。让我们拆解一下，用Offboard模式控制一架无人机到底需要几步。

2.1 Offboard模式的安全逻辑

PX4的Offboard模式是一种外部控制模式，意味着飞控完全听从外部计算机（即你的脚本）发送的设定点指令。为了安全，PX4设定了几个进入Offboard模式的前提条件：

1. 飞控与外部计算机（MAVROS）必须建立稳定连接。
2. 必须在进入Offboard模式前，持续以高于2Hz的频率发送设定点指令。这是为了防止通信中断后无人机失控。通常我们会以10Hz或20Hz的频率发送。
3. 必须成功切换模式到OFFBOARD，并且成功给无人机上锁（Arm）。

这个过程就像一个安全握手协议。你的脚本需要先“证明”自己在线且能持续提供指令，飞控才敢把控制权交出来。下面这个流程图概括了核心步骤：

[连接MAVROS] -> [持续发布设定点] -> [切换至OFFBOARD模式] -> [上锁] -> [持续控制]
       |               |                    |                 |         |
    等待连接        频率>2Hz             调用服务          调用服务   发布新目标点

2.2 用Python实现单机定点悬停

理解了逻辑，代码就非常直观了。我们创建一个名为single_drone_hover.py的文件：

#!/usr/bin/env python
import rospy
import time
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode

class SimpleOffboard:
    def __init__(self):
        self.current_state = State()
        self.local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        self.state_sub = rospy.Subscriber('/mavros/state', State, self.state_cb)
        self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
        self.set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
        self.rate = rospy.Rate(20)  # 20Hz，满足>2Hz的要求
        self.target_pose = PoseStamped()

    def state_cb(self, msg):
        self.current_state = msg

    def run(self):
        # 等待飞控连接
        while not rospy.is_shutdown() and not self.current_state.connected:
            self.rate.sleep()
        rospy.loginfo("PX4 Connected!")

        # 发送一些初始设定点
        self.target_pose.pose.position.x = 0
        self.target_pose.pose.position.y = 0
        self.target_pose.pose.position.z = 2  # 目标高度2米
        for _ in range(100):  # 发送100次，持续约5秒
            self.local_pos_pub.publish(self.target_pose)
            self.rate.sleep()

        # 尝试切换到OFFBOARD模式
        offb_set_mode = SetMode()
        offb_set_mode.request.custom_mode = 'OFFBOARD'
        last_request = rospy.Time.now()

        while not rospy.is_shutdown():
            # 如果当前不是OFFBOARD模式，且距离上次请求超过5秒，则再次尝试
            if self.current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
                if self.set_mode_client.call(offb_set_mode).mode_sent:
                    rospy.loginfo("OFFBOARD mode enabled")
                last_request = rospy.Time.now()
            else:
                # 如果未上锁，尝试上锁
                if not self.current_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
                    if self.arming_client.call(True).success:
                        rospy.loginfo("Vehicle armed")
                    last_request = rospy.Time.now()

            # 持续发布目标点
            self.local_pos_pub.publish(self.target_pose)
            self.rate.sleep()

if __name__ == '__main__':
    rospy.init_node('offboard_node', anonymous=True)
    controller = SimpleOffboard()
    controller.run()

运行这个脚本前，确保你的Gazebo仿真已经启动。然后在终端运行 python single_drone_hover.py。如果一切正常，你会看到无人机缓缓起飞并悬停在(0, 0, 2)的位置。这就是Offboard控制最基础的形式。

2.3 坐标系：ENU与FLU的抉择

在上面的代码中，我们直接设置了x=0, y=0, z=2。这里的坐标系是本地ENU（东北天）坐标系，原点通常是无人机上电或解锁时的位置。

• X轴： 指向东 (East)
• Y轴： 指向北 (North)
• Z轴： 指向天 (Up)，即垂直地面向上

这是ROS和PX4在仿真中默认使用的坐标系，非常直观。

但有时，我们更希望用机体坐标系（FLU：前-左-上）来发送指令。比如，你想让无人机“向前飞5米”，而不关心它当前机头指向哪个地理方向。这时就需要进行坐标转换。

坐标系	原点	X轴	Y轴	Z轴	适用场景
ENU (本地)	解锁点	东 (East)	北 (North)	天 (Up)	全局路径规划，基于地图的导航
FLU (机体)	无人机质心	前 (Forward)	左 (Left)	上 (Up)	基于机体的相对运动，如视觉避障

从FLU转换到ENU需要知道无人机当前的偏航角（Yaw）。公式并不复杂：

ENU_X = FLU_X * cos(yaw) - FLU_Y * sin(yaw) + 当前ENU_X
ENU_Y = FLU_X * sin(yaw) + FLU_Y * cos(yaw) + 当前ENU_Y
ENU_Z = FLU_Z + 当前ENU_Z

MAVROS的setpoint_raw/local话题支持直接指定坐标系。在mavros_msgs/PositionTarget消息中，coordinate_frame字段可以设置为8（FRAME_BODY_NED，类似FLU）或9（FRAME_LOCAL_NED，类似ENU）。这省去了我们手动计算的麻烦，是更推荐的做法。

3. 多机仿真启动与编队控制核心

单机控制只是热身，多机编队才是我们真正的目标。在Gazebo中启动多架无人机，并让它们各自拥有独立的MAVROS通信节点，是第一步。

3.1 启动一个多机仿真世界

PX4支持通过修改启动文件来生成多个无人机实例。每个实例会有独立的ROS命名空间，例如/uav0/, /uav1/，从而隔离它们的话题和服务。

最方便的方法是使用PX4提供的multi_uav_mavros_sitl.launch启动文件。在你的PX4固件目录下，可以这样启动一个包含3架无人机的仿真：

cd ~/px4_ws/src/PX4-Autopilot
source ~/px4_ws/devel/setup.bash
roslaunch px4 multi_uav_mavros_sitl.launch

这个启动文件会做几件事：

1. 启动3个PX4 SITL实例，每个绑定到不同的TCP端口。
2. 启动3个Gazebo模型，并放置在初始位置略有不同的地方。
3. 为每个实例启动一个MAVROS节点，话题前缀分别是/uav0/mavros/， /uav1/mavros/， /uav2/mavros/。

启动后，用rostopic list | grep mavros命令，你会看到大量重复但带有命名空间前缀的话题。

3.2 设计一个极简的编队控制类

现在，我们要编写一个Python类，能够同时管理多架无人机。核心思想是：为每架无人机创建一个控制对象，该对象封装了与该无人机通信的所有发布器、订阅器和服务客户端。

下面这个DroneController类，就是实现“5行代码编队”的关键。它高度封装了连接、模式切换、上锁和发送目标位置的过程：

#!/usr/bin/env python
import rospy
import threading
import numpy as np
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import CommandBool, SetMode

class DroneController:
    def __init__(self, namespace='/uav0'):
        """
        初始化针对单一命名空间（一架无人机）的控制器。
        :param namespace: 无人机的ROS命名空间，如 /uav0, /uav1
        """
        self.ns = namespace.rstrip('/')
        self.state = None
        self.rate = rospy.Rate(20)

        # 初始化发布器和订阅器
        self.local_pos_pub = rospy.Publisher(f'{self.ns}/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        self.state_sub = rospy.Subscriber(f'{self.ns}/mavros/state', State, self.state_cb)
        # 服务客户端
        self.arm_srv = rospy.ServiceProxy(f'{self.ns}/mavros/cmd/arming', CommandBool)
        self.set_mode_srv = rospy.ServiceProxy(f'{self.ns}/mavros/set_mode', SetMode)

        # 等待初始化
        rospy.loginfo(f"Waiting for connection to {self.ns}...")
        while not rospy.is_shutdown() and self.state is None:
            self.rate.sleep()
        while not rospy.is_shutdown() and not self.state.connected:
            self.rate.sleep()
        rospy.loginfo(f"{self.ns} connected!")

    def state_cb(self, msg):
        self.state = msg

    def takeoff(self, target_height=2.0):
        """让无人机起飞并悬停在指定高度"""
        target_pose = PoseStamped()
        target_pose.pose.position.z = target_height

        # 1. 持续发送设定点
        rospy.loginfo(f"{self.ns}: Sending setpoints...")
        for _ in range(100):
            self.local_pos_pub.publish(target_pose)
            self.rate.sleep()

        # 2. 切换到OFFBOARD模式
        rospy.loginfo(f"{self.ns}: Attempting to set OFFBOARD mode...")
        set_mode_req = SetMode()
        set_mode_req.request.custom_mode = 'OFFBOARD'
        for _ in range(10):  # 尝试多次
            if self.set_mode_srv.call(set_mode_req).mode_sent:
                rospy.loginfo(f"{self.ns}: OFFBOARD enabled")
                break
            self.rate.sleep()

        # 3. 上锁
        rospy.loginfo(f"{self.ns}: Arming...")
        arm_cmd = CommandBool()
        arm_cmd.request.value = True
        for _ in range(10):
            if self.arm_srv.call(arm_cmd).success:
                rospy.loginfo(f"{self.ns}: Armed")
                break
            self.rate.sleep()

        # 4. 持续悬停
        while not rospy.is_shutdown() and self.state.armed and self.state.mode == 'OFFBOARD':
            self.local_pos_pub.publish(target_pose)
            self.rate.sleep()

    def goto_position(self, x, y, z):
        """发送ENU坐标系下的目标位置"""
        target_pose = PoseStamped()
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        self.local_pos_pub.publish(target_pose)

这个类把复杂的连接和安全检查都隐藏在了初始化函数和takeoff方法里。现在，控制多架无人机变得异常简单。

4. 实战：5行代码实现三角形编队飞行

有了强大的DroneController类，我们就可以像指挥乐队一样控制无人机群了。假设我们已经通过multi_uav_mavros_sitl.launch启动了三架无人机，命名空间分别是/uav0, /uav1, /uav2。

4.1 编队脚本与可视化

创建一个名为triangle_formation.py的主脚本：

#!/usr/bin/env python
import rospy
import time
from drone_controller import DroneController  # 假设上面的类保存在这个模块

def main():
    rospy.init_node('formation_control_node')

    # 第1-3行：为三架无人机创建控制器
    drone0 = DroneController(namespace='/uav0')
    drone1 = DroneController(namespace='/uav1')
    drone2 = DroneController(namespace='/uav2')

    drones = [drone0, drone1, drone2]

    # 使用多线程让所有无人机同时起飞，节省时间
    threads = []
    for drone in drones:
        t = threading.Thread(target=drone.takeoff, kwargs={'target_height': 2.0})
        t.start()
        threads.append(t)
    for t in threads:
        t.join()

    rospy.loginfo("All drones armed and in OFFBOARD mode!")
    time.sleep(5)  # 稳定悬停一会儿

    # 第4行：定义三角形编队的三个顶点（相对位置）
    # 假设以(0,0,2)为中心，形成一个边长为3米的等边三角形
    formation_positions = [
        (0.0, 0.0, 2.0),      # 无人机0：中心点
        (1.5, -2.598, 2.0),   # 无人机1：右下顶点 (计算自等边三角形几何)
        (-1.5, -2.598, 2.0)   # 无人机2：左下顶点
    ]

    # 第5行：发送编队目标位置
    for i, drone in enumerate(drones):
        x, y, z = formation_positions[i]
        drone.goto_position(x, y, z)
        rospy.loginfo(f"Drone {i} -> ({x}, {y}, {z})")

    # 保持编队
    rospy.loginfo("Formation holding. Press Ctrl+C to exit.")
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 持续发布位置以维持Offboard模式
        for i, drone in enumerate(drones):
            x, y, z = formation_positions[i]
            drone.goto_position(x, y, z)
        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

看，核心的控制逻辑是不是非常清晰？创建控制器、起飞、指定目标位置。这就是“5行代码”精神的体现——将底层复杂性封装起来，暴露简洁的接口。

4.2 在RVIZ中可视化编队

在Gazebo里看无人机飞行固然直观，但对于算法调试，RVIZ是更轻量、更专业的选择。RVIZ可以清晰地显示每架无人机的坐标系、传感器数据（如激光雷达点云）和规划路径。

首先，为每架无人机启动一个RVIZ配置，或者使用一个能显示多个tf框架的RVIZ配置。更简单的方法是，在脚本中发布每架无人机的位置为tf变换，然后在同一个RVIZ中查看。

我们可以扩展DroneController类，添加一个tf发布器：

import tf2_ros
import tf_conversions
from geometry_msgs.msg import TransformStamped

class DroneControllerWithTF(DroneController):
    def __init__(self, namespace, drone_name):
        super().__init__(namespace)
        self.drone_name = drone_name
        self.tf_broadcaster = tf2_ros.TransformBroadcaster()
        self.local_pose_sub = rospy.Subscriber(f'{self.ns}/mavros/local_position/pose',
                                                PoseStamped,
                                                self.pose_cb)
    def pose_cb(self, msg):
        # 将接收到的位置信息发布为tf
        t = TransformStamped()
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "world"  # 父坐标系
        t.child_frame_id = self.drone_name  # 子坐标系，如 "uav0"
        t.transform.translation.x = msg.pose.position.x
        t.transform.translation.y = msg.pose.position.y
        t.transform.translation.z = msg.pose.position.z
        t.transform.rotation = msg.pose.orientation
        self.tf_broadcaster.sendTransform(t)

在RVIZ中，添加TF显示插件，你就能看到三个分别代表无人机的坐标系框架在空中移动，形成稳定的三角形。这对于调试编队队形的保持精度非常有帮助。

4.3 进阶：动态编队与避障思路

静态编队只是开始。在实际应用中，我们往往需要编队整体移动，或者根据环境动态调整队形。这需要引入一个“编队中心”或“领航者”的概念。

一种简单的实现方式是定义一个虚拟的领航点，其他无人机的位置都相对于这个领航点来计算。当领航点移动时，整个编队也随之移动。

class FormationController:
    def __init__(self, leader_namespace, follower_namespaces):
        self.leader = DroneController(leader_namespace)
        self.followers = [DroneController(ns) for ns in follower_namespaces]
        self.formation_offsets = [(-2, 0, 0), (0, 2, 0), (0, -2, 0)]  # 假设3架跟随者的偏移量

    def move_formation_to(self, target_x, target_y, target_z):
        """移动整个编队到目标点（领航者位置）"""
        # 1. 移动领航者
        self.leader.goto_position(target_x, target_y, target_z)

        # 2. 计算并发送跟随者位置
        for i, follower in enumerate(self.followers):
            offset_x, offset_y, offset_z = self.formation_offsets[i]
            follower.goto_position(target_x + offset_x,
                                   target_y + offset_y,
                                   target_z + offset_z)

对于避障，思路可以是在每架无人机上模拟一个“斥力”。当两架无人机之间的距离小于安全阈值时，为它们施加一个相互排斥的力，微调其目标位置。这可以在FormationController的循环中实现，形成一种分布式的避障策略。当然，更复杂的情况需要用到集中的路径规划算法，如APF（人工势场法）、VO（速度障碍法）等，但那是另一个话题了。

通过这篇文章，你应该已经掌握了用Python和ROS在Gazebo中控制PX4无人机编队的基础方法。从单机到多机，从静态悬停到动态编队，关键在于理解ROS的通信机制、PX4的Offboard模式安全逻辑，以及坐标系转换。剩下的，就是发挥你的想象力，去实现更酷的集群行为了。