保姆级教程：用MAVROS在ROS Noetic下控制PX4无人机（从话题订阅到飞控通信）

保姆级教程：用MAVROS在ROS Noetic下控制PX4无人机（从话题订阅到飞控通信）

当你第一次尝试用ROS控制无人机时，可能会被各种陌生的术语和复杂的通信流程搞得晕头转向。MAVROS作为连接ROS和PX4飞控的桥梁，是每个无人机开发者必须掌握的利器。本文将带你从零开始，一步步实现用Python节点通过MAVROS控制无人机的基础流程。

1. 环境准备与连接检查

在开始编写控制代码前，确保你的硬件和软件环境已经正确配置。你需要：

• 一台运行Ubuntu 20.04的计算机（推荐配置）
• 已安装ROS Noetic完整版
• PX4固件烧录到飞控硬件（如Pixhawk 4）
• 机载计算机与飞控通过USB或数传电台连接

首先检查MAVROS与飞控的连接状态：

# 启动MAVROS节点 roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

连接成功后，你应该能看到终端输出类似以下信息：

[ INFO] [1620000000.000000]: FCU URL: udp://:14540@127.0.0.1:14557 [ INFO] [1620000000.100000]: Connected to FCU

验证连接状态的几种方法：

1. 查看/mavros/state话题：

   rostopic echo /mavros/state

   确认connected字段为True

2. 检查心跳包：

   rostopic hz /mavros/state

   正常情况应该能看到稳定的1Hz频率

注意：如果连接失败，首先检查飞控是否正确上电，USB连接是否可靠，以及fcu_url参数是否与你的实际连接方式匹配。

2. 关键ROS话题解析

MAVROS会发布和订阅多个ROS话题，理解这些话题的含义对开发控制程序至关重要。以下是几个最常用的核心话题：

让我们深入看看/mavros/state话题的具体内容：

Header header bool connected bool armed bool guided string mode uint8 system_status

• connected: MAVROS与飞控的连接状态
• armed: 无人机是否解锁（电机是否运转）
• mode: 当前飞行模式（如"POSCTL"位置控制模式）
• system_status: 飞控系统状态代码

3. 编写基础控制节点

现在我们来创建一个简单的Python节点，通过发布位置指令控制无人机。以下是完整的代码示例：

#!/usr/bin/env python import rospy from geometry_msgs.msg import PoseStamped from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool, SetMode class DroneControl: def __init__(self): 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) self.current_state = State() self.rate = rospy.Rate(20) # 20Hz发布频率 def state_cb(self, state): self.current_state = state def set_mode(self, mode): try: self.set_mode_client(custom_mode=mode) return True except rospy.ServiceException as e: rospy.logerr("设置模式失败: %s" % e) return False def arm(self, arm): try: self.arming_client(value=arm) return True except rospy.ServiceException as e: rospy.logerr("解锁/锁定失败: %s" % e) return False def run(self): # 等待MAVROS与飞控建立连接 while not rospy.is_shutdown() and not self.current_state.connected: self.rate.sleep() # 创建初始位置指令 pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 2 # 目标高度2米 # 先发送一些指令，避免飞控拒绝切换模式 for i in range(100): self.local_pos_pub.publish(pose) self.rate.sleep() # 设置飞行模式为OFFBOARD if self.set_mode("OFFBOARD"): rospy.loginfo("OFFBOARD模式设置成功") else: rospy.logerr("无法设置OFFBOARD模式") return # 解锁无人机 if self.arm(True): rospy.loginfo("无人机已解锁") else: rospy.logerr("解锁失败") return # 主控制循环 while not rospy.is_shutdown(): self.local_pos_pub.publish(pose) self.rate.sleep() if __name__ == '__main__': try: controller = DroneControl() controller.run() except rospy.ROSInterruptException: pass

4. 常见问题排查

即使按照教程一步步操作，你仍可能遇到各种问题。以下是几个常见问题及其解决方案：

1. MAVROS无法连接飞控

   • 检查飞控是否正确上电
   • 确认fcu_url参数正确
   • 尝试不同的连接方式（USB/数传）

2. 无法切换到OFFBOARD模式

   • 确保在切换模式前已经持续发送了足够多的setpoint消息
   • 检查飞控参数COM_RCL_EXCEPT是否设置为禁用遥控器覆盖

3. 无人机不响应位置指令

   • 确认飞行模式确实是OFFBOARD
   • 检查/mavros/setpoint_position/local话题是否有数据发布
   • 查看飞控状态灯是否显示为位置控制模式

4. 位置控制不稳定

   • 调整PX4的位置控制参数（如MPC_XY_P、MPC_Z_P）
   • 确保机载计算机的计算资源充足
   • 检查传感器数据（特别是高度计）是否准确

提示：在正式飞行前，务必在仿真环境中充分测试你的代码。推荐使用Gazebo配合PX4 SITL进行仿真测试。

5. 进阶控制技巧

掌握了基础控制后，你可以尝试更复杂的控制策略：

1. 轨迹跟踪

   def generate_circle_trajectory(radius=3, height=2, speed=0.5): waypoints = [] for angle in range(0, 360, 10): rad = math.radians(angle) pose = PoseStamped() pose.pose.position.x = radius * math.cos(rad) pose.pose.position.y = radius * math.sin(rad) pose.pose.position.z = height waypoints.append(pose) return waypoints

2. 速度控制

   from geometry_msgs.msg import Twist vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', Twist, queue_size=10) def send_velocity_command(vx, vy, vz): cmd_vel = Twist() cmd_vel.linear.x = vx cmd_vel.linear.y = vy cmd_vel.linear.z = vz vel_pub.publish(cmd_vel)

3. 混合位置和速度控制

   • 在远距离使用位置控制接近目标
   • 在近距离切换为速度控制进行精细调整
   • 结合两者实现平滑的接近和悬停

6. 性能优化建议

随着控制逻辑变得复杂，你需要考虑代码的性能和可靠性：

1. 消息发布频率

   • 位置控制建议20-30Hz
   • 速度控制建议50Hz以上
   • 姿态控制可能需要100Hz或更高

2. 多线程处理

   import threading class StateMonitor(threading.Thread): def __init__(self): super(StateMonitor, self).__init__() self.current_state = None self.subscriber = rospy.Subscriber('/mavros/state', State, self.callback) def callback(self, data): self.current_state = data def run(self): rospy.spin()

3. 异常处理

   • 添加对连接丢失的处理
   • 实现安全机制（如自动返航）
   • 记录关键数据用于事后分析

在实际项目中，我发现最常遇到的问题不是控制算法本身，而是通信延迟和状态同步。确保你的控制循环能够及时获取最新的传感器数据和飞控状态，这对实现稳定控制至关重要。