从零玩转无人机仿真：用MAVROS在Gazebo里控制PX4无人机完成起飞、悬停与降落（Python代码示例）

从零玩转无人机仿真：用MAVROS在Gazebo里控制PX4无人机完成起飞、悬停与降落（Python代码示例）

当你第一次在Gazebo中看到PX4无人机模型成功加载时，那种兴奋感很快会被现实冲淡——面对密密麻麻的ROS话题和服务，究竟该如何用代码让这个虚拟的钢铁蜻蜓真正飞起来？本文将带你绕过commander takeoff这类终端命令的舒适区，直接切入Python编程控制的核心逻辑。

1. 环境准备与基础概念

在开始编写控制代码前，我们需要明确几个关键组件的关系链。PX4作为飞控软件提供底层飞行算法，Gazebo负责物理仿真环境渲染，而MAVROS则是连接ROS与PX4的通信桥梁。这种架构使得我们可以用ROS标准的消息和服务来控制无人机，而不必直接处理PX4的底层协议。

确保你的系统已经配置好以下环境：

• Ubuntu 20.04/22.04 LTS（推荐）
• ROS Noetic或ROS2 Humble
• PX4 v1.13+ 和 Gazebo Classic 11
• MAVROS软件包

验证环境是否正常工作：

roslaunch px4 mavros_posix_sitl.launch

这个命令会同时启动PX4仿真、Gazebo环境和MAVROS节点。如果看到Gazebo界面中出现一架iris无人机，且终端没有报错，说明基础环境已经就绪。

2. MAVROS通信机制解析

MAVROS将PX4的MAVLink协议转换成了ROS中的话题和服务，主要控制接口包括：

关键点理解：

• 所有控制指令都需要无人机处于OFFBOARD模式
• 必须先解锁电机（arming）才能执行飞行操作
• 位置指令需要持续发送（>2Hz），否则飞控会触发失控保护

3. 最小化控制代码实现

下面这个Python脚本展示了完整的控制流程，包含解锁、起飞、悬停和降落：

#!/usr/bin/env python3 import rospy from geometry_msgs.msg import PoseStamped from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool, SetMode class DroneController: def __init__(self): self.current_state = State() rospy.init_node('drone_control_node', anonymous=True) # 订阅无人机状态 self.state_sub = rospy.Subscriber( '/mavros/state', State, self.state_cb) # 发布位置指令 self.local_pos_pub = rospy.Publisher( '/mavros/setpoint_position/local', PoseStamped, queue_size=10) # 服务客户端 self.arming_client = rospy.ServiceProxy( '/mavros/cmd/arming', CommandBool) self.set_mode_client = rospy.ServiceProxy( '/mavros/set_mode', SetMode) def state_cb(self, data): self.current_state = data def takeoff(self, height): rate = rospy.Rate(20) # 20Hz控制频率 # 等待MAVROS连接 while not rospy.is_shutdown() and not self.current_state.connected: rate.sleep() # 先发送一些初始指令 for i in range(100): self.local_pos_pub.publish(self.create_pose(0, 0, height)) rate.sleep() # 设置OFFBOARD模式 if self.set_mode_client(0, 'OFFBOARD').mode_sent: rospy.loginfo("OFFBOARD模式已启用") # 解锁电机 if self.arming_client(True).success: rospy.loginfo("电机已解锁") # 起飞到指定高度 for i in range(100): self.local_pos_publish(self.create_pose(0, 0, height)) rate.sleep() rospy.loginfo(f"已到达目标高度 {height}米") def create_pose(self, x, y, z): pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z return pose if __name__ == '__main__': try: controller = DroneController() controller.takeoff(3) # 起飞到3米高度 rospy.spin() except rospy.ROSInterruptException: pass

注意：运行此脚本前，请确保已经通过roslaunch px4 mavros_posix_sitl.launch启动了仿真环境

4. 进阶控制技巧与调试方法

当基础飞行功能实现后，你可能需要以下进阶技巧：

常见问题排查清单：

1. 无人机不响应指令

   • 检查/mavros/state话题确认连接状态
   • 确认当前模式是否为OFFBOARD
   • 验证指令发布频率是否足够（>2Hz）

2. 无人机突然坠落

   • 检查Gazebo是否报物理引擎错误
   • 确认没有其他节点在发布冲突指令
   • 查看PX4日志确认失控保护原因

3. 位置控制精度差

   • 调整PX4参数MPC_XY_P和MPC_Z_P
   • 增加控制指令发布频率
   • 检查Gazebo实时因子（real-time factor）是否接近1

性能优化建议：

# 使用rospy.Time.now()而不是time.time()保证时间同步 pose.header.stamp = rospy.Time.now() # 重用PoseStamped对象减少内存分配 self.target_pose = PoseStamped() self.target_pose.header.frame_id = "map" def update_pose(self, x, y, z): self.target_pose.header.stamp = rospy.Time.now() self.target_pose.pose.position.x = x self.target_pose.pose.position.y = y self.target_pose.pose.position.z = z return self.target_pose

5. 从仿真到实机的关键调整

虽然本文基于仿真环境，但这些代码稍作修改即可用于真实无人机。主要差异点包括：

• 硬件接口配置

  roslaunch mavros px4.launch fcu_url:="udp://:14540@192.168.1.1:14557"

• 安全考虑增加的检查项

  def safety_check(self): if self.current_state.mode != "OFFBOARD": rospy.logwarn("非OFFBOARD模式！") return False if not self.current_state.armed: rospy.logwarn("电机未解锁！") return False return True

• 增加RC遥控器覆盖检测

  if self.current_state.system_status == 4: # MAV_STATE_CRITICAL rospy.logerr("紧急状态！") self.emergency_land()

6. 扩展应用：构建自动化任务链

掌握了基础控制后，可以尝试组合多个动作为复杂任务。例如实现自动起飞-巡航-降落流程：

def mission_sequence(self): waypoints = [ (0, 0, 3), # 起飞点 (5, 0, 3), # 第一个航点 (5, 5, 3), # 第二个航点 (0, 0, 3) # 返回起点 ] for wp in waypoints: start_time = rospy.Time.now() while (rospy.Time.now() - start_time) < rospy.Duration(5): # 每个点停留5秒 self.local_pos_pub.publish(self.create_pose(*wp)) self.rate.sleep() self.land() # 自动降落

配合ROS的actionlib可以实现更复杂的任务管理，比如中断当前任务执行紧急降落，或者动态添加新的航点。