【PX4SimulinkGazebo联合仿真】在Simulink中使用ROS2控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化

在Simulink中使用ROS2控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化

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

本篇文章介绍如何使用ROS2控制无人机进入Offboard模式起飞悬停并在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联合仿真。

在这里插入图片描述

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模型,我这里命名为Offboard.slx,双击使用Simulink打开。

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

整体框架

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

在这里插入图片描述

各子系统实现原理

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号,之后param2param7设置为空,但是这里的注释好像写错了。

在源码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 mode

uint64 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 controlled

uint64 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_setpoint
    receive: true
  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
    base:    vehicle_local_position_setpoint
    receive: 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模块发布这些话题。

在这里插入图片描述

实现效果

Ubuntu中启动Gazebo仿真和microrts_agent守护进程,运行Simulink模型,可以看到Gazebo中的无人机已经进入Offboard模式并起飞悬停在5m的高度。

在这里插入图片描述

在这里插入图片描述


参考资料:

PX4 Gazebo Simulation

Control a Simulated UAV Using ROS 2 and PX4 Bridge

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/409991.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

C语言编程安全规范

目的 本规范旨在加强编程人员在编程过程中的安全意识,建立编程人员的攻击者思维,养成安全编码的习惯,编写出安全可靠的代码。 2 宏 2.1 用宏定义表达式时,要使用完备的括号 2.2 使用宏时,不允许参数发生变化 3 变量 3.1 所有变量在定义时必须赋初值 变量声明赋予初值,可…

matlab simulink永磁同步电机pid控制

1、内容简介 略 53-可以交流、咨询、答疑 2、内容说明 略 摘 要 19世纪90年代&#xff0c;美国西屋电气公司研制出了世界上第一台交流同步电机。随着科学技术的迅猛发展和生产工艺的持续进步&#xff0c;在20世纪50年代出现了永磁同步电机。它以永磁体代替电励磁绕组&#…

CSS重点

第一章&#xff1a;CSS类型 1、行内样式 <div style"color:red;font-size:30px;font-weight: 900;font-style: italic;">qcby</div>注意&#xff1a;行内样式&#xff0c;作用力优先级最高&#xff0c;但是不利于html与css的书写以及修改&#xff0c;会…

曲线生成 | 图解B样条曲线生成原理(附ROS C++/Python/Matlab仿真)

目录 0 专栏介绍1 控制点计算之插值2 控制点计算之近似3 仿真实现3.1 ROS C实现3.2 Python实现3.3 Matlab实现 0 专栏介绍 &#x1f525;附C/Python/Matlab全套代码&#x1f525;课程设计、毕业设计、创新竞赛必备&#xff01;详细介绍全局规划(图搜索、采样法、智能算法等)&a…

990-11产品经理:Team Building in Project Management 项目管理中的团队建设

Introduction One of the most important developments in management during the 1970’s has been the widespread application广泛应用 of project teams to a variety of complex tasks. Project managers quickly learn the critical significance批判意义 of the effect…

Android RecyclerView 如何展示自定义列表 Kotlin

Android RecyclerView 如何展示自定义列表 Kotlin 一、前提 有这么一个对象 class DeviceDemo (val name: String, val type: String, val address: String)要展示一个包含这个对象的列表 bluetoothDevices.add(DeviceDemo("bb 9800", "LE", "32:…

Qt QWiget 实现简约美观的加载动画 第三季

&#x1f603; 第三季来啦 &#x1f603; 这是最终效果: 只有三个文件,可以直接编译运行 //main.cpp #include "LoadingAnimWidget.h" #include <QApplication> #include <QVBoxLayout> #include <QGridLayout> int main(int argc, char *argv[]…

《Docker 简易速速上手小册》第8章 Docker 在企业中的应用(2024 最新版)

文章目录 8.1 Docker 在开发环境中的应用8.1.1 重点基础知识8.1.2 重点案例&#xff1a;Python Web 应用开发环境8.1.3 拓展案例 1&#xff1a;Python 数据分析环境8.1.4 拓展案例 2&#xff1a;Python 自动化测试环境 8.2 Docker 在生产环境的实践8.2.1 重点基础知识8.2.2 重点…

R语言在生态环境领域中的应用

R语言作为新兴的统计软件&#xff0c;以开源、自由、免费等特点风靡全球。生态环境领域研究内容广泛&#xff0c;数据常多样而复杂。利用R语言进行多元统计分析&#xff0c;从复杂的现象中发现规律、探索机制正是R的优势。为此&#xff0c;本课程以鱼类、昆虫、水文、地形等多样…

精品基于springboot健身房管理系统-教练会员卡管理

《[含文档PPT源码等]精品基于springboot健身房管理系统[包运行成功]》该项目含有源码、文档、PPT、配套开发软件、软件安装教程、项目发布教程、包运行成功&#xff01; 软件开发环境及开发工具&#xff1a; Java——涉及技术&#xff1a; 前端使用技术&#xff1a;HTML5,CS…

异常统一处理:Exception(兜底异常)

一、引言 本篇内容是“异常统一处理”系列文章的重要组成部分&#xff0c;主要聚焦于对 Exception&#xff08;兜底异常&#xff09; 的原理解析与异常处理机制&#xff0c;并给出测试案例。 关于 全局异常统一处理 的原理和完整实现逻辑&#xff0c;请参考文章&#xff1a; 《…

docker搭建zookeeper集群

文章目录 1. 集群搭建2. Leader选举3. Zookeeper集群角色 1. 集群搭建 这里我们使用docker-compose 搭建伪集群 version: 3.1 services:zoo1:image: zookeeperrestart: alwayscontainer_name: zoo1ports:- 2181:2181volumes:- /home/zk/zoo1/data:/data- /home/zk/zoo1/datal…

【数据结构初阶 7】二叉树:链式二叉树的基本操作实现

文章目录 &#x1f308; Ⅰ 定义二叉树结点&#x1f308; Ⅱ 创建二叉树结点&#x1f308; Ⅲ 遍历二叉树1. 先序遍历2. 中序遍历3. 后序遍历4. 层序遍历 &#x1f308; Ⅳ 销毁二叉树 &#x1f308; Ⅰ 定义二叉树结点 1. 每个结点都由三部分组成 数据域&#xff1a;存储本结…

【JVM】线上一次fullGC排查思路

fullGC问题背景 监控告警发现&#xff0c;今天开始我们线上应用频繁出现fullGC&#xff0c;并且每次出现后磁盘都会被占满 查看监控 查看监控发现FULLGC的机器均为同一个机房的集器&#xff0c;并且该机房有线上error报错&#xff0c;数据库监控对应的时间点也有异常&#x…

Vue3 路由配置 + 路由跳转 + 路由传参(动态路由传参 + 普通路由传参)

Vue Router&#xff1a; Vue.js 的官方路由。它与 Vue.js 核心深度集成&#xff0c;让用 Vue.js 构建单页应用变得轻而易举。 效果 一、介绍 1、官方文档&#xff1a;https://router.vuejs.org/zh/introduction.html 介绍 | Vue RouterVue.js 的官方路由https://router.vuejs.…

Java/Python/Go不同开发语言基础数据结构和相关操作总结-GC篇

Java/Python/Go不同开发语言基础数据结构和相关操作总结 1. 常见gc方式1.1 gc判断对象是否存活1.2 引用计数法1.2 标记-清除算法1.3 复制算法1.4 标记-压缩算法1.5 分代收集算法 2. java的gc方式以及垃圾回收器2.1 gc方式2.1 gc回收器2.1.1 Serial收集器2.1.2 ParNew收集器2.1.…

SSM---Mybatis查询数据库的功能

Mybatis查询数据库的功能流程&#xff1a; 在maven中加入mybatis依赖&#xff0c;mysql驱动依赖创建一张student表创建表对应的实体类&#xff1a;student类&#xff0c;用来保存表中的每行数据创建持久层的DAO接口&#xff0c;用来定义操作数据库的方法创建这个表对应的sql映…

计算机设计大赛 深度学习图像风格迁移 - opencv python

文章目录 0 前言1 VGG网络2 风格迁移3 内容损失4 风格损失5 主代码实现6 迁移模型实现7 效果展示8 最后 0 前言 &#x1f525; 优质竞赛项目系列&#xff0c;今天要分享的是 &#x1f6a9; 深度学习图像风格迁移 - opencv python 该项目较为新颖&#xff0c;适合作为竞赛课题…

IDEA下新建SpringBoot项目详细步骤

在IDEA下使用Spring Initializer&#xff1a; 一、新建项目&#xff0c;利用阿里云网址https://start.aliyun.com/下载项目&#xff0c;来到Spring Initializer模块&#xff1a; 我的jdk是8&#xff0c;构建Maven类型的项目&#xff0c;Java版本选8&#xff0c;Group为公司名。…

二阶低通滤波器(博途PLC SCL源代码)

在学习滤波器之前我们先了解下截止频率的准确定义,周期正弦信号经过传递函数后的输出信号,其幅值衰减-3dB时对应的频率。-3dB的含义是幅值衰减为原来的约0.707。更多滤波器信号处理相关内容请参看下面文章链接: 1、PLC一阶低通滤波器 https://rxxw-control.blog.csdn.net/…