【PX4SimulinkGazebo联合仿真】在Simulink中使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化

2024-02-21 17:36
文章标签 使用 自定义 联合 可视化 仿真 simulink 轨迹 控制 圆形 ros2 无人机 gazebo 飞行 px4simulinkgazebo

本文主要是介绍【PX4SimulinkGazebo联合仿真】在Simulink中使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

在Simulink中使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化

    • 系统架构
    • Matlab官方例程Control a Simulated UAV Using ROS 2 and PX4 Bridge
    • 运行所需的环境配置
    • PX4&Simulink&Gazebo联合仿真实现方法
      • 建立Simulink模型并完成基本配置
      • 整体框架
      • 各子系统实现原理
        • Arm子系统
        • Enable Offboard Control子系统
        • Takeoff子系统
        • Trajectory Flight子系统
    • 实现效果

本篇文章介绍如何使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化，提供了Matlab/Simulink源代码，以及演示效果图。

环境：

MATLAB : R2022b

Ubuntu ：20.04 LTS

Windows ：Windows 10

ROS ：ROS2 Foxy

Python: 3.8.2

Visual Studio ：Visual Studio 2019

PX4 ：1.13.0

系统架构

ROS2的应用程序管道非常简单，这要归功于本地通信中间件（DDS/RTPS）。microRTPS桥接工具由运行在PX4上的客户端和运行在计算机上的服务端组成，它们进行通信以提供uORB和ROS2话题格式之间的双向数据交换和话题转换。使得可以创建直接与PX4的uORB话题接口的ROS2订阅服务器或发布服务器节点，其结构如下图所示。

在这里插入图片描述

ROS 2使用px4_msgs包和px4_ROS_com包来确保使用匹配的话题定义来创建客户端和服务端代码。

px4_msgs包：px4 ROS话题定义，当构建该项目时会生成相应的兼容ROS2节点的话题类型，以及IDL文件，由fastddsgen用于生成microRTPS代码。

px4_ros_com包：服务端发布者和订阅者的microRTPS代码模板，构建过程运行一个fastddsgen实例来生成micrortps_agent的代码，该代码可编译为单个可执行文件。

这样在Ubuntu中就生成了一个可以调用uORB话题接口的ROS2节点，这个节点可以和运行在同一局域网下的Matlab/Simulink上的ROS2节点进行通信，以实现PX4&Simulink&Gazebo联合仿真。

20240221092830.png" width="800">

Matlab官方例程Control a Simulated UAV Using ROS 2 and PX4 Bridge

Matlab官方给出了一个示例，该示例演示了如何从具有PX4自动驾驶仪的模拟无人机接收传感器读数和自动驾驶仪状态，并发送控制命令来导航模拟无人机，可以作为参考。

Control a Simulated UAV Using ROS 2 and PX4 Bridge

可以在Matlab命令行中输入以下命令打开该例程所在位置。

openExample('uav_ros/ControlASimulatedUAVUsingROS2AndPX4BridgeExample')

运行所需的环境配置

请确保已经安装前一篇文章配置好了PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink联合调试环境。

【PX4-AutoPilot教程-开发环境】搭建PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink联合调试环境

PX4&Simulink&Gazebo联合仿真实现方法

建立Simulink模型并完成基本配置

在Matlab工作文件夹中models文件夹中新建一个Simulink模型，我这里命名为TrajectoryFlight.slx，双击使用Simulink打开。

在这里插入图片描述

在【建模】栏打开【模型设置】，【求解器】栏中【求解器类型】选为【定步长】。

20240123211557.png" width="600">

【硬件实现】栏中【Hardware board】选择【ROS2】。

在这里插入图片描述

【代码生成】栏中【接口】勾选【连续时间】。

在这里插入图片描述

仿真调速界面勾选【启用调速以减慢仿真】。

在这里插入图片描述

整体框架

整体框架如下，主体是对时钟进行判断，1-3秒是触发Arm子系统，3-5秒是触发Enable Offboard Control子系统，5-7秒是触发Takeoff子系统，7秒后是触发Trajectory Flight子系统。

在这里插入图片描述

各子系统实现原理

Arm子系统

Arm子系统中使用ROS2 Subscribe模块订阅/fmu/timesync/out话题，并使用Bus Selector分解话题获取时间戳，将时间戳传入Subsystem子系统。

在这里插入图片描述

无人机的解锁是通过vehicle_command话题进行的，它的定义在源码Firmware/msg/vehicle_command.msg中，这个话题是地面站/nsh等终端发送的控制指令用的。

我们可以从任意已经编译过的固件中的Firmware\build\px4_fmu-v5_default\uORB\topics\vehicle_command.h文件中看到vehicle_command话题的结构体定义。

	uint64_t timestamp;double param5;double param6;float param1;float param2;float param3;float param4;float param7;uint32_t command;uint8_t target_system;uint8_t target_component;uint8_t source_system;uint8_t source_component;uint8_t confirmation;bool from_external;uint8_t _padding0[2]; // required for logger

可以看到其结构为：

时间戳+command命令+目标系统号+目标组件号+发出命令系统号+发出命令组件号+收到命令次数+数据包

在源码Firmware/msg/vehicle_command.msg中可以检索到解锁的命令ID是：

uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|

可以在注释中看到用法，只需将param1的值赋值为1即可解锁。

综上，通过ROS2对无人机进行解锁的方法为：

订阅/fmu/timesync/out获得时间戳–>command设置为400、param1设置为1、target_system设置为1–>发布/fmu/vehicle_command/in话题。

Subsystem子系统中使用ROS2 Blank Message获得px4_msgs/vehicle_command的话题类型，导入获取到的时间戳、命令编号、传入参数等，并使用ROS2 Publish模块发布该话题。

在这里插入图片描述

Enable Offboard Control子系统

Enable Offboard Control子系统中使用ROS2 Subscribe模块订阅/fmu/timesync/out话题，并使用Bus Selector分解话题获取时间戳，将时间戳传入Subsystem子系统。

在这里插入图片描述

无人机进入Offboard模式也是通过vehicle_command话题进行的。

在源码Firmware/msg/vehicle_command.msg中可以检索到设置系统模式的命令ID是：

uint16 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|

这里的注释写的是将第一个参数param1设为模式的ID号，之后param2到param7设置为空，但是这里的注释好像写错了。

在源码Firmware/src/modules/commander/Commander.cpp中，官方写的调节模式的命令是：

send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD);

send_vehicle_command()函数的定义为：

static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{vehicle_command_s vcmd{};vcmd.command = cmd;vcmd.param1 = param1;vcmd.param2 = param2;vcmd.param3 = param3;vcmd.param4 = param4;vcmd.param5 = param5;vcmd.param6 = param6;vcmd.param7 = param7;uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};vcmd.source_system = vehicle_status_sub.get().system_id;vcmd.target_system = vehicle_status_sub.get().system_id;vcmd.source_component = vehicle_status_sub.get().component_id;vcmd.target_component = vehicle_status_sub.get().component_id;uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};vcmd.timestamp = hrt_absolute_time();return vcmd_pub.publish(vcmd);
}

可以看出需要将param1赋值为1，将param2赋值为PX4_CUSTOM_MAIN_MODE_OFFBOARD才能切换为Offboard模式。

查询PX4_CUSTOM_MAIN_MODE_OFFBOARD的定义，在源码Firmware/src/modules/commander/px4_custom_mode.h中找到：

enum PX4_CUSTOM_MAIN_MODE {PX4_CUSTOM_MAIN_MODE_MANUAL = 1,PX4_CUSTOM_MAIN_MODE_ALTCTL,PX4_CUSTOM_MAIN_MODE_POSCTL,PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_MAIN_MODE_ACRO,PX4_CUSTOM_MAIN_MODE_OFFBOARD,PX4_CUSTOM_MAIN_MODE_STABILIZED,PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};

PX4_CUSTOM_MAIN_MODE_OFFBOARD对应的数字是6。

综上，通过ROS2对无人机进入Offboard模式的方法为：

订阅/fmu/timesync/out获得时间戳–>command设置为176、param1设置为1、param2设置为6、target_system设置为1–>发布/fmu/vehicle_command/in话题。

Subsystem子系统中使用ROS2 Blank Message获得px4_msgs/vehicle_command的话题类型，导入获取到的时间戳、命令编号、传入参数等，并使用ROS2 Publish模块发布该话题。

在这里插入图片描述

Takeoff子系统

Takeoff子系统中使用ROS2 Subscribe模块订阅/fmu/timesync/out话题，并使用Bus Selector分解话题获取时间戳，将时间戳传入SendCommand子系统。

在这里插入图片描述

offboard_control_mode话题是Offboard模式的心跳包，为了保证飞行的安全性，心跳包必须以最低2Hz的频率发布，PX4在两个Offboard命令之间有一个500ms的延时，如果超过此延时，系统会将回到无人机进入Offboard模式之前的最后一个模式。

在源码Firmware/msg/offboard_control_mode.msg中可以看到offboard_control_mode话题的定义。

# Off-board control modeuint64 timestamp		# time since system start (microseconds)bool position
bool velocity
bool acceleration
bool attitude
bool body_rate
bool actuator

因为要进行位置控制所以需要将position赋值为true。

trajectory_setpoint话题是期望的位置，在源码Firmware/msg/vehicle_local_position_setpoint.msg中可以看到trajectory_setpoint话题的定义。

# Local position setpoint in NED frame
# setting something to NaN means the state should not be controlleduint64 timestamp	# time since system start (microseconds)float32 x		# in meters NED
float32 y		# in meters NED
float32 z		# in meters NED
float32 yaw		# in radians NED -PI..+PI
float32 yawspeed	# in radians/sec
float32 vx		# in meters/sec
float32 vy		# in meters/sec
float32 vz		# in meters/sec
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust	# normalized thrust vector in NED# TOPICS vehicle_local_position_setpoint trajectory_setpoint

其中trajectory_setpoint话题和vehicle_local_position_setpoint话题的内容是一样的，源码Firmware/msg/tools/urtps_bridge_topics.yaml中可以看到以下代码。

  - msg:     vehicle_local_position_setpointreceive: true- msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpointbase:    vehicle_local_position_setpointreceive: true

可以看出trajectory_setpoint话题是基于vehicle_local_position_setpoint话题的。

这里需要注意坐标系是NED坐标系，即北东地坐标系，所以想让无人机飞起来，z的赋值应该为负数。

综上，通过ROS2对无人机进入Offboard模式起飞悬停的方法为：

订阅/fmu/timesync/out获得时间戳–>position设置为true、x设置为0、y设置为0、z设置为-5、target_system设置为1–>发布offboard_control_mode话题和trajectory_setpoint话题。

SendCommand子系统中使用ROS2 Blank Message获得offboard_control_mode的话题类型和trajectory_setpoint的话题类型，导入获取到的时间戳、传入参数、期望位置等，并使用ROS2 Publish模块发布这些话题。

在这里插入图片描述

Trajectory Flight子系统

Trajectory Flight子系统跟Takeoff子系统大体一样，只不过在Desired Position部分有所改动，改为实时的发送自定义圆轨迹上的位置。

在这里插入图片描述

在这里插入图片描述

MATLAB Function将仿真时间作为输入，输出的是期望位置，函数内部的代码为：

function y = fcn(t)r = 5;%圆的半径
w = 0.5;%绕圆心的角速度t = t - 7;%去掉轨迹飞行开始前的时间position_x = r * (1 - cos(w * t));
position_y = r * sin(w * t);y = single([position_x position_y -5]);

这里给定了圆的半径、绕圆心的角速度，结算出每一时刻无人机的期望位置。

实现效果

Ubuntu中启动Gazebo仿真和microrts_agent守护进程，运行Simulink模型，可以看到Gazebo中的无人机已经进入Offboard模式并起飞悬停在5m的高度后沿自定义圆形轨迹飞行。

在这里插入图片描述

无人机在Gazebo中飞行时，无人机始终处于画面中央，会带着视角乱晃，在Gazebo中进行任何操作视角都无法固定，分析原因是PX4在Gazebo仿真中写了一个脚本来使无人机一直处于画面中央。

在这里插入图片描述

在Tools/sitl_run.sh文件中有如下的代码，控制Gazebo中的视角跟随无人机。

# Disable follow mode
if [[ "$PX4_NO_FOLLOW_MODE" != "1" ]]; thenfollow_mode="--gui-client-plugin libgazebo_user_camera_plugin.so"
elsefollow_mode=""
fi

在运行仿真命令时加上前缀PX4_NO_FOLLOW_MODE=1来屏蔽视角跟随部分代码。

PX4_NO_FOLLOW_MODE=1 make px4_sitl_rtps gazebo

之后再次运行即可在固定视角下观察无人机的运动。

在这里插入图片描述

在这里插入图片描述

参考资料：

PX4 Gazebo Simulation

Control a Simulated UAV Using ROS 2 and PX4 Bridge

这篇关于【PX4SimulinkGazebo联合仿真】在Simulink中使用ROS2控制无人机沿自定义圆形轨迹飞行并在Gazebo中可视化的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/732556。

相关文章

Java使用ANTLR4对Lua脚本语法校验详解

Java使用ANTLR4对Lua脚本语法校验详解

《Java使用ANTLR4对Lua脚本语法校验详解》ANTLR是一个强大的解析器生成器,用于读取、处理、执行或翻译结构化文本或二进制文件,下面就跟随小编一起看看Java如何使用ANTLR4对Lua脚本... 目录什么是ANTLR？第一个例子ANTLR4 的工作流程Lua脚本语法校验准备一个Lua Gramm
阅读更多...
Java Optional的使用技巧与最佳实践

Java Optional的使用技巧与最佳实践

《JavaOptional的使用技巧与最佳实践》在Java中,Optional是用于优雅处理null的容器类,其核心目标是显式提醒开发者处理空值场景,避免NullPointerExce... 目录一、Optional 的核心用途二、使用技巧与最佳实践三、常见误区与反模式四、替代方案与扩展五、总结在 Java
阅读更多...
使用Java将DOCX文档解析为Markdown文档的代码实现

使用Java将DOCX文档解析为Markdown文档的代码实现

《使用Java将DOCX文档解析为Markdown文档的代码实现》在现代文档处理中,Markdown（MD）因其简洁的语法和良好的可读性,逐渐成为开发者、技术写作者和内容创作者的首选格式,然而,许多文... 目录引言1. 工具和库介绍2. 安装依赖库3. 使用Apache POI解析DOCX文档4. 将解析
阅读更多...
Qt中QUndoView控件的具体使用

Qt中QUndoView控件的具体使用

《Qt中QUndoView控件的具体使用》QUndoView是Qt框架中用于可视化显示QUndoStack内容的控件,本文主要介绍了Qt中QUndoView控件的具体使用,具有一定的参考价值,感兴趣的... 目录引言一、QUndoView 的用途二、工作原理三、 如何与 QUnDOStack 配合使用四、自
阅读更多...
C++使用printf语句实现进制转换的示例代码

C++使用printf语句实现进制转换的示例代码

《C++使用printf语句实现进制转换的示例代码》在C语言中,printf函数可以直接实现部分进制转换功能,通过格式说明符（formatspecifier）快速输出不同进制的数值,下面给大家分享C+... 目录一、printf 原生支持的进制转换1. 十进制、八进制、十六进制转换2. 显示进制前缀3. 指
阅读更多...
使用Python构建一个Hexo博客发布工具

使用Python构建一个Hexo博客发布工具

《使用Python构建一个Hexo博客发布工具》虽然Hexo的命令行工具非常强大,但对于日常的博客撰写和发布过程,我总觉得缺少一个直观的图形界面来简化操作,下面我们就来看看如何使用Python构建一个... 目录引言Hexo博客系统简介设计需求技术选择代码实现主框架界面设计核心功能实现1. 发布文章2. 加
阅读更多...
shell编程之函数与数组的使用详解

shell编程之函数与数组的使用详解

《shell编程之函数与数组的使用详解》：本文主要介绍shell编程之函数与数组的使用,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录shell函数函数的用法俩个数求和系统资源监控并报警函数函数变量的作用范围函数的参数递归函数shell数组获取数组的长度读取某下的
阅读更多...
使用Python开发一个带EPUB转换功能的Markdown编辑器

使用Python开发一个带EPUB转换功能的Markdown编辑器

《使用Python开发一个带EPUB转换功能的Markdown编辑器》Markdown因其简单易用和强大的格式支持,成为了写作者、开发者及内容创作者的首选格式,本文将通过Python开发一个Markd... 目录应用概览代码结构与核心组件1. 初始化与布局 (__init__)2. 工具栏 (setup_t
阅读更多...
Python虚拟环境终极(含PyCharm的使用教程)

Python虚拟环境终极(含PyCharm的使用教程)

《Python虚拟环境终极(含PyCharm的使用教程)》：本文主要介绍Python虚拟环境终极(含PyCharm的使用教程),具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,... 目录一、为什么需要虚拟环境？二、虚拟环境创建方式对比三、命令行创建虚拟环境（venv）3.1 基础命令3
阅读更多...
Python Transformer 库安装配置及使用方法

Python Transformer 库安装配置及使用方法

《PythonTransformer库安装配置及使用方法》HuggingFaceTransformers是自然语言处理（NLP）领域最流行的开源库之一,支持基于Transformer架构的预训练模... 目录python 中的 Transformer 库及使用方法一、库的概述二、安装与配置三、基础使用：Pi
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2