引言
无人机自主飞行是机器人领域的热门方向，PX4 作为开源飞控配合 ROS 的灵活性，成为实现高级自主飞行的组合。初学者常对 PX4 的飞行模式理解不清，难以编写可靠的 Offboard 控制程序。
本文将解析 PX4 核心飞行模式，实现无人机的自动起飞、悬停、轨迹跟踪（圆形/方形/螺旋）与降落。
第一部分：PX4 飞行模式深度剖析
PX4 的飞行模式可以看作一个控制权逐级递增的层级结构。理解这些模式是编写控制程序的前提。
1. 稳定模式（STABILIZED / MANUAL / ACRO）
- 核心特点：纯手动姿态控制。飞控仅保证机体自身姿态稳定，但不进行位置或高度锁定。
- 操控方式：摇杆控制俯仰/横滚角；油门直接控制升力。松开摇杆，飞机会回正到水平，但会随风漂移。
- 应用场景：起飞、降落、特技飞行、紧急手动接管。
2. 定高模式（ALTCTL）
- 核心特点：高度保持 + 姿态稳定。飞控将油门通道重映射为爬升率指令。
- 操控方式：俯仰/横滚摇杆控制姿态和水平速度；油门摇杆中位时维持高度，上下推动控制爬升/下降速率。
- 应用场景：低空侦察、无 GPS 或光流情况下的高度稳定飞行。
3. 位置模式（POSCTL）
- 核心特点：全自主位置与高度锁定。飞控使用 GPS/视觉定位将飞机稳定在固定的三维坐标点。
- 操控方式：俯仰/横滚摇杆控制水平速度；油门摇杆控制垂直速率。松开摇杆，飞机立即刹车并悬停。
- 应用场景：最常用、最安全的辅助模式，是进入 Offboard 模式前的理想过渡。
4. 自动模式（AUTO）
包括任务模式 (MISSION)、返航模式 (RTL)、降落模式 (LAND)、起飞模式 (TAKEOFF) 等。由飞控内部逻辑执行预设任务，无需外部持续指令。
5. Offboard 模式
- 核心特点：外部控制模式。飞控的位置/姿态/速度期望值完全由通过 MAVLink 接收的外部指令设定。这是 ROS/PX4 集成的核心接口。
- 工作方式：外部计算机（如机载电脑）以>10Hz 频率持续发送设定点指令。一旦指令流中断，飞控触发失效保护，自动切换到预设安全模式（如 RTL 或 LAND）。
- 应用场景：SLAM 建图、视觉伺服、集群编队、复杂轨迹跟踪等所有需要高级算法的场景。
第二部分：ROS 可控制的模式分析
通过 ROS（通常借助 MAVROS）控制 PX4，主要有两种方式：模式切换和直接指令发送。
1. 可切换的模式
MAVROS 理论上可以切换到任何已配置的飞行模式，最常用于：
OFFBOARD：最关键的模式，切换到此模式才能发送流式控制指令。POSCTL：作为 Offboard 前的准备，或安全回退模式。AUTO.MISSION/AUTO.RTL/AUTO.LAND：启动自动任务、触发返航或降落。
服务调用示例：
rosservice call /mavros/set_mode "base_mode: 0 custom_mode: 'OFFBOARD'"
2. 可发送实时控制指令的模式
| 模式 | 是否接受 ROS 流式控制 | 控制方式 |
|---|---|---|
OFFBOARD | 是 | 发送 setpoint_position/setpoint_velocity 等指令 |
POSCTL/ALTCTL | 否（可模拟 RC） | 通过 mavros/rc/override 模拟遥控器输入，不推荐 |
| 其他 | 否 | 仅能通过服务切换模式，不能实时控制 |
结论： 实现 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）
生成圆形轨迹：
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) {
geometry_msgs::PoseStamped pose;
double angle = 2.0 * M_PI * i / points;
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 模式切换与解锁：
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 中显示路径。
第九部分：安全注意事项
- 真机飞行前务必在 SITL 中充分测试。
- 设置 PX4 失效保护参数：
COM_RCL_EXCEPT：允许在遥控器丢失时仍保持 Offboard（谨慎）。COM_OF_LOSS_T：Offboard 指令超时后切换到什么模式（建议设为 21 或 20，即降落或返航）。
- 确保 GPS 信号良好（真机），或使用视觉定位。
- 保持紧急切换能力：遥控器设置为 Position 模式，以便随时接管。
- 代码中增加心跳机制：持续发送指令，并监控
mavros/state中的armed和mode，异常时自动降落。
第十部分：总结与扩展
本文详细解析了 PX4 飞行模式，并提供了一个完整的 ROS Offboard 控制实现。在此基础上可扩展更多轨迹（如八字形、自定义路径点）、添加传感器融合（结合 VIO 或激光雷达实现室内导航）、实现集群控制或多机协同、集成视觉算法（目标跟踪、避障）。
参考文献：
- PX4 用户指南
- MAVROS 文档
- ROS Wiki
