ROS-Based Multi-Agent Systems COntrol Simulation Testbed (MASCOT)

Figure 1 shows the standard definitions [16] of the reference frames used in this simulation. $\{E\}$ is the Earth reference frame having its $z$-axis facing upward in the space. The body coordinate frame, $\{B\}$ is attached to the center of the quadcopter having its $z$-axis facing downwards in the space. $\{B^{\prime}\}$ reference frame is attached to the body having its origin same as $\{B\}$ and $x^{B^{\prime}}y^{B^{\prime}}$ plane is parallel to $x^{E}y^{E}$.

The angles $\theta_{p}$ (pitch), $\theta_{r}$ (roll), and $\theta_{y}$ (yaw) represents the rotation of the body along $x$, $y$, and $z$-axis of the reference frame respectively. As $x^{B^{\prime}}y^{B^{\prime}}$ plane is parallel to $x^{E}y^{E}$ thus $\{B^{\prime}\}$ can only rotate along the z-axis with angle $\theta_{p}$. The change in orientation between $\{B^{\prime}\}$ and {E} is given by a rotation matrix

$$\mathbf{R}_{B^{\prime}}^{E}=\begin{bmatrix}c\theta_{y}&s\theta_{y}&0\\ -s\theta_{y}&c\theta_{y}&0\\ 0&0&1\end{bmatrix}$$

where $c(.)\coloneqq\cos{(.)}$ and $s(.)\coloneqq\sin{(.)}$

The upward thrust along the $-z^{B}$ axis is given by

$$T_{i}=b\bar{\omega}_{i}^{2}$$

where $\omega_{i}$ is the angular velocity of the rotor and $b>0$ is a lift constant that depends on the air density, number of blades, cube of rotor blade radius, and chord length of the blade. According to Newton’s second law of motion, the translation dynamics of the quadcopter in $\{E\}$ is given by

$$m\dot{\mathbf{v}}^{E}=\begin{bmatrix}0&0&mg\end{bmatrix}^{T}-\mathbf{R}_{B}^{E}\begin{bmatrix}0&0&T\end{bmatrix}^{T}-B\mathbf{v}$$

(1)

where $\mathbf{v}$ is the velocity of quadcopter in {E} given by $\mathbf{v}=\begin{bmatrix}{v_{x}}^{E}&{v_{y}}^{E}&{v_{z}}^{E}\end{bmatrix}^{T}\in\mathbb{R}^{3}$ , $B$ is aerodynamics friction and $T=\Sigma T_{i}$ is total upward thrust generated by the rotors in $-z^{B}$ direction, $g$ is the gravitational acceleration, and $m$ is the mass of quadcopter. In (1), first term represents the gravitational force acting in $-z^{E}$ direction, second term represents the total upward thrust generated in $-z^{B}$ direction in $\{B\}$ and rotated in $\{E\}$.

The rotation in each axis is obtained by varying pairwise differences in rotor thrust. Torque about $x$ and $y$ axes is given by

$$\tau_{x}=dT_{4}-dT_{2}=db(\bar{\omega}_{4}^{2}-\bar{\omega}_{2}^{2})$$

$$\tau_{y}=dT_{1}-dT_{3}=db(\bar{\omega}_{1}^{2}-\bar{\omega}_{3}^{2})$$

The torque applied to each motor is opposed by aerodynamic drag and exerts a reaction torque on the frame which is given by $Q_{i}=k\bar{\omega}_{i}$ where $k$ depends on the same factors as $b$. The total reaction torque about $z$-axis is given by

$$\tau_{z}=Q_{1}-Q_{2}+Q_{3}-Q_{4}=k\left(\bar{\omega}_{1}^{2}+\bar{\omega}_{3}^{2}-\bar{\omega}_{2}^{2}-\bar{\omega}_{4}^{2}\right)$$

(2)

The total torque applied to the quadcopter body is $\mathbf{\Gamma}=\begin{bmatrix}{\tau}_{x}&{\tau}_{y}&{\tau}_{z}\end{bmatrix}$. By Euler’s equation of motion rotational acceleration is given is by

$$J\bm{\dot{\omega}}=-\bm{\omega}\bm{\times}J\bm{\omega}+\mathbf{\Gamma}$$

(3)

where J is the $3\times 3$ inertia matrix of the quadcopter and $\omega$ is a angular velocity vector of the quadcopter body frame. Overall quadcopter motion equation [16] is obtained by integrating equations (1) and (2) which is given by

$\displaystyle\begin{bmatrix}\mathbf{T}&\mathbf{\Gamma}\end{bmatrix}^{T}$

$\displaystyle=A\begin{bmatrix}\bar{\omega}_{1}^{2}&\bar{\omega}_{2}^{2}&\bar{\omega}_{3}^{2}&\bar{\omega}_{4}^{2}\end{bmatrix}^{T}$

(4)

The overall motion in the plane $x^{B^{\prime}}y^{B^{\prime}}$ is obtained by pitching and rolling the quadcopter by an angle $\theta_{p}$ and $\theta_{r}$. The total force acting on a quadcopter is given by

$$\mathbf{f}^{B^{\prime}}=\mathbf{R}{x}\left(\theta{r}\right)\mathbf{R}{y}\left(\theta{p}\right)\begin{bmatrix}0&0&T\end{bmatrix}^{T}$$

(5)

where,

$\displaystyle\mathbf{R}{x}\left(\theta{r}\right)=\begin{bmatrix}1&0&0\\ 0&c\theta_{r}&s\theta_{r}\\ 0&-s\theta_{r}&c\theta_{r}\end{bmatrix},~{}\mathbf{R}{y}\left(\theta{p}\right)=\begin{bmatrix}c\theta_{p}&0&s\theta_{p}\\ 0&1&0\\ -s\theta_{p}&0&c\theta_{p}\end{bmatrix}$

are the Rotation matrix about $x$ and $y$ axes respectively.
Simplifying (5) we get $\mathbf{f}^{B^{\prime}}$ as

$$\mathbf{f}^{B^{\prime}}=\begin{bmatrix}T\sin{\theta_{p}}&T\sin{\theta_{r}}\cos{\theta_{p}}&T\cos{\theta_{r}}\cos{\theta_{p}}\end{bmatrix}^{T}$$

for small $\theta_{p}$ and $\theta_{r}$ the above equation can be approximated by

$$\mathbf{f}^{B^{\prime}}\approx\begin{bmatrix}T\theta_{p}&T\theta_{r}&T\end{bmatrix}^{T}$$

i.e $f_{x}^{B^{\prime}}\approx T\theta_{p}$ and $f_{y}^{B^{\prime}}\approx T\theta_{r}$
According to Newton’s Second law of motion $F=ma$, we can write

$$ma_{x}^{B^{\prime}}=T\theta_{p}$$

$$\theta_{p}=\dfrac{m}{T}a_{x}^{B^{\prime}},~{}~{}\theta_{r}=\dfrac{m}{T}a_{y}^{B^{\prime}}$$

where $a_{x}^{B^{\prime}}$ is $a_{y}^{B^{\prime}}$ is acceleration of quadcopter in $\{B^{\prime}\}$ frame.
As the controller will receive the position and velocity in $\{E\}$ frame so it will compute the acceleration in $\{E\}$ frame which needs to be rotated to the $\{B^{\prime}\}$ frame.

$$\mathbf{f}^{B^{\prime}}=\mathbf{R}_{E}^{B^{\prime}}\mathbf{f^{E}}$$

$$m\mathbf{a}^{B^{\prime}}=\mathbf{R}_{E}^{B^{\prime}}m\mathbf{a^{E}}$$

$$\mathbf{a}^{B^{\prime}}=\mathbf{R}_{E}^{B^{\prime}}\mathbf{a^{E}}$$

This equation shows the motion of the quadcopter in $x^{B^{\prime}}y^{B^{\prime}}$ plane.

A homogeneous multi-agent system is a collection of $n$ agents with identical dynamics communicating with each other over a communication graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$, where the set of vertices $\mathcal{V}=\{\alpha_{i},~{}i=1,...,n\}$ represents the agents and the edges $(\alpha_{i},\alpha_{j})\in\mathcal{E}$ denote the communication from agent $\alpha_{i}$ to $\alpha_{j}$. The communication graph $\mathcal{G}$ is undirected if $(\alpha_{i},\alpha_{j})\in\mathcal{E}\implies(\alpha_{j},\alpha_{i})\in\mathcal{E}$ otherwise it is directed. A path in a graph is a sequence of edges. An undirected graph is said to be connected if it has a path from each agent to every other agent. A directed graph is said to contain a directed spanning tree if there is an agent $\alpha_{r}$ (called root) such that there is a directed path from $\alpha_{r}$ to every other agent. Set of neighbours for each agent is given by $\mathcal{N}_{i}:=\{j\in\mathcal{V}:(\alpha_{i},\alpha_{j})\in\mathcal{E}\}$. The Laplacian matrix $\mathcal{L}$ of a graph $\mathcal{G}$ is given by $\mathcal{L}_{n}=\left[l{ij}\right]\in\mathbb{R}^{n\times n};l_{ij}=-a_{ij},i\neq j,l_{ii}=\sum_{j=1,j\neq i}^{n}a_{ij}$, where $a_{ij}$ is the weight of the edge $(\alpha_{i},\alpha_{j})$. Some control algorithms use leader-follower configuration of the multi-agent systems in which, the set of agents is divided into a set of leader $\mathbf{L}$ and a set of followers $\mathbf{F}$.

Figure 3 shows the Control block diagram of the system. The low-level control of the quadcopter is achieved by four independent controllers given below:

• •

  Heading Control

• •

  Altitude Control

• •

  Pitch and Roll angle control

• •

  Position Control

The simulation architecture of MASCOT is shown in Fig. 4.

In this system, the Gazebo simulator simulates the model and its sensors with the system plugin. Gazebo’s internal scheduler provides the ROS interfaces. This ROS interface enables a user to manipulate the properties of the simulation environment over ROS, as well as spawn and introspect on the state of models in the environment. ROS works as a middleware that runs the independent controller of individual robots spawned in Gazebo environments, ROS provides the intercommunication channel within the nodes, where all the controller nodes communicate with each other and Gazebo over the TCPROS protocol and exchange the information. The control node is written in python for the high-level control of the quadcopter the low-level control and plugin are developed in C++ which provides the tuned low-level control of the quadcopter. If needed user can modify or tune this low-level control.

For simulating the multi-agent systems using MASCOT, users can provide configuration parameters and control laws using a plain text configuration file, which is used by the internal programs for the initialization and simulation of the quadrotor.

The configurable parameters required are as follows:

• •

  Robot: Details of the Robots to be simulated

  • –

    Number: No. of Agents to be spawned in Gazebo.

  • –

    IntialPosition: Flag to enable random initial position or specified initial position.

  • –

    Position: Initial Position of Robot (x,y,z).

• •

  Control: Controls laws to be used

  • –

    Custom-Control: Flag to Enable Custom Control.

  • –

    Tutorial Examples: Flag to Enable Tutorial.

    1. 1.

       Waypoint Navigation:

       • *

         P-Gain: Proportial Gain , Default = 1.0

       • *

         D-Gain: Differential Gain , Default = 1.0

    2. 2.

       Consensus: Flag to Enable Consensus Control.

       • *

         Leader: Specify the robot number that to be leader, 0-for leaderless consensus.

       • *

         Communication Graph: Flag for communication graph

       • *

         L-mat: Laplacian Matrix.

    3. 3.

       Min-max Consensus: Enable Min-max Consensus Control.

• •

  Output: Configuration of output

  • –

    Velocity : Enable it to generate the velocity plot.

  • –

    Position : Enable it to generate the position plot.

  • –

    Save-plot : Enable to save the plots.

  • –

    Show-plot : Enable it to show plots.

  • –

    Save-data : Enable to save as numpy-array

A user needs to specify their control laws by setting the Custom-Control flag (set as default) as plain text using python syntax. The user can use the states of the quadcopter such as position, velocity, acceleration, and orientation in the control algorithm and can publish the acceleration command which is an output of the control algorithm. Few demos are made available as tutorials for the users which are described in Section IV below.

Theoretically, there is no limit on the number of agents in MASCOT, however, the simulation performance may be affected based on the number of agents and computer specifications. The effective control of the quadcopters depends on the system specifications as well as the computational complexity of the algorithm being tested.

The quadcopter is initialized at a $(0,0,1)$ position and the different waypoints are given in the $x^{E}y^{E}$ plane. From Figure 6, it can be inferred that the quadcopter achieves its goal position with a very small overshoot, which depends on the parameters of the controller.

We demonstrate two consensus algorithms that have linear control laws [18]: 1) for leaderless asymptotic consensus, and 2) leader-follower configuration i.e. consensus tracking In a leaderless case, four agent system is considered which are initialized at random positions, while in leader-follower configuration a ten-agent system is considered with $\alpha_{4}\in\textbf{L}$.

The control algorithms used is as follows:

$$\textbf{f}_{i}^{E}=\left\{\begin{array}[]{l}\sum_{j=1}^{n}a_{ij}\left(\mathbf{p}{j}^{E}-\mathbf{p}{i}^{E}\right)-\beta\textbf{v}_{i}^{E}\text{ if }\alpha_{i}\in\mathbf{F}\\ \\ 0\text{ if }\alpha_{i}\in\mathbf{L}\end{array}\right.$$

(7)

where $\beta$ is a positive constant. Note that, in a leaderless case, all the agents are assumed to be followers. All the quadcopters run their controllers simultaneously and plugin under their respective namespace. Figure 5 shows the visualization of the convergence of quadcopters in the Gazebo simulator. For the leaderless case, Fig. 7(a) shows the trajectory followed by all the quadcopters to reach the consensus point. Fig. 8 shows the convergence of the position of the quadcopter in the $x^{E}$ and $y^{E}$ axis. For leader-follower configuration Fig. 7(b) shows the trajectory followed by the follower and Fig. 9 show the convergence of the position of the Quadcopter in the $x^{E}$ and $y^{E}$-axis.

To demonstrate the simulations of non-linear distributed control laws, a min-max time consensus tracking algorithm proposed in [19] is used. This control algorithm extracts a directed spanning tree from the communication graph between the agents and treats each edge as a leader-follower pair. The root of the spanning tree gives the reference trajectory for consensus tracking. The agents are assumed to have bounded inputs. Using the difference of states between the leader ($\alpha_{p}$) and the follower ($\alpha_{c}$) in each edge, a distributed bang-bang control law is given for the following agents:

$$\mathbf{f}^{E}_{c}=\beta_{c}\text{sign}(2(\beta_{c}-\beta_{p})(\mathbf{p}_{c}-\mathbf{p}_{p})+(\mathbf{v}_{c}-\mathbf{v}_{p})^{2}\text{sign}(\mathbf{v}_{c}-\mathbf{v}_{p}))$$

(8)

where $\beta_{p}$ and $\beta_{c}$ are the input bounds of $\alpha_{p}$ and $\alpha_{c}$ respectively.

For simulation, a four-agent system is used with $\alpha_{1}$ as the root agent. Figure 10 shows the test result for this control.