投稿
  1. 首页
  2. 博客
  3. 建模仿真

Gazebo多无人机编队仿真

254
0
6天前
Judez
Judez

上篇文章中我们主要讲到了怎么利用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，增加对应的<group>即可。
根据多无人机编队的需求，这里我们使用了六架无人机进行仿真模拟，其中展示效果如下所示：
Gazebo多无人机编队仿真插图
那么我们怎么让这六架无人机按照我们想要的编队效果进行运动呢？这就需要我们写一个位置式的无人机控制器，通过读取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”，最终通过分别给六架无人机发布位置式的指令，控制无人机的运动，达到编队飞行的效果，最终运行起来的节点图如下所示：
Gazebo多无人机编队仿真插图(1)
多无人机编队仿真演示
我们将编写的launch文件放入rotor_gazebo文件夹中，编译之后使用运行指令：

roslaunch rotor_gazebo multi_uav_simulation.launch

即可启动多无人机Gazebo仿真程序，多无人机编队飞行的效果可以参考下面录制的视频，在Gazebo仿真结果我们可以看到，六架无人机可以比较好的保持队形进行平移运动与旋转运动，大家有兴趣的话也可以自己动手尝试。期间有遇到任何问题可以通过 940696183@qq.com 邮箱与笔者一起探讨。

原创文章，作者：Judez，如若转载，请注明出处：古月居 https://www.guyuehome.com/8981
赞 (3)
Judez Judez
1 0
基于px4的无人机自主导航
« 上一篇 6天前

相关文章

发表评论

后才能评论