Ardupilot与Gazebo仿真中无人机解锁后无法起飞的深度排查与解决方案

1. 问题现象与初步排查：为什么我的无人机“赖在地上”？

如果你也像我一样，在Ardupilot和Gazebo搭建的仿真世界里，满怀期待地输入解锁指令，看着无人机的桨叶呼呼转起来，心里正激动呢，结果它却像个耍赖的孩子，死活不肯离地，只是在原地不停地“解锁-上锁-解锁”，那你来对地方了。我刚开始用Ardupilot做仿真时，这个问题足足困扰了我一个周末，网上搜到的教程十有八九都是针对PX4的，直接套用过来根本不好使。那种感觉就像拿到了钥匙，却怎么也打不开自家的门，非常 frustrating。

首先，我们得把问题搞清楚。你看到的“能解锁但不起飞”，具体表现通常是：通过MAVROS发送解锁（Arming）命令后，QGroundControl或者你的ROS节点会反馈“ARMED”成功，Gazebo里的无人机模型桨叶开始高速旋转，声音也起来了，但飞机就是稳稳地扎在地上。更诡异的是，过一会儿它可能自己又上锁（Disarm）了，然后循环往复。这时候你可能会怀疑人生：我的代码是照着官方例子写的啊，Gazebo模型也没报错，到底哪儿出问题了？

这里最大的一个思维误区，就是把PX4的经验直接套用到Ardupilot上。原始文章里提到的那个经典代码，确实是PX4官网的Offboard示例，它在PX4生态里运行良好。但Ardupilot的飞行逻辑和MAVLink消息处理方式与PX4有关键区别。最核心的一点在于：在Ardupilot中，仅仅向mavros/setpoint_position/local发布目标位置，并不足以触发起飞动作。对于Ardupilot来说，解锁（Arming）只是让电机进入怠速（IDLE）状态，获得了动力，但还没有收到明确的“起飞”指令。它需要一个明确的“起飞”（Takeoff）命令，或者切换到特定的飞行模式（如GUIDED）并配合一个向上的目标，才会真正执行离地动作。

所以，我们的排查思路就要从“为什么位置控制不起作用”，转变为“如何给Ardupilot发送正确的起飞指令”。这涉及到对Ardupilot飞行模式、MAVROS服务以及消息流的深入理解。别担心，下面我会带你一步步拆解，从环境检查到代码修改，把每个坑都填平。

2. 理解核心：Ardupilot与PX4在Offboard模式下的关键差异

要解决问题，不能光靠试错，得先明白背后的道理。Ardupilot和PX4虽然都是优秀的开源飞控，但在设计哲学和具体实现上各有侧重，这就导致了在仿真中行为的不同。

首先，飞行模式（Flight Mode）的语义不同。在PX4的Offboard模式中，一旦解锁，飞控会立刻尝试执行来自外部计算机（你的ROS节点）的第一条指令，比如你发布的那个z=2的位置设定点（Setpoint）。PX4会将其视为起飞指令并执行爬升。而Ardupilot的GUIDED模式（相当于PX4的Offboard）逻辑更“谨慎”一些。在Ardupilot中，解锁后飞控会等待一个明确的起飞指令。这个指令可以是一个MAV_CMD_NAV_TAKEOFF命令，也可以是一个比当前高度高得多的位置设定点（但这个高度差需要足够大，且需要配合正确的流程）。

其次，MAVROS服务/话题的兼容性与行为。MAVROS是一个桥梁，它把ROS的消息和服务转换成MAVLink协议消息发给飞控。虽然PX4和Ardupilot都支持MAVLink，但它们对同一条MAVLink消息的解释和响应可能不同。例如，对于SET_POSITION_TARGET_LOCAL_NED这条消息（对应mavros/setpoint_position/local话题），Ardupilot可能要求飞控必须处于“已起飞”状态，或者需要先执行一个起飞动作，才会乖乖地跟随这个位置目标。否则，它只会“看看”，不执行。

最后，是“起飞”状态的判定。Ardupilot内部有一个状态机，其中“起飞”（Takeoff）是一个明确的状态。只有当飞控进入这个状态，它才会积极地去追踪位置或速度指令。而触发这个状态，就需要我们前面说的那个明确的起飞命令。

为了更直观，我画了一个简单的对比表格：

理解了这个差异，我们就知道方向了：不能只做解锁和设模式，必须在解锁后，立即通过MAVROS调用Ardupilot的起飞服务，让飞控正式进入起飞爬升阶段。之后，我们发布的位置指令才会被有效执行。这就是解决“赖地不起”问题的钥匙。

3. 环境与配置检查：排除基础陷阱

在动手改代码之前，我们必须确保仿真环境本身是健康的。很多不起飞的问题，根源其实是环境配置不对，或者基础通信就没建立好。我建议你按照以下清单过一遍，这些坑我都踩过。

第一，Gazebo模型与Ardupilot的适配。你用的无人机SDF或URDF模型是否正确配置了Ardupilot的插件？例如，在启动Gazebo时，我们通常会用这样的命令：

roslaunch ardupilot_gazebo iris_ardupilot.launch

这里的iris_ardupilot.launch文件，就确保了Gazebo中的Iris模型加载的是与Ardupilot通信的插件（libgazebo_motor_model.so等），而不是PX4的插件。如果用了PX4的模型启动文件，通信协议对不上，自然无法正确控制。请确认你的启动文件来自ardupilot_gazebo包，而不是px4或mavlink_sitl_gazebo包。

第二，SITL（软件在环）与MAVROS的连接。这是最容易出问题的一环。标准的流程是：

1. 在一个终端启动SITL仿真：sim_vehicle.py -v ArduCopter -f gazebo-iris --console。看到终端输出“Ready to FLY”等信息。
2. 在另一个终端启动Gazebo和模型：roslaunch ardupilot_gazebo iris_ardupilot.launch。
3. 在第三个终端启动MAVROS，连接SITL：roslaunch mavros apm.launch fcu_url:="udp://:14540@127.0.0.1:14557"。这里的端口号14557是SITL默认的TCP输出端口，14540是MAVROS的监听端口，务必匹配。

如何检查连接是否成功？运行rostopic echo /mavros/state，查看输出。关键字段是connected是否为true，以及mode是否显示为诸如“GUIDED”、“STABILIZE”等。如果connected一直是false，那说明MAVROS根本没连上飞控，后面的一切操作都是徒劳。这时你需要检查fcu_url参数是否正确，以及SITL和Gazebo是否都正常启动了。

第三，参数配置。有些Ardupilot参数会影响起飞行为。虽然新手很少动这里，但检查一下没坏处。你可以通过QGroundControl连接上SITL（UDP连接，地址127.0.0.1，端口14550），查看几个关键参数：

• DISARM_DELAY：解锁后自动上锁的延迟时间。如果这个值设得太小，飞机可能还没等你发起飞指令就自己锁上了。默认值通常是0（禁用）或10秒，但最好确认一下。
• FS_THR_ENABLE：油门失控保护。在仿真中如果油门通道设置异常，可能触发保护导致无法起飞。仿真环境下可以考虑暂时禁用（设为0）以排除干扰。
• 确保ARMING_CHECK相关的参数没有阻止解锁。不过既然你已经能解锁，说明这一关已经过了。

完成以上检查，确保Gazebo模型能动、MAVROS显示已连接、飞控模式可切换，我们就为代码调试打下了坚实的基础。接下来，就是重头戏——修改我们的控制代码。

4. 解决方案：修改ROS节点代码，实现正确起飞

现在，我们基于原始文章中的代码，来构建一个真正能在Ardupilot上工作的版本。我会逐段解释为什么这么改，并提供完整的、可运行的代码示例。请准备好你的ROS工作空间。

第一步：必不可少的头文件与初始化我们的代码需要与MAVROS交互，所以相关的ROS头文件和消息类型必须包含。注意，我们这里引入了mavros_msgs::CommandTOL，这是“起飞着陆”（Takeoff and Landing）命令的服务类型，正是我们需要的起飞指令。

#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/Twist.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/CommandTOL.h> // 关键！用于起飞服务 #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } int main(int argc, char **argv) { ros::init(argc, argv, "ardupilot_offboard_node"); ros::NodeHandle nh; // ... 订阅和服务声明 }

第二步：等待连接与设置模式这部分和之前类似，确保MAVROS与飞控（SITL）建立了连接。连接成功后，我们首先将模式设置为GUIDED。在Ardupilot中，GUIDED模式允许接收来自地面站或外部计算机的高级指令（如起飞、飞向某点）。

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10); // 声明模式设置和解锁服务客户端 ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode"); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming"); // 关键！声明起飞服务客户端 ros::ServiceClient takeoff_client = nh.serviceClient<mavros_msgs::CommandTOL>("mavros/cmd/takeoff"); ros::Rate rate(20.0); // 等待飞控连接 while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } ROS_INFO("FCU Connected"); // 设置GUIDED模式 mavros_msgs::SetMode guided_set_mode; guided_set_mode.request.custom_mode = "GUIDED"; if(set_mode_client.call(guided_set_mode) && guided_set_mode.response.mode_sent){ ROS_INFO("GUIDED mode enabled"); } else { ROS_ERROR("Failed to set GUIDED mode"); return -1; }

第三步：解锁与发送起飞命令这是最关键的修改点。解锁之后，不要立即开始发布位置设定点。而是先调用起飞服务。

// 解锁 mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; if(arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); } else { ROS_ERROR("Arming failed"); return -1; } // 重要：给飞控一点时间稳定，并非必须，但是个好习惯 ros::Duration(2.0).sleep(); // 发送起飞命令 mavros_msgs::CommandTOL takeoff_cmd; takeoff_cmd.request.altitude = 5; // 目标高度，单位：米 takeoff_cmd.request.latitude = 0; // 仿真中通常忽略，但需要提供 takeoff_cmd.request.longitude = 0; // 仿真中通常忽略，但需要提供 takeoff_cmd.request.min_pitch = 0; // 最小俯仰角，保持0即可 takeoff_cmd.request.yaw = 0; // 起飞时的偏航角 if(takeoff_client.call(takeoff_cmd) && takeoff_cmd.response.success){ ROS_INFO("Takeoff command sent!"); } else { ROS_ERROR("Failed to execute takeoff"); return -1; }

注意takeoff_cmd.request.altitude是相对于起飞点的目标高度。飞控收到这个命令后，会自主控制无人机爬升到指定高度并悬停。

第四步：等待起飞完成，再开始位置控制起飞命令是异步的，飞控需要时间执行。如果你在发送起飞命令后，立刻开始轰炸式地发布位置设定点，可能会干扰飞控的起飞逻辑。一个简单有效的办法是等待一段时间。

// 关键：等待起飞完成。时间取决于起飞高度，5米高大概需要3-5秒。 ROS_INFO("Waiting for takeoff to complete..."); ros::Duration(8.0).sleep(); // 等待8秒，确保已达到目标高度并稳定

这8秒的等待至关重要。在这期间，飞控正在专心执行起飞动作。你可以通过rostopic echo /mavros/global_position/rel_alt来观察相对高度的变化，确认飞机已经离地。

第五步：开始发布位置设定点现在，飞机已经悬停在5米高了。此时，我们再开始向mavros/setpoint_position/local发布目标位置，飞控就会乖乖地跟随着移动了。

ROS_INFO("Starting position control..."); geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 5; // 保持和起飞高度一致，或更高 ros::Time last_request = ros::Time::now(); while(ros::ok()){ // 示例：让飞机做一个正方形的航线 // 这里可以替换成任何你需要的轨迹生成算法 double time = (ros::Time::now() - last_request).toSec(); pose.pose.position.x = 2.0 * sin(time * 0.5); pose.pose.position.y = 2.0 * cos(time * 0.5); local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0;

至此，一个完整的、能在Ardupilot仿真中让无人机解锁、起飞并执行航点的ROS节点就完成了。核心就是加入了“起飞服务调用”和“起飞过程等待”这两个环节。

5. 进阶：实现复杂动作与避坑指南

解决了基本起飞问题，我们就可以玩点花样了，比如让无人机画个圆，或者实现更复杂的轨迹跟踪。这里我分享一个让无人机做“水平圆周运动”的进阶例子，并附上几个我踩过的坑，帮你节省时间。

进阶示例：水平圆周运动+高度变化在起飞并悬停稳定后，我们可以用一段更精彩的代码替换上面第五步的简单正方形航线。这个例子结合了位置设定点和速度设定点，让运动更平滑。

// 在开始循环前，定义一些变量 geometry_msgs::Twist vel_cmd; // 速度控制消息 ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 10); double circle_radius = 3.0; // 圆半径，米 double angular_speed = 0.5; // 角速度，弧度/秒 ros::Time start_time = ros::Time::now(); while(ros::ok()){ double t = (ros::Time::now() - start_time).toSec(); // 计算期望的圆周轨迹位置 pose.pose.position.x = circle_radius * cos(angular_speed * t); pose.pose.position.y = circle_radius * sin(angular_speed * t); // Z轴可以保持不变，也可以做正弦运动 pose.pose.position.z = 5.0 + 1.0 * sin(t * 0.3); // 在5米基准上上下1米波动 // 计算对应的速度指令（位置控制的导数，用于前馈，使控制更精准） vel_cmd.linear.x = -circle_radius * angular_speed * sin(angular_speed * t); vel_cmd.linear.y = circle_radius * angular_speed * cos(angular_speed * t); vel_cmd.linear.z = 1.0 * 0.3 * cos(t * 0.3); // Z轴速度 // 同时发布位置和速度指令（Ardupilot的位置控制器可以融合速度前馈） local_pos_pub.publish(pose); local_vel_pub.publish(vel_cmd); ros::spinOnce(); rate.sleep(); }

实战避坑指南：

1. 起飞高度与位置控制高度的衔接：你的起飞命令设置了altitude=5，那么后续位置控制的pose.pose.position.z最好不要低于5，或者至少要从5开始缓慢变化。如果直接设为2，飞控可能会试图俯冲下降，行为可能不符合预期。稳妥的做法是，初始位置控制的Z值等于或略高于起飞高度。

2. 坐标系理解：mavros/setpoint_position/local使用的是飞控的局部NED坐标系（北-东-地）。在Gazebo仿真中，通常世界坐标系是ENU（东-北-天）。MAVROS内部会进行转换。但你需要知道的是，对于Ardupilot，position.z的正值是向下的（因为NED是地坐标系）。而我们在起飞命令和直觉中，高度都是向上的。这里存在一个正负号的转换。在大多数通过MAVROS发送的指令中（如起飞高度），正值为向上；在位置设定点消息中，Z的正值为向下。这是一个非常关键的细节！所以，如果你想让飞机在起飞后继续爬升，位置设定点的z值应该设置为负数（例如z = -7表示在起飞点上方7米）。

3. “解锁-上锁”循环问题：如果严格按照上述流程，起飞后应该不会再出现这个问题。如果还出现，请返回去检查DISARM_DELAY参数，并确保你的ROS节点在起飞后持续运行，没有崩溃。飞控如果一段时间（取决于参数）没有收到任何外部指令，可能会自动上锁。

4. Gazebo模型卡住或抖动：有时无人机在Gazebo中会轻微抖动甚至卡在地面。这可能是物理引擎参数或模型关节阻尼设置问题。可以尝试在Gazebo模型SDF文件中调整<mu1>、<mu2>（摩擦系数）和<kp>、<kd>（关节PID参数），或者直接换一个已知能正常工作的模型（如ardupilot_gazebo包中的iris或typhoon_h480）。

5. 使用QGC辅助调试：强烈建议在运行仿真时，同时打开QGroundControl。它能给你一个直观的飞行状态视图，包括高度、模式、解锁状态等。如果代码发送了起飞指令但QGC里高度没变化，那问题可能出在通信或指令本身；如果高度变化了但Gazebo里模型没动，那问题可能出在Gazebo模型或物理引擎。