无人机多点巡航

公爵爱学习2026-09-07 10:37

1.建立一个world里面添加 无人机常用的测试门文件

核心代码：

bash 复制代码 
     <!-- ==================== 4 个穿越门（开口2.4m） ==================== -->

    <!-- 门1：红色，位置(4,0,2.5)，朝向90° -->
    <model name='gate_red'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>4 0 2.5 0 0 1.5708</pose>
    </model>

    <!-- 门2：绿色，位置(0,4,2.8)，朝向180° -->
    <model name='gate_green'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 1 0 1</ambient><diffuse>0 1 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>0 4 2.8 0 0 3.1416</pose>
    </model>

    <!-- 门3：蓝色，位置(-4,0,3.0)，朝向-90° -->
    <model name='gate_blue'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>0 0 1 1</ambient><diffuse>0 0 1 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>-4 0 3.0 0 0 -1.5708</pose>
    </model>

    <!-- 门4：橙色，位置(0,-4,2.6)，朝向0° -->
    <model name='gate_orange'>
      <static>1</static>
      <link name='post_left'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>-1.2 0 0 0 0 0</pose>
      </link>
      <link name='post_right'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>1.2 0 0 0 0 0</pose>
      </link>
      <link name='top_bar'>
        <visual name='visual'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
          <material><ambient>1 0.5 0 1</ambient><diffuse>1 0.5 0 1</diffuse></material>
        </visual>
        <collision name='collision'>
          <geometry><cylinder><radius>0.05</radius><length>2.4</length></cylinder></geometry>
        </collision>
        <pose>0 0 1.2 0 1.5708 0</pose>
      </link>
      <pose>0 -4 2.6 0 0 0</pose>
    </model>

    <!-- ==================== 门结束 ==================== -->

四个门东南西北

第二步：多点巡航的程序,核心代码如下，完整代码联系我

python 复制代码 
   mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::SetMode land_set_mode;
    land_set_mode.request.custom_mode = "AUTO.LAND";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    int wp_index = 0;
    ros::Time last_wp = ros::Time::now();
    bool cruise_started = false;      // 是否已开始巡航
    bool land_sent = false;           // 是否已发降落指令

    while(ros::ok()){
        // ① 切 offboard 模式
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(2.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(2.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }

第三步：订阅RVIZ话题的程序

python 复制代码 
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    if(pts.empty() || dist(msg->pose.position, pts.back().pose.position) > 0.1)
    {
        pts.push_back(*msg);
        cols.push_back(current_color);   // 记录该点对应的门颜色

        visualization_msgs::Marker m;
        m.header.frame_id = "map";
        m.header.stamp = ros::Time::now();
        m.ns = "gate_traj";
        m.id = 0;
        m.type = visualization_msgs::Marker::LINE_STRIP;
        m.action = visualization_msgs::Marker::ADD;
        m.scale.x = 0.12;                // 粗线

        for(size_t i = 0; i < pts.size(); i++){
            m.points.push_back(pts[i].pose.position);
            m.colors.push_back(cols[i]);
        }
        marker_pub.publish(m);

最后一步：在Cmake.list里面添加 编译规则

终端1无人机 终端2订阅RVIZ 终端3.打开RVIZ，这里要选择Marker话题选对 终端4.发布巡航程序

当然也可以进行rosbag记录速度与位置信息

然后如上一篇操作，进行转化成csv格式，把下面代码放入matlab中

python 复制代码 
%% ============ 图1：3D轨迹 + 门 ============
figure('Color','w','Position',[80 80 650 560]);
plot3(x, y, z, '-', 'Color', [0 0.45 0.74], 'LineWidth', lw); hold on;
for i = 1:4
    plot3(gx(i), gy(i), gz(i), 's', 'MarkerSize', 15, ...
        'MarkerFaceColor', gc(i,:), 'MarkerEdgeColor','k','LineWidth',1.2);
end
hold off;
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
legend([{'Trajectory'},{'Gate1','Gate2','Gate3','Gate4'}], ...
    'Location','best','Box','off');
grid on; box on; axis equal; view(40,25);
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig1_3d_traj.pdf','ContentType','vector');

%% ============ 图2：位置-时间 ============
figure('Color','w','Position',[80 680 650 400]);
plot(tp,x,'-','Color',[0.85 0.33 0.10],'LineWidth',lw); hold on;
plot(tp,y,'-','Color',[0.47 0.67 0.19],'LineWidth',lw);
plot(tp,z,'-','Color',[0.00 0.45 0.74],'LineWidth',lw); hold off;
xlabel('Time (s)'); ylabel('Position (m)');
legend({'x','y','z'},'Location','best','Box','off','NumColumns',3);
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig2_position.pdf','ContentType','vector');

%% ============ 图3：速度-时间（平滑）============
figure('Color','w','Position',[780 80 650 400]);
plot(tv,vx_s,'-','Color',[0.85 0.33 0.10],'LineWidth',lw); hold on;
plot(tv,vy_s,'-','Color',[0.47 0.67 0.19],'LineWidth',lw);
plot(tv,vz_s,'-','Color',[0.00 0.45 0.74],'LineWidth',lw); hold off;
xlabel('Time (s)'); ylabel('Velocity (m/s)');
legend({'v_x','v_y','v_z'},'Location','best','Box','off','NumColumns',3);
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig3_velocity.pdf','ContentType','vector');

%% ============ 图4：总速度 ============
figure('Color','w','Position',[780 560 650 400]);
plot(tv, speed_s, '-', 'Color', [0.20 0.20 0.20], 'LineWidth', lw);
xlabel('Time (s)'); ylabel('Speed |v| (m/s)');
grid on; box on;
set(gca,'FontSize',fs,'TickDir','out');
exportgraphics(gcf,'fig4_speed.pdf','ContentType','vector');
相关推荐
公爵爱学习2 小时前
关于无人机定点飞行的一些解释和Matlab程序绘制
游戏引擎·无人机·cocos2d
梵亿2 小时前
基于 STM32F103C8T6 的无人机低功耗设计与实现
stm32·嵌入式硬件·无人机·低功耗
YOLO数据集集合3 小时前
工地开放危险区域检测数据集 | 工地安全 开放区域检测 临边防护9050期
安全·无人机·工地·工地安全·工地检测·危险区域监测
YOLO数据集集合15 小时前
无人机高分辨率多树种单木分割数据集 | 单木分割 无人机林业 树种识别 实例分割 点云 遥感数据集 深度学习 精准林业9047期
人工智能·深度学习·数据集·无人机·遥感数据集·点云数据集·树木分割
wgslucky16 小时前
Godot GDScript初学者遇到的问题记录
游戏引擎·godot
点心的游戏开发世界18 小时前
GDScript 入门笔记（五）：函数
笔记·游戏引擎·godot
沈阳昊天环宇无人机小编辑19 小时前
沈阳无人机考证科普|CAAC执照是什么？考试内容详解
无人机
谢斯21 小时前
[unity]如何在unity中添加newtonsoft-json
unity·json·游戏引擎
公爵爱学习1 天前
无人机定点飞行-介绍
开发语言·图像处理·python·学习·线性回归·无人机
热门推荐
01国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）02GitHub 镜像站点03历年考研数学一、数学二、数学三真题试卷及答案PDF04蜘蛛磁力 搜索引擎大全，如何使用蜘蛛磁力查找磁力链接05ZCode 周末送额度活动开启：3 亿 Token 免费领取062026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？07微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新08Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费09AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路10VSCode+CodeX扩展，无法启动