【PX4&Simulink&Gazebo联合仿真】在Simulink中使用ROS2控制无人机沿自定义8字形轨迹正向飞行（带偏航角控制）并在Gazebo中可视化

后厂村路直博生2024-02-26 20:26

在Simulink中使用ROS2控制无人机沿自定义8字形轨迹正向飞行（带偏航角控制）并在Gazebo中可视化

本篇文章介绍如何使用使用ROS2控制无人机沿自定义8字形轨迹正向飞行（带偏航角控制）并在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 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模块发布这些话题。

Trajectory Flight子系统

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

8字形曲线是一种特殊的利萨茹曲线，其函数为：

x = cos(t)
y = sin(t)cos(t)

但是这个曲线t=0时刻是从点(0,1)开始的，我们的无人机初始在点(0,0)，所以将cos(t)改为sin(t)，这样曲线的形状不变，起始点就变成了点(0,0)。

同时为了扩大该8字形曲线的大小，给其x、y分别乘以一个参数a、b用以放大曲线。则8字形的曲线函数变成了：

x = a * sin(t)
y = b * sin(t)cos(t)

同时可以用x、y的增量求出每个点的切线的正切值，这个正切值可以用于计算每一时刻无人机的期望航向角。

dx = a * cos(t) * dt
dy = b * cos(2t) * dt

yaw =atan2( b * cos(2t) , a * cos(t))

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

function y = fcn(t)

a = 8;%x轴上的范围
b = 6;%y轴上的范围
w = 0.5;%角速度

t = t-7;%去掉轨迹飞行开始前的时间

position_x = a * sin(w * t);
position_y = b * sin(w * t) .* cos(w * t);
position_z = -5;
position_yaw =atan2( b * cos(2 * w * t) , a * cos(w * t));

%航向角的取值应限制到-pi到pi
while position_yaw > pi || position_yaw < pi
    if position_yaw > pi
        position_yaw = position_yaw - 2 * pi;
    elseif position_yaw < -pi
        position_yaw = position_yaw + 2 * pi;
    else
        break
    end
end

y = single([position_x position_y position_z position_yaw]);

这里给定了8字形的x轴上的范围、y轴上的范围、角速度，结算出每一时刻无人机的期望位置和航向角。

需要注意的是在源码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的高度后沿自定义8字形轨迹飞行，并且机头始终朝着前进方向。

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

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

# Disable follow mode
if [[ "$PX4_NO_FOLLOW_MODE" != "1" ]]; then
    follow_mode="--gui-client-plugin libgazebo_user_camera_plugin.so"
else
    follow_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

利萨茹曲线（Curve专题1）------三角函数系Curve

相关推荐
创小董7 小时前
高海拔低温地区无人机大载重吊运技术详解
无人机
创小董13 小时前
垂起固定翼无人机大面积森林草原巡检技术详解
无人机
IT猿手20 小时前
基于PWLCM混沌映射的麋鹿群优化算法（Elk herd optimizer，EHO）的多无人机协同路径规划，MATLAB代码
算法·elk·机器学习·matlab·无人机·聚类·强化学习
创小董20 小时前
无人机飞防高效率喷洒技术详解
无人机
云卓SKYDROID2 天前
反无人机防御系统概述！
无人机·科普·高科技·云卓科技
EasyDSS2 天前
视频直播点播平台EasyDSS与无人机技术的森林防火融合应用
音视频·无人机
IT猿手3 天前
SDMTSP：黑翅鸢算法（Black-winged kite algorithm，BKA）求解单仓库多旅行商问题，可以更改数据集和起点（MATLAB代码）
人工智能·深度学习·机器学习·matlab·无人机·智能优化算法
创小董3 天前
低温高海拔大载重无人机吊运技术详解
无人机
白嫖叫上我3 天前
Cesium 无人机航线规划（航点航线）
无人机·cesium
创小董3 天前
低空无人机产教融合技术详解
无人机
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02YOLOv8训练好模型后，追加轮数继续训练、或者提前终止训练，缩减训练轮数03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05HCIA-datacom数通题库和录播视频资料06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10树莓派 5 AI 套件（Hailo-8L）使用教程