【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联合仿真。

在这里插入图片描述

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打开。

在这里插入图片描述

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

在这里插入图片描述

【硬件实现】栏中【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 = 2;%圆的半径
w = 0.5;%绕圆心的角速度t = t;%去掉轨迹飞行开始前的时间position_x = r * (1 - cos(w * t));
position_y = r * sin(w * t);
position_z = -5;
position_yaw = pi / 2 - w * t;%航向角的取值应限制到-pi到pi
while position_yaw > pi || position_yaw < piif position_yaw > piposition_yaw = position_yaw - 2 * pi;elseif position_yaw < -piposition_yaw = position_yaw + 2 * pi;elsebreakend
endy = single([position_x position_y position_z position_yaw]);

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

需要注意的是在源码Firmware/msg/vehicle_local_position_setpoint.msg中trajectory_setpoint话题中yaw的取值范围。

float32 yaw		# in radians NED -PI..+PI

这里加入了航向角的控制，航向角是沿无人机顺时针从角度-pi到pi，飞机在Gazebo环境初始生成时的朝向是pi/2角度。

实现效果

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

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

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

模板（类模板）---C++

模板（类模板）---C++

模板目录 2.类模板2.1 类模板语法2.2 类模板与函数模板区别2.3 类模板中成员函数创建时机2.4 类模板对象做函数参数2.5 类模板与继承2.6 类模板成员函数类外实现2.7 类模板分文件编写2.8 类模板与友元2.9 类模板案例 2.类模板 2.1 类模板语法 类模板作用&#xff1a; 建立一个…
阅读更多...
[嵌入式AI从0开始到入土]15_orangepi_aipro欢迎界面、ATC bug修复、镜像导出备份

[嵌入式AI从0开始到入土]15_orangepi_aipro欢迎界面、ATC bug修复、镜像导出备份

[嵌入式AI从0开始到入土]嵌入式AI系列教程 注&#xff1a;等我摸完鱼再把链接补上 可以关注我的B站号工具人呵呵的个人空间&#xff0c;后期会考虑出视频教程&#xff0c;务必催更&#xff0c;以防我变身鸽王。 第1期 昇腾Altas 200 DK上手 第2期 下载昇腾案例并运行 第3期 官…
阅读更多...
notepad++运行python闪一下就没啦

notepad++运行python闪一下就没啦

问题&#xff1a;Notepad直接快捷键运行Python代码,出现闪一下就没了 解决措施&#xff1a; ①点击菜单运行(Run) --> 运行(Run)弹出的对话框 ②把 cmd /k python "$(FULL_CURRENT_PATH)" & ECHO. & PAUSE & EXIT 粘贴进入这个对话框内 ③点击保存&a…
阅读更多...
Rust Vs Go:从头构建一个web服务

Rust Vs Go:从头构建一个web服务

Go 和 Rust 之间的许多比较都强调它们在语法和初始学习曲线上的差异。然而&#xff0c;最终的决定性因素是重要项目的易用性。 “Rust 与 Go”争论 Rust vs Go 是一个不断出现的话题&#xff0c;并且已经有很多关于它的文章。部分原因是开发人员正在寻找信息来帮助他们决定下…
阅读更多...
EventStream获得数据流，前端配置获得推送的流

EventStream获得数据流，前端配置获得推送的流

如上图所示&#xff0c;请求一个接口&#xff0c;接口以数据流的方式向客户端推送数据&#xff0c;默认需要消息收集一条&#xff0c;在原来的基础上追加&#xff0c;在create-react-app生成的工程中&#xff0c;如果代理使用了中间件http-proxy-middleware&#xff0c;同时dev…
阅读更多...
强化学习（GPS）

强化学习（GPS）

GPS——Guided Policy Search引导策略搜索 基于模型的强化学习算法 GPS目前被作为基础算法广泛应用于各种强化学习任务中&#xff0c;其出发点在于纯粹的策略梯度方法在更新参数时不会用到环境模型因而属于一种无模型强化学习算法。由于没有利用任何环境的内在属性&#xff0…
阅读更多...
stm32cubemx简单介绍

stm32cubemx简单介绍

&#xff08;本文为简单介绍&#xff0c;内容源于网络&#xff09; STM32CubeMX是STMicroelectronics推出的一款用于STM32微控制器系列的图形化配置工具&#xff0c;旨在简化嵌入式软件开发过程。本文将对STM32CubeMX进行简要介绍&#xff0c;包括其功能特点、优势以及在嵌入式…
阅读更多...
猫头虎分享已解决Bug || RuntimeError: size mismatch, m1: [32 x 100], m2: [500 x 10]

猫头虎分享已解决Bug || RuntimeError: size mismatch, m1: [32 x 100], m2: [500 x 10]

博主猫头虎的技术世界 &#x1f31f; 欢迎来到猫头虎的博客 — 探索技术的无限可能&#xff01; 专栏链接&#xff1a; &#x1f517; 精选专栏&#xff1a; 《面试题大全》 — 面试准备的宝典&#xff01;《IDEA开发秘籍》 — 提升你的IDEA技能&#xff01;《100天精通鸿蒙》 …
阅读更多...
2.22学习总结

2.22学习总结

1.营救 2.租用游艇 3.砍树 4.买礼物 5.刷题统计 砍树https://www.dotcpp.com/oj/problem3157.html 题目描述 给定一棵由 n 个结点组成的树以及 m 个不重复的无序数对 (a1, b1), (a2, b2), . . . , (am, bm)&#xff0c;其中 ai 互不相同&#xff0c;bi 互不相同&#xff0c;ai…
阅读更多...
Spring 类型转换、数值绑定与验证（一）— DataBinder

Spring 类型转换、数值绑定与验证（一）— DataBinder

DataBinder 是Spring用于数据绑定、类型转换及验证的类。使用场景有&#xff1a;1&#xff09;xml配置文件定义bean,Spring 内部使用DataBinder 来完成属性的绑定&#xff1b;2&#xff09;Web请求参数绑定&#xff0c;在Spring MVC 中&#xff0c;Controller的方法参数通常会自…
阅读更多...
还在为选择办公软件而烦恼吗?不妨试试ONLYofficeV8.0

还在为选择办公软件而烦恼吗?不妨试试ONLYofficeV8.0

目录 一.优势一DOC 1.丰富的文字处理功能 2.按用户既定的规则编辑 3.使用AI助手 4.保持创意 5.深入分析文本 6.改善团队工作流程 7.轻松对比文档 8.扩展编辑功能 二.优势二sheet 1.数据分析 2.轻松实现精准计算 3.轻松分析数据 4.可视化呈现数据 5.增强团队协作…
阅读更多...
opencv图像放缩与插值-resize函数

opencv图像放缩与插值-resize函数

在OpenCV中&#xff0c;resize函数用于对图像进行尺寸调整&#xff08;放大或缩小&#xff09;&#xff0c;这个过程中通常需要用到插值方法来计算新尺寸下图像像素的值。插值方法对于放缩的质量有着直接影响。 void resize(InputArray src, OutputArray dst, Size dsize, dou…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023