从仿真到实机：用XTDrone+ROS Noetic给PX4无人机写个自动巡航脚本-程序员充电站

从仿真到实机：用XTDrone+ROS Noetic给PX4无人机写个自动巡航脚本

无人机自动巡航是智能飞行系统的核心功能之一。想象一下，当你需要让无人机完成从A点到B点的自主飞行，或者执行复杂的巡检任务时，一套可靠的自动巡航系统能大幅提升工作效率。本文将带你从零开始，基于XTDrone仿真环境和PX4飞控，使用ROS Noetic开发一个实用的自动巡航脚本。

1. 环境准备与验证

在开始编写代码前，确保你的开发环境已经正确配置。这里假设你已经完成了以下基础环境的搭建：

Ubuntu 20.04 LTS操作系统
ROS Noetic完整安装
PX4 v1.13飞控软件
XTDrone仿真环境

验证环境是否正常工作：

# 启动PX4 SITL仿真 cd ~/PX4-Autopilot make px4_sitl_default gazebo # 新开终端，启动MAVROS roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

如果看到Gazebo界面正常加载无人机模型，且MAVROS连接状态显示为connected: True，说明基础环境配置正确。

2. ROS节点设计与实现

我们将创建一个简单的ROS节点，负责处理航点信息和发送飞行指令。首先建立工作空间和功能包：

mkdir -p ~/uav_ws/src cd ~/uav_ws/src catkin_create_pkg waypoint_nav roscpp mavros geometry_msgs

在src目录下创建waypoint_nav.cpp文件，实现核心功能：

#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.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, "waypoint_nav_node"); ros::NodeHandle nh; // 订阅MAVROS状态 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); // 等待MAVROS连接 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()){ 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; }

修改CMakeLists.txt文件添加可执行目标：

add_executable(waypoint_nav_node src/waypoint_nav.cpp) target_link_libraries(waypoint_nav_node ${catkin_LIBRARIES})

3. 航点飞行逻辑实现

基础框架完成后，我们需要扩展航点飞行功能。在原有代码基础上添加航点队列管理：

#include <vector> #include <cmath> // 定义航点结构体 struct Waypoint { float x; float y; float z; float acceptance_radius; // 到达判定半径 }; // 航点列表 std::vector<Waypoint> waypoints = { {0, 0, 2, 0.5}, // 起飞点 {5, 0, 2, 0.5}, // 第一个航点 {5, 5, 2, 0.5}, // 第二个航点 {0, 5, 2, 0.5}, // 第三个航点 {0, 0, 2, 0.5} // 返回原点 }; // 检查是否到达当前航点 bool checkWaypointReached(const geometry_msgs::Point& current_pos, const Waypoint& wp){ float dx = current_pos.x - wp.x; float dy = current_pos.y - wp.y; float dz = current_pos.z - wp.z; float distance = sqrt(dx*dx + dy*dy + dz*dz); return distance < wp.acceptance_radius; }

然后在主循环中添加航点切换逻辑：

// 在main函数中添加 ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped> ("mavros/local_position/pose", 10, [&](const geometry_msgs::PoseStamped::ConstPtr& msg){ static size_t current_wp = 0; if(current_wp < waypoints.size()){ const auto& wp = waypoints[current_wp]; if(checkWaypointReached(msg->pose.position, wp)){ current_wp++; if(current_wp < waypoints.size()){ ROS_INFO("Moving to waypoint %zu", current_wp); pose.pose.position.x = waypoints[current_wp].x; pose.pose.position.y = waypoints[current_wp].y; pose.pose.position.z = waypoints[current_wp].z; } else { ROS_INFO("Mission complete!"); } } } });

4. 仿真测试与调试技巧

编译并运行节点：

cd ~/uav_ws catkin_make source devel/setup.bash rosrun waypoint_nav waypoint_nav_node

测试过程中可能会遇到以下常见问题及解决方案：

无人机不响应指令
- 检查MAVROS连接状态：rostopic echo /mavros/state
- 确保飞控模式已切换为OFFBOARD
- 验证电机是否已解锁
无人机飞行不稳定
- 调整PID参数：rosrun rqt_reconfigure rqt_reconfigure
- 降低飞行速度：逐步增加航点间距
Gazebo仿真卡顿
- 降低图形质量设置
- 关闭不必要的插件

实用的调试命令：

# 查看MAVROS状态 rostopic echo /mavros/state # 查看当前位置 rostopic echo /mavros/local_position/pose # 手动发送指令 rostopic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped ...

5. 从仿真到实机部署

当仿真测试通过后，可以将代码部署到真实无人机。需要注意以下关键点：

硬件配置检查
- 确保飞控固件版本与仿真环境一致
- 校准所有传感器（加速度计、陀螺仪、磁力计等）
- 测试遥控器故障保护功能
安全措施
- 首次测试选择开阔无人的场地
- 设置合理的飞行高度限制
- 准备紧急停止方案
参数调整
- 根据实际飞行性能调整PID参数
- 修改航点接受半径以适应定位精度
- 测试不同电池状态下的飞行表现

实机测试建议流程：

在仿真环境中完整测试所有功能
进行系留测试（无人机固定，验证控制指令）
低空短距离试飞
逐步增加飞行距离和复杂度

#!/usr/bin/env python # 简单的航点监控脚本 import rospy from mavros_msgs.msg import State from geometry_msgs.msg import PoseStamped class Monitor: def __init__(self): self.current_state = State() rospy.Subscriber("/mavros/state", State, self.state_cb) rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_cb) def state_cb(self, data): self.current_state = data def pose_cb(self, data): rospy.loginfo(f"Position: x={data.pose.position.x:.2f}, y={data.pose.position.y:.2f}, z={data.pose.position.z:.2f}") rospy.loginfo(f"Mode: {self.current_state.mode}, Armed: {self.current_state.armed}") if __name__ == "__main__": rospy.init_node("flight_monitor") monitor = Monitor() rospy.spin()

6. 高级功能扩展

基础航点飞行实现后，可以考虑添加以下高级功能：

动态航点更新
- 通过服务或话题接收新航点
- 实时调整飞行路线

// 添加动态航点服务 #include <waypoint_nav/AddWaypoint.h> ros::ServiceServer add_wp_service = nh.advertiseService( "add_waypoint", [&](waypoint_nav::AddWaypoint::Request& req, waypoint_nav::AddWaypoint::Response& res){ Waypoint new_wp; new_wp.x = req.x; new_wp.y = req.y; new_wp.z = req.z; new_wp.acceptance_radius = req.radius; waypoints.push_back(new_wp); res.success = true; return true; });

避障功能集成
- 订阅激光雷达或深度相机数据
- 检测障碍物并调整航点
任务中断与恢复
- 处理紧急情况下的任务暂停
- 记录当前状态以便恢复
地面站交互
- 实现与QGroundControl的深度集成
- 上传/下载任务计划

性能优化建议：

使用动作服务器(Action Server)管理航点任务
实现航点预加载和缓存机制
优化消息发布频率，减少不必要的计算

# 航点可视化工具 import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D def plot_waypoints(waypoints): fig = plt.figure() ax = fig.add_subplot(111, projection='3d') x = [wp.x for wp in waypoints] y = [wp.y for wp in waypoints] z = [wp.z for wp in waypoints] ax.plot(x, y, z, 'b-o') ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') plt.title('Flight Path Visualization') plt.show()