Gazebo多无人机编队仿真
上篇文章中我们主要讲到了怎么利用Matlab根据仿真多无人机编队的情况,最终的效果通过Matlab的可视化工具展示了出来,这篇文章,我们就来介绍怎么将Matlab的多无人机编队的效果通过Gazebo的3d物理仿真软件展示出来,这里我们主要利用了rotor_simulition的无人机仿真工具。
多无人机仿真环境搭建
在多无人机仿真环境搭建中我们主要利用了rotors_simulation的无人机仿真平台,下面将介绍一下如何搭建rotors_simulation无人机仿真平台
本项目的主要实现环境是在ubuntu 18.04、ROS melodic版本下实现的
1 . 下载安装好ROS相关功能包、wstool、catkin-tools
2 . 创建工作文件夹,下载rotor_simulations
mkdir -p ~/catkin_ws/src #创建文件夹
catkin_init_workspace #初始化工作空间
wstool init #工具初始化
wget
https://raw.githubusercontent.com/ethz-asl/rotors_simulator/master/rotors_hil.rosinstall #下载rotor_simulation工具
wstool merge rotors_hil.rosinstall
wstool update
3 . 编译rotors_simulation功能包
cd ~/catkin_ws/
catkin build
Gazebo多无人机编队实现
多无人机的仿真实现与多机器人类似,具体的原理操作大家可以参考古月居中的多机器人的配置。在本项目中,我们通过编写launch文件,调用rotor_simulation中的文件,以及实现多无人机Gazebo仿真,具体的代码可以参考:
multi_uav_simulation.launch
<!-- 参数声明-->
<arg name="mav_name" default="firefly"/>
<arg name="world_name" default="cloister"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="log_file" default="$(arg mav_name)" />
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<arg name="paused" default="true"/>
<arg name="verbose" default="false"/>
<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
<env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
<!-- 启动Gazebo-->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
<arg name="debug" value="$(arg debug)" />
<arg name="paused" value="$(arg paused)" />
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="$(arg verbose)"/>
</include>
<!-- 多无人机模型UAV1-->
<group ns="UAV1">
<!--启用UAV的模型-->
<include file="$(find rotors_gazebo)/launch/multi/multi_spawn_mav.launch">
<arg name="namespace" default="UAV1"/>
<arg name="mav_name" value="$(arg mav_name)" />
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
<arg name="x" default="0.0"/>
<arg name="y" default="4.0"/>
<arg name="z" default="0.1"/>
</include>
<!-- 无人机位置式控制-->
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
<rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
<rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
<!-- 无人机控制器-->
<node name="motion_controller" pkg="rotors_teleop" type="rotors_formation_son.py" output="screen">
<param name = "UAV" type="string" value = "/UAV1/"/>
<param name = "seq" type="int" value = "0"/>
</node>
<!-- 开启相关节点-->
<node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</group>
<!-- 第二架无人机-->
<group ns="UAV2">
<include file="$(find rotors_gazebo)/launch/multi/multi_spawn_mav.launch">
<arg name="namespace" default="UAV2"/>
<arg name="mav_name" value="$(arg mav_name)" />
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
<arg name="x" default="0.0"/>
<arg name="y" default="2.4"/>
<arg name="z" default="0.1"/>
</include>
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
<rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
<rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
<node name="motion_controller" pkg="rotors_teleop" type="rotors_formation_son.py" output="screen">
<param name = "UAV" type="string" value = "/UAV2/"/>
<param name = "seq" type="int" value = "1"/>
</node>
<node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</group>
以上是在rotor_simulation的环境下启用多无人机的launch文件,我们如果想改变无人机的数量的话,只需要更换namespace,增加对应的即可。
根据多无人机编队的需求,这里我们使用了六架无人机进行仿真模拟,其中展示效果如下所示:
那么我们怎么让这六架无人机按照我们想要的编队效果进行运动呢?这就需要我们写一个位置式的无人机控制器,通过读取Matlab仿真得到的无人机位置变化,分别对无人机进行控制,可以参考下面的python控制代码:
rotors_formation_son.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#导入相关的库
import rospy
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
import sys, select, termios, tty
from numpy import *
import operator
from os import listdir
import time
#建立结构体
twist= Twist()
pose=PoseStamped()
#Create Publisher
#初始化ros节点
rospy.init_node('formation_publish')
r=rospy.Rate(1)
UAV=rospy.get_param('~UAV')
seq=rospy.get_param('~seq')
print(UAV+'command/pose')
print(seq)
num=seq
pose_pub = rospy.Publisher(UAV+'command/pose',PoseStamped,queue_size=1)
#初始化函数
def __init__():
pose.pose.position.x=0
pose.pose.position.y=0
pose.pose.position.z=0
#读取Matlab中的数据
def file2matrix(filename):
fr = open(filename)
numberOfLines = len(fr.readlines()) #get the number of lines in the file
returnMat = zeros((numberOfLines,3)) #prepare matrix to return
classLabelVector = [] #prepare labels return
fr = open(filename)
index = 0
for line in fr.readlines():
line = line.strip()
listFromLine = line.split('\t')
classLabelVector.append(int(listFromLine[0]))
returnMat[index,:] = listFromLine[1:4]
index += 1
return returnMat,numberOfLines
#计算无人机位置
def odomCallback(msg):
global num
if msg.position.x>=pose.pose.position.x-0.1 and num<numberOfLines:
num += 6
time.sleep(0.01)
pose.pose.position.x=dataMat[num][0]
pose.pose.position.y=dataMat[num][1]*2
pose.pose.position.z=dataMat[num][2]*0.5
pose_pub.publish(pose)
__init__();
dataMat,numberOfLines = file2matrix("/home/zdz/source/data/pos_data.txt")
#Create subscriber
odom_sub = rospy.Subscriber('odometry_sensor1/pose',Pose, odomCallback,queue_size = 1)
# spin
rospy.spin()
上述代码主要实现了读取Matlab中无人机编队的位置,并实时发布无人机位置的指令“command/pose”,最终通过分别给六架无人机发布位置式的指令,控制无人机的运动,达到编队飞行的效果(其中pos_data.txt是通过将Matlab中运行后得到的数据vehicle_pos转存为txt格式得到)
最终运行起来的节点图如下所示:
多无人机编队仿真演示
我们将编写的launch文件放入rotor_gazebo文件夹中,编译之后使用运行指令:
roslaunch rotor_gazebo multi_uav_simulation.launch
即可启动多无人机Gazebo仿真程序,多无人机编队飞行的效果可以参考下面录制的视频,在Gazebo仿真结果我们可以看到,六架无人机可以比较好的保持队形进行平移运动与旋转运动,大家有兴趣的话也可以自己动手尝试。期间有遇到任何问题可以在古月居论坛的无人机版块与笔者一起探讨。
评论列表(12条)
为什么运行3d仿真的时候,有个路劲报错v = VideoWriter(‘C:\Users\Jens\Documents\Uni Stuttgart\BA\BA ohne Video\Matlab\Consensus_3D\3 formation_control_quadro_feedback_final\Videos\Gut\vehicle_trajectories_3D_mit_des.mp4′,’MPEG-4’);,怎么修改
您好,请问”pos_data.txt是通过将Matlab中运行后得到的数据vehicle_pos转存为txt格式得到”存好的数据文本格式有要求吗?如果您能有时间回复一下我的疑惑对我的帮助真的非常大,不胜感激!
您好,我参照您的教程,第一篇用matlab跑出来非常成功,在继续学习了ros很多基本操作后,依旧不明白“pos_data.txt是通过将Matlab中运行后得到的数据vehicle_pos转存为txt格式得到”这一步该如何操作呢,是需要将结果存为txt然后存在哪个文件夹呢?然后使用语句调用吗?如果您能有时间回复一下我的疑惑对我的帮助真的非常大,不胜感激!
您好,我跟着您的教程学了有好一段时间了,现在也在ubuntu中安装好了matlab,特别想清教您“pos_data.txt是通过将Matlab中运行后得到的数据vehicle_pos转存为txt格式得到”,这一步具体是如何实现呢?我找了这个是在simulink中的一个模块的输出,请问是将结果存成矩阵形式吗?有行列数要求吗?如果您能回复这条留言真的不胜感激
请问multi_uav_simulation.launch文件中,的multi/multi_spawn_mav.launch是您自己写的吗?因为rotor_simulition的rotors_gazebo)/launch文件夹下,是没有更深一层文件夹的,都是一些launch文件。
您的这个问题解决了马,multi/multi_spawn_mav.launch文件应该是作者自己写的,但是我不会写
您好,小白求教,我想做无人机威胁评估的项目,但很难找到数据,请问有相关软件可以仿真出大量数据吗?
小白求教:编写完的launch文件需要放在哪个文件下catkin build呢
在仿真的时候出现了无人机一顿一顿的情况,推测是发布的控制频率问题,但是修改rospy.Rate依然没有改变,请问该如何解决?
建议这么去看,首先将无人机位置控制的topic echo出来,观察输出的数据刷新频率是否符合预期,如果符合预期,可以判断是电脑显卡不足,Gazebo带不动。
应该不是电脑的问题,就两架,而且如果按照源代码中的rospy.Rate(1),time.sleep(0.5),每次向前1m,就很流畅😂更改time.sleep(0.5)或者步进的距离就会出现停顿或者突然发散的状态…..
您好请问这个源码哪里有哇,想学习一下。2312307525@qq.com