PX4飞控与MAVROS实战：从零搭建无人机ROS控制节点（附完整代码）

当我在实验室第一次成功通过ROS节点控制无人机完成自主飞行时，那种成就感至今难忘。PX4飞控与MAVROS的组合为无人机开发者提供了前所未有的灵活性和控制精度，让复杂的飞控算法开发变得触手可及。本文将带你深入这个激动人心的领域，从硬件连接到代码实现，一步步构建完整的无人机控制解决方案。

1. 环境准备与硬件连接

在开始编写代码之前，我们需要确保硬件和软件环境正确配置。我建议使用Ubuntu 20.04或22.04系统，并安装ROS Noetic或ROS2 Foxy版本。以下是详细的配置步骤：

1.1 安装MAVROS

MAVROS是连接ROS与PX4飞控的桥梁，可以通过以下命令安装：

sudo apt-get install ros-noetic-mavros ros-noetic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
chmod +x install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh

常见问题排查：

• 如果遇到依赖问题，尝试运行rosdep install --from-paths src --ignore-src -y
• 确保你的ROS环境已正确配置（通过source /opt/ros/noetic/setup.bash）

1.2 硬件连接配置

PX4飞控通常通过USB或串口与计算机连接。连接后需要检查设备权限：

ls /dev/ttyACM*
sudo usermod -a -G dialout $USER
sudo chmod a+rw /dev/ttyACM0

提示：每次重新插拔飞控后，设备名称可能会变化，建议使用udev规则固定设备名称

创建/etc/udev/rules.d/99-px4.rules文件，内容如下：

SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", MODE="0666", GROUP="dialout"

2. MAVROS通信机制深度解析

MAVROS的核心功能是将MAVLink协议转换为ROS话题和服务。理解这一转换机制对开发高级控制算法至关重要。

2.1 坐标系转换原理

PX4飞控使用NED（北东地）坐标系，而ROS标准是ENU（东北天）坐标系。MAVROS会自动处理这种转换，但开发者必须清楚背后的数学原理：

坐标系类型	PX4(飞控)	MAVROS(ROS)	转换关系
位置X	北(N)	东(E)	x_ros = y_px4
位置Y	东(E)	北(N)	y_ros = x_px4
位置Z	地(D)	天(U)	z_ros = -z_px4

四元数姿态的转换公式为：

q_ros = [qx_px4, -qy_px4, -qz_px4, qw_px4]

2.2 关键话题与服务

MAVROS提供了丰富的话题和服务接口，以下是开发中最常用的几个：

核心状态话题：

• /mavros/state - 连接状态、解锁状态、当前模式
• /mavros/battery - 电池状态信息
• /mavros/imu/data - IMU原始数据

位置控制话题：

• /mavros/setpoint_position/local - 发布本地位置设定点
• /mavros/local_position/pose - 订阅当前位置反馈

重要服务：

• /mavros/cmd/arming - 解锁/锁定无人机
• /mavros/set_mode - 设置飞行模式（如OFFBOARD）

3. Offboard模式控制实战

Offboard模式允许外部计算机通过MAVROS完全控制无人机。这是实现自主飞行的关键步骤。

3.1 基础控制节点实现

创建一个完整的Offboard控制节点需要以下几个关键组件：

#include <ros/ros.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <geometry_msgs/PoseStamped.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, "offboard_control");
    ros::NodeHandle nh;
    
    // 订阅状态
    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 arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    // 设置发布频率
    ros::Rate rate(20.0);

    // 等待连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
    
    // 初始化位置设定点
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;
    
    // 发送一些初始设定点
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    
    // 设置Offboard模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    
    // 解锁指令
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    
    ros::Time last_request = ros::Time::now();
    
    while(ros::ok()){
        // 尝试切换到Offboard模式
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            // 尝试解锁
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
        
        // 持续发布设定点
        local_pos_pub.publish(pose);
        
        ros::spinOnce();
        rate.sleep();
    }
    
    return 0;
}

3.2 安全注意事项

在实际飞行中，Offboard模式需要特别注意安全：

1. 遥控器备用：始终准备好可以随时接管控制权的遥控器
2. 心跳机制：确保以足够高的频率（>2Hz）持续发送设定点
3. 超时处理：实现逻辑检测通信中断并自动切换回稳定模式
4. 地理围栏：在代码中设置合理的飞行边界

警告：在实飞前，务必在仿真环境中充分测试所有代码。推荐使用Gazebo与PX4 SITL进行仿真测试。

4. 高级控制技巧与性能优化

掌握了基础控制后，我们可以进一步优化系统性能和实现更复杂的功能。

4.1 轨迹跟踪实现

实现平滑轨迹跟踪需要处理好几个关键点：

import numpy as np
from geometry_msgs.msg import PoseStamped

class TrajectoryGenerator:
    def __init__(self):
        self.waypoints = [
            [0, 0, 2],
            [5, 0, 2],
            [5, 5, 2],
            [0, 5, 2],
            [0, 0, 2]
        ]
        self.current_wp = 0
        self.wp_threshold = 0.3
        self.max_speed = 1.0
        
    def get_next_pose(self, current_pose):
        target = self.waypoints[self.current_wp]
        dx = target[0] - current_pose.pose.position.x
        dy = target[1] - current_pose.pose.position.y
        dz = target[2] - current_pose.pose.position.z
        distance = np.sqrt(dx*dx + dy*dy + dz*dz)
        
        if distance < self.wp_threshold:
            self.current_wp = (self.current_wp + 1) % len(self.waypoints)
            target = self.waypoints[self.current_wp]
            dx = target[0] - current_pose.pose.position.x
            dy = target[1] - current_pose.pose.position.y
            dz = target[2] - current_pose.pose.position.z
            distance = np.sqrt(dx*dx + dy*dy + dz*dz)
        
        # 归一化方向向量
        if distance > 0:
            dx /= distance
            dy /= distance
            dz /= distance
        
        # 计算下一位置（简单线性插值）
        next_pose = PoseStamped()
        next_pose.pose.position.x = current_pose.pose.position.x + dx * self.max_speed * 0.05
        next_pose.pose.position.y = current_pose.pose.position.y + dy * self.max_speed * 0.05
        next_pose.pose.position.z = current_pose.pose.position.z + dz * self.max_speed * 0.05
        
        return next_pose

4.2 性能优化技巧

1. 多线程处理：将状态监控、控制算法和通信分离到不同线程
2. 消息频率优化：关键控制话题保持20-50Hz，非关键数据降低频率
3. 数据缓存：对传感器数据实现简单的滤波算法
4. QoS配置：合理设置ROS2的QoS策略（如果使用ROS2）

典型性能指标对比：

优化措施	平均延迟(ms)	CPU占用率(%)	备注
基础实现	12.5	45	单线程
多线程	8.2	32	控制与状态分离
频率优化	6.7	28	非关键数据降频
全优化	5.1	25	综合优化

5. 常见问题解决方案

在实际开发中，我们经常会遇到各种问题。以下是几个典型问题及其解决方法：

5.1 连接问题排查

症状：MAVROS无法连接PX4飞控

排查步骤：

1. 检查物理连接和端口权限
2. 确认波特率设置一致（通常为921600或57600）
3. 检查飞控固件版本与MAVROS兼容性
4. 查看roslaunch mavros px4.launch的输出日志

5.2 坐标系混乱问题

当出现位置控制异常时，很可能是坐标系理解错误导致。记住这些关键点：

• PX4内部使用NED坐标系
• MAVROS默认发布ENU坐标系数据
• 机体系(FLU)前(X)指向无人机前方

5.3 Offboard模式退出问题

如果无人机频繁退出Offboard模式，检查：

1. 设定点发布频率是否足够（>2Hz）
2. 遥控器是否设置了模式切换保护
3. 飞控参数COM_RCL_EXCEPT是否配置正确

6. 扩展应用：视觉辅助控制

结合视觉信息可以大幅提升无人机自主能力。以下是简单的视觉伺服实现框架：

import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class VisualServoing:
    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        self.target_pos_pub = rospy.Publisher('/target_position', PoseStamped, queue_size=10)
        
    def image_callback(self, msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            
            # 简单的颜色阈值检测（示例）
            hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
            mask = cv2.inRange(hsv, (30, 50, 50), (90, 255, 255))
            
            # 计算目标中心
            M = cv2.moments(mask)
            if M["m00"] > 0:
                cX = int(M["m10"] / M["m00"])
                cY = int(M["m01"] / M["m00"])
                
                # 发布目标位置（简单映射）
                target = PoseStamped()
                target.pose.position.x = (cX - 320) * 0.01  # 假设的映射关系
                target.pose.position.y = (cY - 240) * 0.01
                self.target_pos_pub.publish(target)
                
        except Exception as e:
            rospy.logerr("Error processing image: %s"%e)

7. 仿真与实飞测试建议

在将代码部署到真实无人机前，完善的测试流程至关重要：

1. Gazebo仿真测试：

   make px4_sitl_default gazebo
   roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
   

2. 硬件在环(HITL)测试：

   • 连接真实飞控但保持电机禁用
   • 通过QGroundControl监控飞控状态

3. 系留测试：

   • 将无人机固定在测试台上
   • 验证控制响应而不实际起飞

4. 户外实飞：

   • 选择开阔无干扰环境
   • 逐步增加飞行高度和复杂度

记得在每次代码修改后，至少要在仿真环境中验证基本功能。我在项目中曾因为跳过仿真测试直接实飞，导致无人机出现意外行为，这个教训让我深刻理解了仿真测试的重要性。