Control of Multiple Mobile Robots Based on Data Fusion from Proprioceptive and Actuated Exteroceptive Onboard Sensors

1. Introduction

The paper presents a team of Automated Guided Vehicles (AGVs) equipped with open-source, perception-enhancing rotating devices. Each device has 10 ArUco markers, employed to compute the relative pose of other AGVs. Behavior trees are used to rotate the devices on each AGVs. ArUco diamond markers are used as landmarks. Each AGV calculates its own global pose after the detection of the landmarks and also detects the other AGVs in the environment. The goal of task is autonomous localization of multiple robots and also avoiding collisions with other robots in the environment and to reach to their virtual positions. The proposed control is verified using OptiTrack motion capture system providing ground truth in the experimental results.

The paper proposes a novel combination of nonlinear continuous motion control algorithm for robots or AGVs which uses an on-board actuated camera, data fusion (proprioceptive sensors and exteroceptive sensors) and the decision on the direction of perception realized based on behaviour trees to cope with limited computing and sensory resources. In the paper [1] authors proposed actuated camera devices, but it required different hardware configuration for leader and follower robots. The limitation of using different hardware for leader-follower robots is overcome by using proposed hardware [2] by authors which supports any type of robot or AGV’s.

Fiducial markers to which ArUco belongs have become very popular for camera-based positioning. They are often used for comparative studies, e.g. [3,4] and also used in pose estimation experiments [5]. For example, ArUco markers were experimentally compared with other types of markers in [3]. It turned out that their advantages were: good position and orientation results, great detection rate, and low computational cost for single markers. However, they had such disadvantages as: sensitivity to smaller marker sizes and larger distances, computational cost scales with multiple markers.

Figure 1. Robots with rotating platform.

Figure 1. Robots with rotating platform.

ArUco markers are used in various fields of robotics. Some works on manipulators like [6,7] can be mentioned. Examples of papers on mobile robots in which such a markers were applied are [8,9,10]. Other systems where such markers are useful are flying robots [11,12,13,14,15,16,17]. Markers are also used in cooperative robotics and nuclear decommissioning [18] or to determine the position of a passively cooperating spacecraft [19].

The Extended Kalman Filter (EKF) is used for the localization of an indoor robot [20,21,22,23]. It is also possible to detect cyber attacks using an EKF as shown in [24]. Paper [25] proposes a positioning method for a Magnetic Encoder Guided Vehicle (MEGV) using an EKF. It can also be noted that the neural network EKF is proposed to linearize the output signals [26] or to model unknown disturbances and improve the robot state transition model [27].

The Robot Operating System (ROS) is installed on the AGVs to manage high-level controls. The efficacy of the proposed solution is confirmed through a series of experiments. This research contributes to the advancement of AGV technology and its potential applications in various fields.

The considerations presented in Subsections 2–4 pertain to algorithms operating within the onboard systems of robots based on locally available data. For simplicity, authors have omitted the robot indices. However, it should be noted that they appear N times, corresponding to the number of mobile robots. Only in Subsections 5 and 6 authors introduces indices in the variables to denote the robot numbers, as the algorithm discussed there accounts for the interactions between agents.

An important value of this work is the fact that the presented theoretical considerations were supported by research carried out using a real experiment. Therefore, it can be concluded that the proposed approach to the issues under consideration has undoubted value in laboratory practice involving multi-agent systems. The behavior tree is explained in Section 2. One degree of freedom rotating platform is described in Section 3. Detection of ArUco markers, pose calculation, averaging of ArUco markers, and relative pose between the devices are explained in Section 4. The EKF method is presented in Section 5. All control methods for rotating platforms, collision avoidance techniques, and the kinematic model of robots are explained in Section 6. The methodology of the experiments is explained in Section 7. The experimental results are given in Section 8. The conclusion is discussed in Section 9.

2. Behavior Tree

Behavior trees (BT) are used in computer science, control systems, robotics, and especially in video games. BT is a mathematical model of task execution and the main advantage of BT is to perform complex tasks divided into simple subtasks. The graphical representation of BT is a directed tree consisting of the root node, control nodes, and leaf nodes.

Figure 2 represents the BT for the robot’s rotating platform, which gives the freedom to observe the environment irrespective of the movement of the robot’s mobile platform. Each robot has its own behavior tree. The Python library [28] is used to implement the BT in ROS. The topmost block in Figure 2 represents the root that generates a "tick" signal that propagates through the tree. After the root, the "Sequence 1" node, first sends the tick to the "Look towards Landmarks", the same action node is responsible for moving the rotating platform to look towards the landmarks on the walls for 5 seconds. Control details of the rotating platform are given in Section 6. When the functioning of the block is completed, it sends "SUCCESS" back to the same sequence node, otherwise, it sends "RUNNING". After receiving the "SUCCESS", the next block which is a "Fallback" node is executed. A fallback node works as an "OR" function. When any block below a fallback block gives a "FAILURE" then the other block is executed. The "Sequence 2" node and "Sequence 3" node are under the "Fallback" node. If another robot is detected in the environment in the camera range then the action node "Robot Detected = 1" under the sequence node will give a "SUCCESS" and then the next action node "Look towards Closest Robot" will be executed. The action node "Look towards Closest Robot" will reorient the rotating platform towards the closest robot in the environment around the observing robot. The "Sequence 2" node will send the "FAILURE" to the "Fallback" node when no robot is detected nearby, and then "Sequence 3" is executed, first, it verifies with the action node "Robot Detected = 0". When no robot is detected, the action node "Robot Detected = 0" will give "SUCCESS" and then the "Look straight" node will get a tick, and then the robot will look toward the center of the x-axis of the mobile platform for 5 seconds.

3. Devices for Multi-Agent Systems

The authors have used a one-degree-of-freedom rotating platform designed for multi-agent systems. The device rotates independently from the movement of the mobile platforms of the robot, which is one of its main advantages. Each device was installed on each agent. Figure 3 shows the exploded view of the device. All of its elements were 3D printed. The authors have uploaded the Standard Triangle Language (.stl) file containing models of [29] of the parts. The Fused Deposition Modeling (FDM) 3D printer is used to print these (.stl) files. The PLA (Polylactic Acid) material is used for the 3D printing.

Each device has a total of 10 ArUco markers pasted on it. Part 1 as seen in Figure 3 is of hexagonal shape which has 6 ArUco markers over it of 5x5 dictionary of 5 cm size each and part 3 is of cuboidal shape has 4 ArUco markers over it of 5x5 dictionary of 2.5 cm size. Table 1 shows the mechanical and electronic parts used in the devices.

The dynamixel motor operates from 9 V to 12 V. The motor has a resolution of 0.29 degrees with precise control of position with 1024 steps. As a result, approximately from 0 to 300 degrees [30] comes with feedback. A library [31] is used to connect Arduino Mega to the dynamixel motor. The IC mentioned in Table 1 is used to convert a half-duplex communication to full-duplex communication. The whole device is used as a ROS node, the library [32] is responsible for using the device as a ROS node.

4. ArUco Markers

ArUco markers are squared-shaped fiducial markers and the main advantage is that a single marker is enough to calculate the pose of the camera. A calibrated camera is required to extract the correct pose from the ArUco markers. The distortion matrix D and the camera matrix K of the calibrated camera are as follows:

$$D=\left[\begin{array}{ccccc}k1& k2& c1& c2& k3\end{array}\right],$$

(1)

$$\begin{array}{c}\hfill K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right].\end{array}$$

(2)

The Intel RealSense cameras are precalibrated, and the distortion matrix (D) and the camera matrix (K) are taken from the ROS topic of the camera information [33].

4.1. Pose Calculation of ArUco Marker

ArUco markers on the devices have some translation distances from the central axis. To get the center point of one device from another device, virtual rotation and translation are required. OpenCV library [34] for ArUco marker generates the translation ${\overrightarrow{t}}_{vec}$, and rotation ${\overrightarrow{r}}_{vec}$ vectors for each marker when it comes into the camera view frame. Figure 4 shows the representations of the axes between the camera and the ArUco marker. The rotation matrix and the translation matrix are calculated from the translation vector ${\overrightarrow{t}}_{vec}$, and rotation vector ${\overrightarrow{r}}_{vec}$.

The rotation angle ${\theta }_v$ of the rotation vector ${\overrightarrow{r}}_{vec}^n$ is:

$${\theta }_v=\left|\right|{\overrightarrow{r}}_{vec}^n\left|\right|.$$

(3)

The rotation vector ${\overrightarrow{r}}_{vec}^n$ is normalized as:

$$u={\displaystyle \frac{{\overrightarrow{r}}_{vec}^n}{{\theta }_v}}.$$

(4)

The skew-symmetric matrix $K_v$ :

$$K_v=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right].$$

(5)

The rotation matrix $R_{CT}^n$ is calculated as:

$$R_{CT}^n=cos{\theta }_{v}I+(1-cos{\theta }_v)u{u}^T+sin{\theta }_v{K}_v,$$

(6)

where I is the identity matrix. The OpenCV library has a build-in function (Rodrigues) [35] to calculate the directly from rotation vector (${\overrightarrow{r}}_{vec}$) to the rotation matrix (camera frame relative to the marker tag) $R_{CT}^n$. Now rotation matrix $R_{CT}^n$ is transformed as follows to change of frames.

$$R_{TC}^n=R_{CT}^{nT}.$$

(7)

To get the center of another device from one device, rotation of the ArUco marker is required before virtual translation towards the center. The aim of the rotation virtually rotate the XY plane of the ArUco marker to make it parallel to the XY plane of the camera. Rotation matrix $R_{TC}^n$ from Equation (7) is used to calculate the Euler angles (${\varphi }_R^n$, ${\theta }_R^n$, ${\psi }_R^n$) where ${\varphi }_R^n$, ${\theta }_R^n$ and ${\psi }_R^n$ represents the roll, pitch and yaw of the $n^{th}$ marker detected in the camera frame.

The rotation matrix $R_y^n\left({\theta }_R^n\right)$ to rotate about the y-axis of the ArUco marker is written as:

$$\begin{array}{c}\hfill R_y^n\left({\theta }_R^n\right)=\left[\begin{array}{ccc}cos{\theta }_R^n& 0& sin{\theta }_R^n\\ 0& 1& 0\\ -sin{\theta }_R^n& 0& cos{\theta }_R^n\end{array}\right].\end{array}$$

(8)

The new rotation matrix $\acute{R_{TC}^n}$ is calculated by multiplying $R_y^n\left({\theta }_R^n\right)$ from Equation (8) and rotation matrix $R_{TC}^n$ from Equation (7):

$$\acute{R_{TC}^n}=R_y^n\left({\theta }_R^n\right)R_{TC}^n.$$

(9)

Now, the distance matrix $\left({\overrightarrow{t}}_{dist}\right)$ is added to translation (${\overrightarrow{t}}_{vec}$) vector as follows:

$${\overrightarrow{\acute{t}}}_{vec}^n={\overrightarrow{t}}_{vec}^n+{\overrightarrow{t}}_{dist},$$

(10)

where ${\overrightarrow{t}}_{dist}={\left[\begin{array}{ccc}{d}_x^n& d_y^n& d_z^n\end{array}]\right.}^T$, $d_x^n=0$, $d_y^n=0$ in every case and $d_z$ is 4.808 cm when the marker is detected on hexagonal shape (3D printed Part 1) or $d_z$ is 1.5 cm when marker is detected on cubical shape (3D printed Part 3).

• The translation matrix ${\overrightarrow{t}}_c^{tn}$ is calculated as follows:

$${\overrightarrow{t}}_c^{tn}=-\acute{R_{TC}^n}{\overrightarrow{\acute{t}}}_{vec}^n.$$

(11)

where $\acute{R_{TC}^n}$ is taken from Equation (9) and ${\overrightarrow{\acute{t}}}_{vec}^n$ is from Equation (10). The transpose of ${\overrightarrow{t}}_c^{tn}$ is as follows:

$${({\overrightarrow{t}}_c^{tn})}^T=\left[\begin{array}{ccc}{x}_m^n& y_m^n& z_m^n\end{array}\right],$$

(12)

where $x_m^n$, $y_m^n$, and $z_m^n$ represent the relative distances from the center of one device to another.

4.2. Averaging of ArUco Marker Measurments

When one device detects another device in the environment, in general more than one ArUco marker is detected in an image frame. All 10 markers on a device should theoretically give the exact center point of the device after virtual rotation and translation, but practically the markers give slightly different measurements. The authors have used the averaging method to obtain one measurement from different markers. Upon detection of the ArUco marker, the OpenCV library [34] also gives the corner points of each marker. Figure 5 shows the corner points of the ArUco markers.

The Shoelace formula [36] is used to calculate the pixel area of each marker $a^n$ which is written as:

$$\begin{array}{cc}\hfill a^n={\displaystyle \frac{1}{2}}|& c_{x_1}^n{c}_{y_2}^n+c_{x_2}^n{c}_{y_3}^n+c_{x_3}^n{c}_{y_4}^n+c_{x_4}^n{c}_{y_1}^n-c_{x_1}^n{c}_{y_4}^n-c_{x_2}^n{c}_{y_1}^n-c_{x_3}^n{c}_{y_2}^n-c_{x_4}^n{c}_{y_3}^n|\hfill \end{array}$$

(13)

where $c{x}_1,...,c{x}_4$ are the x-coordinates of the corner points and $c{y}_1,...,c{y}_4$ represents for y-coordinates of the ArUco marker.

• The total area A is calculated as follows:

$$A=\sum _{n=1}^{N_m}{a}^n,$$

(14)

Average translation matrix $\overline{{\overrightarrow{t}}_c^t}$ is calculated as follows:

$$\overline{{\overrightarrow{t}}_c^t}={\displaystyle \frac{1}{A}}\sum _{n=1}^{N_m}{a}^n{\overrightarrow{t}}_c^{tn},$$

(15)

where A is taken from Equation (14), $a^n$ is from Equation (13) and ${\overrightarrow{t}}_c^{tn}$ is from Equation (11).

$${\overline{{\overrightarrow{t}}_c^t}}^T=\left[\begin{array}{ccc}{x}_m^{avg}& y_m^{avg}& z_m^{avg}\end{array}\right],$$

(16)

where $x_m^{avg}$, $y_m^{avg}$, and $z_m^{avg}$ are the average relative distances from the center of one device to another.

4.3. Relative Pose of One Device to Another

Relative pose of one device to another one is written as follows:

$$D_{pi}={\left[d_{xi}{d}_{yi}{d}_{\theta i}\right]}^{\top }$$

(17)

where i represents the $i^{th}$ device in the environment. As the coordinate frame of the camera and the ArUco marker are different, the distance in the global frame of the x coordinate between the devices will be the distance of the z-axis ($d_{xi}=z_m^{avg}$, from Equation (16)). Similarly, the global y-coordinate distance of the device is the x-coordinate of ArUco marker $d_{yi}=x_m^{avg}$ from the same Equation (16). The orientation of the device $d_{\theta i}$ is calculated as follows:

$$d_{\theta i}={\theta }_R^n+{\theta }_{Mi}+{\theta }_{Oi}.$$

(18)

where ${\theta }_R^n$ is the pitch angle calculated from Equation (7), the marker with the largest area $a^n$ is considered, ${\theta }_{Mi}$ of dynamixel servo motor which is taken from servo feedback. The angle of the observer ${\theta }_{Oi}$ depends on the relative positions between the devices. Figure 6 shows the top view of the rotating device. Suppose IDs 1 to 6 are pasted in the hexagonal part of the rotating device as shown in Figure 6. Now another observer rotating device is just behind the rotating device placed in Region 1 and looks forward to the center of the device, and it detects the ID 1 on another device then ${\theta }_{Oi}=0$ rad as the pose of the ArUco marker also gives the pose of the device itself. In another case where the observer rotating device is in Region 2 and detects ID 2 on another device, ${\theta }_{Oi}=-1.0472$ rad is added to correct the pose of another detected device. Similarly in another situation ${\theta }_{Oi}$ is as follows:

• ${\theta }_{Oi}=-2.094$ rad in Region 3.
• ${\theta }_{Oi}=-3.141$ rad when the pitch angle (${\theta }_R^n$) is positive for the detected marker (ID4) in Region 4.
• ${\theta }_{Oi}=3.141$ rad when the pitch angle (${\theta }_R^n$) is negative of the detected marker (ID4) in Region 4.
• ${\theta }_{Oi}=2.094$ rad in Region 5.
• ${\theta }_{Oi}=1.047$ rad in Region 6.

4.4. Pose From Landmarks

Figure 7a shows the image of the ArUco Diamond marker which is used as a landmark in the environment. ArUco Diamond marker structure has 4 markers with size 3 x 3 squares [37,38]. The authors have chosen ArUco Diamond markers as landmarks as the extracted pose information is the average of four ArUco markers on the diamond structure which is more stable as compared to single ArUco markers. Figure 7b shows the environment with 5 landmarks. Different IDs of ArUco markers are selected to differentiate between markers.

Each landmark in the environment gives the robot its global pose after modification in extracted pose from the detected ArUco diamond in the frame. Figure 4 also shows the axis representations between the camera and the ArUco Diamond markers, as they are the same as those of the single ArUco marker. The rotation vector (${\overrightarrow{r}}_{{vec}^{ln}}$) and the translation vector are (${\overrightarrow{t}}_{{vec}^{ln}}$) of the ArUco Diamond marker extracted by the OpenCV library [38] once detected in the camera frame where $ln$ denotes the landmark number ($ln=1,2,...5$).

The rotation matrix ($R_{CL}^{ln}$) is calculated from (${\overrightarrow{r}}_{{vec}^{ln}}$) using the built-in function (Rodrigues) [35] and after translation ($R_{LC}^{ln}$) the rotating matrix is written as:

$$R_{LC}^{ln}={R_{CL}^{ln}}^T,$$

(19)

The new translation matrix (${\overrightarrow{\acute{t}}}_{vec}^{ln}$) is calculated by adding some translation distance in the y-axis of the markers which is expressed as follows:

$${\overrightarrow{\acute{t}}}_{vec}^{ln}={\overrightarrow{t}}_{vec}^{ln}+{\overrightarrow{L}}_{dist},$$

(20)

where ${\overrightarrow{L}}_{dist}=[l_{ln},0,0]$, the value of $l_{ln}$ are given in Section 8.

The translation matrix ${\overrightarrow{t}}_c^{ln}$ is calculated as follows:

$${\overrightarrow{t}}_c^l=-R_{LC}^{ln}{\overrightarrow{\acute{t}}}_{vec}^{ln}.$$

(21)

The vector ${\overrightarrow{t}}_c^{l^T}$ represents the distance from the landmarks and is written as:

$${\overrightarrow{t}}_c^{l^T}=\left[\begin{array}{ccc}{x}_l^{ln}& y_l^{ln}& z_l^{ln}\end{array}\right],$$

(22)

The rotation matrix $R_{LC}^{ln}$ from Equation (19) is used to calculate the Euler angles (${\varphi }_L^{ln}$, ${\theta }_L^{ln}$, ${\psi }_L^{ln}$) where ${\varphi }_L^{ln}$, ${\theta }_L^{ln}$ and ${\psi }_L^{ln}$ represents the roll, pitch, and yaw of the $l{n}^{th}$ landmark detected in the camera frame. The global pose of the robot in the environment is written:

$$P=\left[\begin{array}{ccc}{z}_l^{ln}& x_l^{ln}& {\theta }_L^{ln}\end{array}\right],$$

(23)

5. Extended Kalman Filter

The aim of the EKF is to localize the robot in the environment using the data coming from the encoder and the global pose from the landmarks. EKF model is taken from the paper [39]. The state vector of the system of EKF is written as:

$$X_r={\left[\begin{array}{ccccc}{x}_r& y_r& {\theta }_r& v_r& {\omega }_r\end{array}\right]}^{\top },$$

(24)

where r represents the robot $r^{th}$ and $(x_r,y_r,{\theta }_r)$ represents the pose expressed in the global coordinate frame. Linear velocity and angular velocity are represented as $v_r$ and ${\omega }_r$, respectively.

The discrete-time model of the system $X_r\left(k\right)={\left[x_r\left(k\right)y_r\left(k\right){\theta }_r\left(k\right)v_r\left(k\right){\omega }_r\left(k\right)\right]}^{\top }$ is written as follows:

$$X_r\left(k\right)=\left[\begin{array}{c}{x}_r(k-1)+v_r\left(k\right)▵tcos{\theta }_r(k-1)\\ y_r(k-1)+v_r\left(k\right)▵tsin{\theta }_r(k-1)\\ {\theta }_r(k-1)+{\omega }_r\left(k\right)▵t\\ v_r(k-1)\\ {\omega }_r(k-1)\end{array}\right],$$

(25)

The state model Equation (26) and measurement model Equation (27) of the filter are as follows:

$$X_r\left(k\right)=f_r\left(X_r(k-1)\right)+w_r(k-1).$$

(26)

$$z_r\left(k\right)=h_r\left(X_r\left(k\right)\right)+{\upsilon }_r\left(k\right),$$

(27)

where $f_r$ represents the nonlinear system and $h_r$ represents the measurement model. The $g{w}_r(k-1)$ represents the Gaussian noise of the dynamic system and $g{\upsilon }_r\left(k\right)$ represents the Gaussian noise of the measurement. Both noise $g{w}_r(k-1)$ and $g{\upsilon }_r\left(k\right)$ are zero mean Gaussian noise and associated with covariance matrices are $Q_r\left(k\right)$ and $R_r\left(k\right)$ as:

$$g{w}_r\left(k\right)\sim N(0,Q_r\left(k\right)),$$

(28)

$$g{\upsilon }_r\left(k\right)\sim N(0,R_r\left(k\right)),$$

(29)

The nonlinear function $f_r$ and $h_r$ is to be linearized to obtain the linearized model of the system. A Jacobian matrix of $f_r$ and $h_r$ at the operating point is calculated as:

$$F_r\left(k\right)={\displaystyle \frac{\partial f_r\left(X\right)}{\partial X_r}}{|}_{{\widehat{X}}_r{k}^{-}}$$

(30)

$$F_r\left(k\right)=\left[\begin{array}{ccccc}1& 0& -v_r\left(k\right)▵tsin{\theta }_r\left(k\right)& ▵tcos{\theta }_r\left(k\right)& 0\\ 0& 1& v_r\left(k\right)▵tcos{\theta }_r\left(k\right)& ▵tsin{\theta }_r\left(k\right)& 0\\ 0& 0& 1& 0& ▵t\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right].$$

(31)

The prediction equations are as follows:

$${\widehat{X}}_r{\left(k\right)}^{-}=f_r\left({\widehat{X}}_r{(k-1)}^{+}\right),$$

(32)

$$P_r{\left(k\right)}^{-}=F_r\left(k\right)P_r(k-1)F_r{\left(k\right)}^T+Q_r\left(k\right).$$

(33)

The update equations are as follows:

$$K_r\left(k\right)=P_r{\left(k\right)}^{-}{H}_r{\left(k\right)}^T{(H_r\left(k\right)P_r{\left(k\right)}^{-}{H}_r^T+R_r\left(k\right))}^{-1},$$

(34)

$${\widehat{X}}_r{\left(k\right)}^{+}={\widehat{X}}_r{\left(k\right)}^{-}+K_r\left(k\right)(z_r\left(k\right)-h_r\left({\widehat{X}}_r{\left(k\right)}^{-}\right)),$$

(35)

$$P_r{\left(k\right)}^{+}=(I-K_r\left(k\right)H_r\left(k\right))P_r{\left(k\right)}^{-}.$$

(36)

where $K_r\left(k\right)$ represents the Kalman gain, ${\widehat{X}}_r{\left(k\right)}^{-},P_r{\left(k\right)}^{-},{\widehat{X}}_r{\left(k\right)}^{+}$ and $P_r{\left(k\right)}^{+}$ are estimates of the a priori and a posteriori state.

5.1. Measurement model

The Subsection 5.1.1 explains the calculation of linear ($v_{wheelsr}$) and angular (${\omega }_{wheelsr}$) velocities of the robot from the wheel encoders. The other subsection 5.1.2 explains the use of the global pose in the EKF of the robot after the detection of the landmark.

5.1.1. Data from wheel encoders

All robots are equipped with encoders on their motors. The wheel angular velocities of the left and right wheels are expressed as:

$$w{a}_{Lr}=(\varphi L_{cr}-\varphi L_{pr})/▵t_{\varphi }L·\pi /180,$$

(37)

$$w{a}_{Rr}=(\varphi R_{cr}-\varphi R_{pr})/▵t_{\varphi }R·\pi /180,$$

(38)

where angular velocities (rad/s) are $w{a}_{Lr}$ of the left wheel and $w{a}_{Rr}$ of the right wheel of the r-th robot. The current encoder ticks and previously saved encoder ticks for the left wheel and right wheel are $\varphi L_{cr}$, $\varphi L_{pr}$ and $\varphi R_{cr}$ and $\varphi R_{pr}$ respectively. The time between the two consecutive readings of the readings of the left and right wheels are $▵t_{\varphi }L$ and $▵t_{\varphi }R$, respectively.

The linear velocities of wheels is calculated by multiplying wheel radius $R_{wheelr}$ to Equations (37) and (38) as:

$$V_{Lr}=w{a}_{Lr}·R_{wheelr},$$

(39)

$$V_{Rr}=w{a}_{Rr}·R_{wheelr},$$

(40)

Equations (39) and (40) are used to calculate the linear and angular velocities of robots as follows:

$$v_{wheelsr}=(V_{Rr}+V_{Lr})/2,$$

(41)

$${\omega }_{wheelsr}=(V_{Rr}-V_{Lr})/L_r,$$

(42)

where the robot’s length of the wheel separation is $L_r$. $v_{wheelsr}$ and ${\omega }_{wheelsr}$ are the linear and angular velocities of the robots calculated from the readings of the wheels encoders.

The measurement matrix $z_{encodersr}\left(k\right)$ is written as:

$$z_{encodersr}\left(k\right)=\left[\begin{array}{cc}{v}_{wheelsr}& {\omega }_{wheelsr}\end{array}\right],$$

(43)

The $H_{encodersr}\left(k\right)$ matrix is as follows:

$$H_{encodersr}\left(k\right)=\left[\begin{array}{ccccc}0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right].$$

(44)

5.1.2. Landmark Data

After the detection of the landmarks by the robot camera $P_r$ matrix from Equation (23) gives the global pose of the robot. The direct angle from the landmark is not considered as the robots are equipped with the rotating platform. The calculation of the angle from the landmark ${L_{\theta }}_r$ is written as:

$$L_{\theta r}={\theta }_r+{\theta }_{Lr}^{ln}+{\theta }_{Mir}+{\theta }_{rc}$$

(45)

where ${\theta }_r$ is the robot orientation, ${\theta }_L^{ln}$ is the angle generated from the landmark detection and is the third component taken from Equation (23) and ${\theta }_{Mi}$ is the angle of the rotating platform of the robot. ${\theta }_{rc}$ depends on the specific position of the robot. The values of ${\theta }_{rc}$ are given in Section 8.

$z_{landmarkr}\left(k\right)$ of the landmark is as follows:

$$z_{landmarkr}\left(k\right)=\left[\begin{array}{ccc}{z}_{lr}^{ln}& x_{lr}^{ln}& L_{\theta r}\end{array}\right],$$

(46)

where as $z_{lr}^{ln}$, $x_{lr}^{ln}$ are taken from Equation (23).

The $H_{landmarkr}\left(k\right)$ matrix is as follows:

$$H_{landmarkr}\left(k\right)=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right].$$

(47)

6. Controls

6.1. Controls of Rotating Device

The rotating device tracks the ArUco markers on another rotating device by the PID controller. The main aim is to minimize the distance between the center of the camera and the ArUco marker as minimum as possible. The distance between the center of the ArUco marker and the camera center ($x_m^n$) is taken from Equation (12) or (16) depending on the case of the ArUco marker.

$$u_{pidr}\left(t\right)=K_{Pr}{e}_r\left(t\right)+K_{ir}\int e_r\left(t\right)d{t}_r+K_{dr}{\displaystyle \frac{d{e}_r}{dtr}},$$

(48)

Where $u_{pidr}\left(t\right)$ is the control signal generated for a time t. $K_{Pr}$, $K_{ir}$ and $K_{dr}$ are the proportional gain, integral gain, and derivative gain parameters, $d{e}_r$ is the change in error, $d{t}_r$ is the change in time, $e_r\left(t\right)$ is expressed as follows:

$$e_r\left(t\right)=S{P}_r-{\theta }_{Mi}.$$

(49)

where $S{P}_r$ is the set point that is set to zero as the camera should point in the middle of the ArUco marker. ${\theta }_{Mi}$ is the current angle of the rotating platform of the robot.

6.2. Collision Avoidance

The collision avoidance used in this paper is the idea presented by [40] and later widely used in multi-agent systems [41,42,43,44,45].

The robot r avoids collisions with all other robots j, $\forall j\in \{1,\dots ,N\}\setminus r$ using Artificial Potential Function (APF). All robots are surrounded by APFs that raise to infinity near the robot’s/obstacle’s boundary, $r_j$ - radius of the robot (j - number of the robot/obstacle), and monotonically decrease to zero at some radius $R_j$, $R_j>r_j$.

The APF [44] is written as :

$$B_{aij}\left(l_{rj}\right)=\left\{\begin{array}{ccc}0& \mathrm{for}& l_{rj}<r_j\\ e^{{\displaystyle \frac{l_{rj}-r_j}{l_{rj}-R_j}}}& \mathrm{for}& r_j\le l_{rj}<R_j\\ 0& \mathrm{for}& l_{rj}\ge R_j\end{array}\right.,$$

(50)

the above function gives the output $B_{arj}\left(l_{rj}\right)\in 〈0,1〉$.

The Euclidean length $l_{rj}=∥{\left[x_j{y}_j\right]}^T-{\left[x_r{y}_r\right]}^T∥$ is used to calculate the distance between r-th robot and the j-th obstacle (or robot) .

To scale the function (50) within the range $\left.〈0,\infty \right)$, the following equation is used:

$$V_{arj}\left(l_{rj}\right)={\displaystyle \frac{B_{arj}\left(l_{rj}\right)}{1-B_{arj}\left(l_{rj}\right)}}.$$

(51)

The scalar function $V_{arj}\left(l_{rj}\right)$, is used in control to generate collision avoidance behaviors by using its spatial partial derivatives. The approach outlined above is also used to avoid collisions with a static obstacle with the center located at point $(x_o,y_o)$, with radius $r_o$ and APF vanishing at distance $R_o$ from the center. In this case, all the above equations remain valid if o is substituted in place of j.

6.3. Control of Mobile Platform

Each moving robot has its own virtual robot which acts as a leader robot. The kinematic model of the $r^{th}$ mobile robots $R_r$ is written as:

$$\left[\begin{array}{c}\dot{x_r}\\ \dot{y_r}\\ \dot{{\theta }_r}\end{array}\right]=\left[\begin{array}{cc}cos{\theta }_r& 0\\ sin{\theta }_r& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_r\\ {\omega }_r\end{array}\right],$$

(52)

where $x_r$, $y_r$, ${\theta }_r$ represent the robot’s pose in the global reference frame. ${\left[\begin{array}{cc}{v}_r& {\omega }_r\end{array}\right]}^{\top }$ represents the control vector where the linear velocity is $v_r$ and ${\omega }_r$ represents the controls of the angular velocity of the robot, respectively.

The following global quantities to zero:

$$\begin{array}{cc}\hfill p_{rx}& =x_{rd}-x_r\hfill \\ \hfill p_{ry}& =y_{rd}-y_r\hfill \\ \hfill p_{r\theta }& ={\theta }_{rd}-{\theta }_r.\hfill \end{array}$$

(53)

where $x_{rd}$, $y_{rd}$ and ${\theta }_{rd}$ represent the components of the pose of the virtual robot ($R_{or}$). $x_r$, $y_r$ and ${\theta }_r$ and are taken from the robot EKF system, Equation (35). Now the system errors with respect to the robot’s fixed frame is as follows:

$$\left[\begin{array}{c}{e}_{rx}\\ e_{ry}\\ e_{r\theta }\end{array}\right]=\left[\begin{array}{ccc}cos\left({\theta }_r\right)& sin\left({\theta }_r\right)& 0\\ -sin\left({\theta }_r\right)& cos\left({\theta }_r\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{rx}\\ p_{ry}\\ p_{r\theta }\end{array}\right].$$

(54)

Adding the collision avoidance components

$${\displaystyle \frac{\partial V_{arj}}{\partial p_{rx}}}={\displaystyle \frac{\partial V_{arj}}{\partial x_r}}{\displaystyle \frac{\partial x_r}{\partial p_{rx}}}=-{\displaystyle \frac{\partial V_{arj}}{\partial x_r}},$$

(55)

$${\displaystyle \frac{\partial V_{arj}}{\partial p_{ry}}}={\displaystyle \frac{\partial V_{arj}}{\partial y_i}}{\displaystyle \frac{\partial y_r}{\partial p_{ry}}}=-{\displaystyle \frac{\partial V_{arj}}{\partial y_r}}.$$

(56)

The gradient of the APF can be expressed with respect to the local coordinate frame fixed to the r-th robot:

$$\left[\begin{array}{c}{\displaystyle \frac{\partial V_{arj}}{\partial e_{rx}}}\\ {\displaystyle \frac{\partial V_{arj}}{\partial e_{ry}}}\end{array}\right]=\left[\begin{array}{cc}cos{\theta }_r& -sin{\theta }_r\\ sin{\theta }_r& cos{\theta }_r\end{array}\right]\left[\begin{array}{c}{\displaystyle \frac{\partial V_{arj}}{\partial p_{rx}}}\\ {\displaystyle \frac{\partial V_{arj}}{\partial p_{ry}}}\end{array}\right].$$

(57)

Using equations (54) and (57) the system errors with collision avoidance components is written as:

$$E_{rx}=p_{rx}cos\left({\theta }_r\right)+p_{ry}sin\left({\theta }_r\right)+\sum _{j=1,j\ne r}^N{\displaystyle \frac{\partial V_{arj}}{\partial e_{rx}}},$$

(58)

$$E_{ry}=-p_{rx}sin\left({\theta }_r\right)+p_{ry}cos\left({\theta }_r\right)+\sum _{j=1,j\ne r}^N{\displaystyle \frac{\partial V_{arj}}{\partial e_{ry}}},$$

(59)

$$e_{r\theta }=p_{r\theta }.$$

(60)

The above equations is re-written as follows:

$$E_{rx}=e_{rx}+\sum _{j=1,j\ne r}^N{\displaystyle \frac{\partial V_{arj}}{\partial e_{rx}}},$$

(61)

$$E_{ry}=e_{ry}+\sum _{j=1,j\ne r}^N{\displaystyle \frac{\partial V_{arj}}{\partial e_{ry}}}.$$

(62)

Due to its simplicity and its effectiveness, the trajectory tracking algorithm is taken from [46]. The control for i-th robot is written as:

$$\begin{array}{c}{v}_r=v_{or}cos{e}_{r\theta }+k_1{E}_{rx}\\ {\omega }_r={\omega }_{or}+k_2\mathrm{sgn}\left(v_{r-1}\right)E_{ry}+k_3{e}_{r\theta },\end{array}$$

(63)

where ${\left[v_{or}{\omega }_{or}\right]}^{\top }$ is the desired velocities vector of the virtual robot where $v_0$ is the linear velocity and ${\omega }_0$ is the angular velocity. $k_1$, $k_2$ and $k_3$ are constant parameters greater then zero and function $\mathrm{sgn}(•)$ is defined as follows:

$$\mathrm{sgn}\left(\xi \right)=\left\{\begin{array}{ccc}-1& \mathrm{for}& \xi <0\\ 0& \mathrm{for}& \xi =0\\ 1& \mathrm{for}& \xi >0\end{array}\right..$$

The detailed analysis of the properties of the control for a single platform without collision avoidance is presented in paper [47].

7. Methodology

The experiment is carried out with four real robots which are shown in Figure 1. All robots are equipped with an Intel NUC where ROS Noetic is deployed. Each robot has its own controller in its own NUC system which works totally independently from each other. Low-level controllers are present in each robot which works as ROS nodes. The low-level controller subscribes to robot linear and angular velocities and publishes the ticks from the encoder. All robots are connected to one WiFi router to have access to roscore. One PC is also connected to the same WiFi router to launch roscore. The robots in the environment work as slaves to the ROS master. In the environment, landmarks are pasted on the vertical surface as seen in Figure 7b.

Two robots worked as static obstacles in the experiment. The other two robots move in the environment to avoid the static robots in the environment and try to reach their virtual robot’s localization. With the use of behavior trees moving robots watch towards the landmarks for 5 seconds and watch towards the closest robot or in a straight moving direction.

8. Experimental Results

The experiment video is uploaded on online platform [https://youtu.be/rDvpCEM7DmE]. Robots Numbers 1 and 2 are moving in the environment. Both the Robots number 1 and 2 have to follow their virtual robot 1 and virtual robot 2 respectively. The experiment is done for 60 seconds. The starting pose and end pose of virtual robot 1 are $(2.75,2.0,0)$ m and $(3.35,2.0,0)$ m, respectively, and for virtual robot 2 they are $(1.24,1.8,3.14)$ m and $(0.65,1.8,3.14)$ m. The virtual robots have to generate a straight path. The desired velocities for both virtual robots are $0.01$ m/s. The values of $r_j$ and $R_j$ of APF in the collision avoidance section 6.2 are considered to be $0.2$ m, $0.6$ m, respectively. $l_{ln}$ of Equation (20) is considered as when the landmark number 1 in the environment detected by the robot in Figure (7b) then $l_1=0$, for other landmarks $l_2=0.786$, $l_3=1.916$, $l_4=3.046$ and $l_5=3.996$. $l_1,l_2,l_3,l_4,l_5$ in Equation (20) are the distance between the landmark 1 to other markers in the environment. ${\theta }_{rc}$ of Equation (45) is considered in two cases. In case 1 is when the robot moves toward the positive x-axis of the environment and the rotating platform is watching toward the landmarks on the wall which are on the positive y-axis of the environment, then ${\theta }_{rc}=1.5708$. In case 2 is when the robot moves toward a negative x-axis and the rotating platform observes the landmarks on the wall that are on the positive y-axis of the environment, then ${\theta }_{rc}=-1.5708$. The constant parameters of the controller Equation (63) are considered as $k_1=0.25$, $k_2=2$ and $k_3=2$.

Figure 8a shows the path for virtual robots 1 and 2, the calculated EKF path of robots 1 and 2, and the Opti-track path for all four robots. Figure 8b shows the $xy$-plot of robot 1 with static robot 4 with activation region. Figure 8c shows the $xy$-plot of robot 2 with virtual robot 2. Figure 8d shows the $xy$-plot of all robots with activation regions. The collision avoidance components $D_v$ and $D_y$ of robot 1 are shown in Figure 8e and Figure 8f respectively. The global errors of the robot, 1 on the x axis, y axis, and with $\theta$ with respect to time, are shown in Figure 8g, Figure 8h and Figure 8i respectively. The linear and angular velocities of robot 1 with respect to time are shown in Figure 8j and Figure 8k.

The collision avoidance components $D_v$ and $D_y$ of robot 2 are shown in Figure 8l and Figure 8m respectively. The global errors of the robot 2 in x axis, y axis and with $\theta$ with respect to time is shown in Figure 8n, Figure 8o and Figure 8p respectively. The linear and angular velocities of robot 2 with respect to time are shown in Figure 8q and Figure 8r.

Figure 8t and Figure 8v show the detections of other robots in the environment by robot 1 and robot 2 respectively. The detection of landmarks by robot 1 and robot 2 is shown in Figure 8s and Figure 8u.

The ground truth from the OptiTrack is used to evaluate the results from on-board measurements. The OptiTrack paths for Robots 1 and 2 are in the range of approximately $\pm 0.2$m with the EKF path generated for robots as seen in Figure 8a. It should be seen as a goal that was reached in this work.

9. Conclusions

The paper shows the algorithm that uses open-source rotating devices installed on all robots. The movement of the devices is independent of the movement of the robot’s mobile platform. Rotating devices are capable of detecting other rotating devices installed on robots in the environment and are also capable of detecting landmarks in the environment. Behavior trees are used to change the orientation of the camera towards the location of landmarks and towards the closest robot or to look straight towards the moving axis. An experiment was performed with four robots, two robots were stationary and worked as static obstacles in the environment. The other two robots were following their virtual robots’ trajectories and avoid collisions with other robots present in the environment. An Extended Kalman Filter is used to localize the robots from the data coming from encoders and from landmarks. The global pose of the robot is calculated and updated after the detection of the landmarks. The proposed research method was verified by an actual experiment, which confirms its usefulness in laboratory practice related to testing multi-agent systems.