PX4 与 ROS 集成实战：Offboard 模式解析与轨迹控制

服务调用示例：

rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"

2. 可发送实时控制指令的模式

结论： 实现 ROS 程序控制的唯一标准方法是使用Offboard 模式。

第三部分：项目概述

我们将创建一个名为 px4_offboard_control 的 ROS 功能包，实现以下功能：

• 自动起飞：上升至指定高度并悬停。
• 悬停等待：稳定数秒后开始轨迹飞行。
• 轨迹跟踪：支持圆形、方形、螺旋三种预设轨迹。
• 速度/位置控制可选：通过参数切换控制模式。
• 状态机管理：安全处理各阶段转换。
• 失效保护：与 PX4 的 Offboard 超时保护无缝配合。

技术栈：ROS Melodic/Noetic、C++14、MAVROS、PX4 SITL、Eigen、TF2。

第四部分：代码结构设计

整体架构

px4_offboard_control/
├── CMakeLists.txt
├── package.xml
├── launch/
│   └── offboard_control.launch # 主启动文件
├── config/
│   └── params.yaml # 参数配置文件
├── include/
│   └── px4_offboard_control/
│       └── trajectory_generator.h # 轨迹生成器头文件
└── src/
    ├── offboard_control.cpp # 主控制节点（含状态机）
    └── trajectory_generator.cpp # 轨迹生成器实现

核心类设计

TrajectoryGenerator：负责生成各种轨迹点，并提供从当前位置到目标点的速度指令计算（P 控制器）。

OffboardControl：主控制类，包含：

• ROS 通信（订阅状态/位置，发布设定点/轨迹）
• 状态机（初始化、解锁、起飞、悬停、轨迹跟踪、降落、锁定）
• 参数加载
• 模式切换/解锁服务调用
• 主循环控制逻辑

状态机流转图

INIT -> ARMING -> TAKEOFF -> HOVER -> FOLLOW_TRAJECTORY -> LAND -> DISARM -> 结束

每个状态都有超时或条件判断，确保安全转换。

第五部分：核心代码详解

1. 轨迹生成器（trajectory_generator.cpp）

生成圆形轨迹： 这里我们利用三角函数生成圆周上的离散点。注意 yaw 角的计算，让无人机始终朝向圆心或切线方向，这取决于你的具体需求。

std::vector<geometry_msgs::PoseStamped> generateCircleTrajectory(
    const geometry_msgs::PoseStamped& start_pose,
    double radius, double height, int points, int num_loops) {
    std::vector<geometry_msgs::PoseStamped> traj;
    for (int loop = 0; loop < num_loops; ++loop) {
        for (int i = 0; i < points; ++i) {
            double angle = 2.0 * M_PI * i / points;
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = start_pose.pose.position.x + radius * cos(angle);
            pose.pose.position.y = start_pose.pose.position.y + radius * sin(angle);
            pose.pose.position.z = height;
            
            // 使无人机始终指向圆心（可选）
            double yaw = atan2(-sin(angle), -cos(angle));
            pose.pose.orientation = getQuaternionFromYaw(yaw);
            traj.push_back(pose);
        }
    }
    return traj;
}

速度指令计算：使用简单的 P 控制器，将位置误差转换为速度指令，并限制最大速度，防止电机过载。

2. 主控制节点（offboard_control.cpp）

状态机处理示例（起飞状态）： 在起飞阶段，我们需要持续发布位置设定点，直到达到目标高度。这里要注意容差设置，避免因为微小抖动导致状态反复跳变。

void handleTakeoffState(ros::Rate& rate) {
    geometry_msgs::PoseStamped target_pose;
    target_pose.pose.position.z = home_pose_.pose.position.z + takeoff_height_;
    
    if (use_position_control_) {
        local_pos_pub_.publish(target_pose);
        if (fabs(current_pose_.pose.position.z - target_pose.pose.position.z) < pos_tolerance_) {
            ROS_INFO("Reached takeoff altitude");
            control_state_ = HOVER;
        }
    } else if (use_velocity_control_) {
        auto vel_cmd = trajectory_gen_.calculateVelocityCommand(current_pose_, target_pose, ...);
        local_vel_pub_.publish(vel_cmd);
        if (vel_cmd.twist.linear.z == 0) {
            control_state_ = HOVER;
        }
    }
}

Offboard 模式切换与解锁： 这两个服务调用是初始化的关键。务必确保先解锁（Arm），再切换模式，否则飞控会拒绝指令。

bool setMode(std::string mode) {
    mavros_msgs::SetMode srv;
    srv.request.custom_mode = mode;
    return set_mode_client_.call(srv) && srv.response.mode_sent;
}

bool arm(bool arm) {
    mavros_msgs::CommandBool srv;
    srv.request.value = arm;
    return arming_client_.call(srv) && srv.response.success;
}

主循环：以 20Hz 运行，根据当前状态调用对应的处理函数，并持续发送设定点以维持 Offboard 模式。心跳丢失会导致飞控自动接管。

第六部分：配置与参数说明

参数文件（params.yaml）

# 基本参数
takeoff_height: 2.0
hover_duration: 5.0
pos_tolerance: 0.1

# 控制模式（只能选一个）
use_position_control: true # 位置控制模式
use_velocity_control: false # 速度控制模式

# 轨迹类型（circle / square / spiral）
trajectory_type: "circle"

# 速度限制
max_linear_vel: 1.0
max_angular_vel: 0.5

# 可视化
publish_trajectory: true

Launch 文件（offboard_control.launch）

启动 MAVROS 和 Offboard 控制节点，可传参：

<arg name="trajectory_type" default="circle"/>
<arg name="takeoff_height" default="2.0"/>
...
<node name="offboard_control" pkg="px4_offboard_control" type="offboard_control" output="screen">
    <param name="trajectory_type" value="$(arg trajectory_type)"/>
    ...
</node>

第七部分：编译与运行

环境要求

• Ubuntu 18.04/20.04
• ROS Melodic/Noetic
• PX4-Autopilot（用于 SITL 仿真）
• MAVROS

编译步骤

cd ~/catkin_ws
catkin_make
source devel/setup.bash

运行仿真

roslaunch px4_offboard_control offboard_control.launch trajectory_type:=circle

将看到无人机自动起飞、悬停、执行圆形轨迹，最后降落。

第八部分：测试与结果

测试场景：

• 圆形轨迹（半径 2m，高度 3m）
• 方形轨迹（边长 4m）
• 螺旋轨迹（最大半径 3m，最大高度 5m）

观察指标：

• 轨迹跟踪精度（<0.2m 误差）
• 状态转换平滑性
• Offboard 模式保持（无超时断开）

可视化：程序会发布 /planned_trajectory 话题，可在 RViz 中显示路径。

第九部分：安全注意事项

1. 真机飞行前务必在 SITL 中充分测试。
2. 设置 PX4 失效保护参数：
   • COM_RCL_EXCEPT：允许在遥控器丢失时仍保持 Offboard（谨慎）。
   • COM_OF_LOSS_T：Offboard 指令超时后切换到什么模式（建议设为 21 或 20，即降落或返航）。
3. 确保 GPS 信号良好（真机），或使用视觉定位。
4. 保持紧急切换能力：遥控器设置为 Position 模式，以便随时接管。
5. 代码中增加心跳机制：持续发送指令，并监控 mavros/state 中的 armed 和 mode，异常时自动降落。

第十部分：总结与扩展

本文详细解析了 PX4 飞行模式，并提供了一个完整的 ROS Offboard 控制实现。你可以在此基础上：

• 扩展更多轨迹：如八字形、自定义路径点。
• 添加传感器融合：结合 VIO 或激光雷达实现室内导航。
• 实现集群控制：多机协同。
• 集成视觉算法：目标跟踪、避障。

参考文献：

• PX4 用户指南
• MAVROS 文档
• ROS Wiki