自学内容网 自学内容网

【PX4&Simulink&Gazebo联合仿真】在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号,之后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模块发布这些话题。

在这里插入图片描述

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


原文地址:https://blog.csdn.net/weixin_45031928/article/details/136243801

免责声明:本站文章内容转载自网络资源,如本站内容侵犯了原著者的合法权益,可联系本站删除。更多内容请关注自学内容网(zxcms.com)!