Guidance, Navigation and Control System for Multi-Robot Network in Monitoring and Inspection Operations

Abstract

This work focuses on the challenges associated with autonomous robot guidance, navigation, and control in multi-robot systems. This study provides an affordable solution by utilizing a group of small unmanned ground vehicles and quadrotors that collaborate on monitoring and inspection missions. The proposed system utilizes a potential fields path planning algorithm to allow a robot to track a moving target while avoiding obstacles in a dynamic environment. To achieve the required performance and provide robust tracking against wind disturbances, a backstepping controller is used to solve the essential stability problem and ensure that each robot follows the specified path asymptotically. Furthermore, the performance is also compared with a proportional-integral-derivative (PID) controller to ensure the superiority of the control system. The system combines a low-cost inertial measurement unit (IMU), a GNSS receiver, and a barometer for UAVs to generate a navigation solution (position, velocity, and attitude estimations), which is then used in the guidance and control algorithms. A similar solution is used for UGVs by integrating the IMU, a GNSS receiver, and encoders. Non-linear complementary filters integrate the measurements in the navigation system to produce high bandwidth estimates of the state of each robotic platform. Experimental results of several scenarios are discussed to prove the effectiveness of the approach.

1. Introduction

Today, unmanned vehicles (UVs) are used to carry out a number of tasks in order to automate processes, complete tasks faster and more accurately, and ensure the safety of humans [1]. Moreover, UVs are often used in smart cities [2,3]. Due to this, researchers have developed ground and aerial robots that were designed to operate under uncertain conditions. Often, it is a burden for a single robot to accomplish a task effectively [4]. For this reason, a group of small, inexpensive robots can work together in a network, each with its own capabilities, to accomplish a common goal [5].

The applications of robotic systems in clean energy systems are continuously developing since there is a global tendency to shift to renewable energy sources such as solar, wind, and hydro-energy systems [6]. However, for these systems to perform well, they must be inspected and monitored on a regular basis [7]. For instance, in [8], a drone with thermal imaging is used for solar panels defect detection. In addition, a climbing robot was utilized to inspect and maintain a solar power plant [9]. To maximize the efficiency of such operations, several studies used multi-robot networks to develop technology to analyze the normal operation and failure of solar modules by attaching optical and thermal infrared sensors to unmanned aerial vehicles (UAVs) [10,11]. A similar study used multi-drone equipped with vision and LiDAR sensors for global inspection, guiding climbing robots in analyzing structure parts using non-destructive inspection methods and locally repairing smaller defects in wind turbines [12]. With the advancement of IoT technology, the use of multi-robot systems to collect data from a network of wireless sensors has received increased attention and has grown dramatically in applications such as agriculture [4], railway track monitoring [13], and search and rescue [14].

To achieve a trusted level of autonomy in multi-robot system operations, it should include the following major components: guidance, navigation, and control [15]. The guidance system is in charge of providing the robot with a planned trajectory that allows it to travel from its starting point to the desired location while avoiding obstacles and tracking a target. In a multi-robot system, each agent must cooperate and coordinate with the others in order to achieve real-time cooperative navigation and free-collision path planning [5,16,17]. The navigation system is used to determine the location, orientation, and velocities of each robot in the multi-robot system at any desired time [18,19,20]. The control system provides the correct forces or torques to achieve the guidance goals. Backstepping (BS), sliding mode control (SMC), feedback linearization, PID, optimal and robust control, learning-based control, and other techniques were used to solve the stabilization control problem and trajectory control for a quadrotor and a mobile robot [21,22]. Backstepping control is considered one of the well-developed nonlinear control approaches that can stabilize dynamic systems while handling uncertainties to achieve the required performance [23,24].

In recent years, swarming robot dynamics and control have been active research topics. The difficulty of validating these algorithms in actual tests has been a barrier to more frequent and widespread use. Many researchers focused their efforts on developing a stable guidance, navigation and control (GNC) system to address this issue. In [25], robust navigation algorithms for multi-agent fixed-wing aircraft are presented. These algorithms are based on adaptive moving mesh partial differential equations controlled by the free energy heat flow equation, and they are experimentally validated using LQR controllers and multi-scale moving point guidance. A similar project used a PID controller and a waypoint-based guidance system to evaluate an INS/ GNSS navigation system [26]. An affordable GNC solution with a visual-based navigation system was tested on a group of small unmanned ground vehicles and quadrotors to achieve a common goal [27]. In a practical marine environment, a hybrid framework for guidance and navigation of swarms of unmanned surface vehicles (USVs) that combines two layers of offline planning and online planning was applied [28].

This work developed a complete system architecture for a guidance, navigation, and control solution to enable small UAVs and UGVs in monitoring and inspection applications. To solve the essential stability problem and ensure that each robot follows the specified path asymptotically, a backstepping controller is used and discussed in detail. A guidance algorithm for generating a flight trajectory based on a potential field method is also described, allowing a robot to track a moving target and avoid obstacles in a dynamic environment. To generate a navigation solution (position, velocity, and attitude (orientation) estimations), the system combines a low-cost inertial measurement unit (IMU), a GNSS receiver, and a barometer for UAVs, which is then used in the guidance and control algorithms. For UGVs, a similar solution is used by integrating IMU, a GNSS receiver, and encoders. The measurements in the navigation system are integrated by non-linear complementary filters that produce high-bandwidth estimates of the state of each robotic platform. What distinguishes the developed system is its direct implementation of its algorithms in embedded systems with minimal programming cost. This work provides users with a reliable and viable GNC structure, which is supported by several practical experiments conducted both internally and externally for life applications. The developed GNC system was tested using a true reference motion capture system to demonstrate the efficiency of the integrated system in terms of stability, accuracy, and maneuverability. Several outdoor tasks were carried out to evaluate the monitoring and inspection process, rather than a single robot. These operations demonstrate the navigation system’s ability to precisely locate the aerial and ground robots.

The current study is structured as follows: Section 2 provides an overview of the dynamic model of each robot. Section 3 discusses the adopted backstepping controller for a quadrotor and a ground robot. A motion planning algorithm is developed and discussed in Section 4. The navigation system for each robotic platform is adopted for accurate state estimations. Section 6 illustrates indoor and outdoor experiments of multi-robot missions. Furthermore, this section discusses the experimental results. Finally, concluding remarks and future research directions are summarized in Section 7.

2. System Modeling and Description

The kinematics and dynamics models of the quadrotor and wheeled robot, in addition to their relevance in the design of robust controllers, can be helpful in understanding the stability nature and operations under uncertain conditions.

2.1. Quadrotor Model

There has been much explanation and study of the mathematical model of the quadrotor in the literature [24,29,30], and we will summarize the general model here. It is established through:

$$\begin{array}{cc}\hfill \dot{\mathit{v}}=& \frac{1}{m}(-\mathit{T}+{\mathit{f}}_d+{\mathit{f}}_w)+g{\mathit{R}}^T{\mathit{u}}_3-\mathit{\omega }\times \mathit{v}\hfill \\ \hfill \dot{\mathit{p}}=& \mathit{R}\mathit{v}\hfill \\ \hfill \dot{\mathit{\omega }}=& {\mathit{J}}^{-1}[\mathit{\tau }-S\left(\omega \right)\mathit{J}\mathit{\omega }]\hfill \\ \hfill \dot{\mathit{R}}=& \mathit{R}S\left(\mathit{\omega }\right)\hfill \end{array}$$

(1)

As shown in Figure 1, the vectors of linear velocity and position are determined by $\mathit{v}={[u,v,w]}^T$ and $\mathit{p}={[x,y,z]}^T$, respectively. Moreover, $\mathit{\omega }={[\rho ,q,r]}^T$ is the angular velocity vector. Any vector $\mathit{z}$ in body frame $\mathit{B}$ can be translated into inertial frame $\mathit{I}$ by a rotation matrix $\mathit{R}$, and any vector $\mathit{z}$ can be represented by the skew symmetric matrix $S\left(\mathit{z}\right)$. The gravity constant g is represented in inertial frame by the vector ${\mathit{u}}_3={[0,0,1]}^T$, and the moment of inertia is given by the matrix $\mathit{J}$ and the mass by m. The terms $\mathsf{\omega }\times \mathit{v}$, ${\mathit{f}}_d$, and ${\mathit{f}}_w$ in Equation (1) are the gyroscopic effects, the drag forces, and the wind disturbances, respectively. Furthermore, the thrust force $\mathit{T}$ and moments $\mathsf{\tau }$ on each body axis are expressed as follows:

$$\begin{array}{cc}\hfill \mathit{T}=& {\left[\begin{array}{ccc}0& 0& c_t({\omega }_{m1}^2+{\omega }_{m2}^2+{\omega }_{m3}^2+{\omega }_{m4}^2)\end{array}\right]}^T\hfill \\ \hfill \mathsf{\tau }=& \left[\begin{array}{c}d{c}_t(-{\omega }_{m1}^2+{\omega }_{m2}^2+{\omega }_{m3}^2-{\omega }_{m4}^2)\\ d{c}_t({\omega }_{m1}^2+{\omega }_{m2}^2-{\omega }_{m3}^2-{\omega }_{m4}^2)\\ c_q({\omega }_{m1}^2-{\omega }_{m2}^2+{\omega }_{m3}^2-{\omega }_{m4}^2)\end{array}\right]\hfill \\ \hfill +& \left[\begin{array}{c}{J}_{m}q\frac{\pi }{30}(-{\omega }_{m1}+{\omega }_{m2}-{\omega }_{m3}+{\omega }_{m4})\\ J_{m}p\frac{\pi }{30}({\omega }_{m1}-{\omega }_{m2}+{\omega }_{m3}-{\omega }_{m4})\\ 0\end{array}\right]\hfill \end{array}$$

(2)

Figure 1. Multi-robot coordinates and control structure. Subscript “d” means “desired”.

The torque and thrust coefficients of the rotor (motor+propeller) are $c_q$ and $c_t$, respectively; $J_m$ is the rotor’s inertia and d is the distance between the center of the rotor to the quadrotor’s center of mass.

2.2. Wheeled Robot Model

In the literature, the kinematic model of two-wheeled differential-drive robots is subjected to non-holonomic constraints, assuming that the wheels do not skid [24,31]. In this case, the robot motion is described as follows:

$$\begin{array}{cc}\hfill v=& \mathit{R}\left(\psi \right){\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\psi }\end{array}\right]}^T=r\frac{{\omega }_R+{\omega }_L}{2}\hfill \\ \hfill \omega =& r\frac{{\omega }_R-{\omega }_L}{d}\hfill \\ \hfill \mathit{R}\left(\psi \right)=& \left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\hfill \end{array}$$

(3)

In the above equation, $\psi$ and $\omega$ represent the angle of rotation and angular velocity around the robot’s z-axis, respectively, and v is the linear velocity; r and d are the radius of the robot’s wheel and the distance between the robot’s rear wheels, respectively; ${\omega }_R$ and ${\omega }_L$ are the angular speeds of the right and the left wheels, respectively. In Figure 1, the body frame of the robot moves relative to an inertial frame O. Therefore, the kinematic equations in Cartesian coordinates are given as follows:

$$\begin{array}{cc}\hfill & \dot{x}=vcos\psi \hfill \\ \hfill & \dot{y}=vsin\psi \hfill \\ \hfill & \dot{\psi }=\omega \hfill \end{array}$$

(4)

3. Backstepping Controller

3.1. Quadrotor Control

The backstepping (BS) controller is used in this work because of its capacity to solve the quadrotor’s essential stability problem. Most crucially, the control system ensures that the quadrotor’s position follows the specified path asymptotically [23,24]. The BS control system integrates both translation and rotation dynamics within a single control.

The adopted control law generates one thrust T and three torques $\mathsf{\tau }=$${[{\tau }_{\varphi },{\tau }_{\theta },{\tau }_{\psi }]}^T$ and a timing law for the path parameter $\gamma \left(t\right)$ that guarantees convergence of the quadrotor’s position $\mathbf{P}\left(t\right)$ to the desired path ${\mathbf{P}}_d\left(\gamma \right)$, which is a function of a virtual arc $\gamma$. The control structures for the thrust and the torque vector are given by Equations (5) and (6), respectively:

$$\begin{array}{cc}\hfill T_d=& m\left|\right|k_1^2{k}_2{\mathit{e}}_2+k_1\dot{\sigma }({\mathit{e}}_1+g{\mathit{u}}_3-{\ddot{\mathit{p}}}_d\left|\right|=m\left|\right|{\mathit{t}}_d\left|\right|,\hfill \\ & T_d>0\hfill \\ \hfill {\mathit{r}}_{3d}& =\frac{{\mathit{t}}_d}{\left|\right|{\mathit{t}}_d\left|\right|}\hfill \\ \hfill T=& {\mathit{r}}_{3d}^T{\mathit{r}}_3{T}_d\hfill \end{array}$$

(5)

where $k_1$ and $k_2$ are positive gains, and m is the mass of the quadrotor; ${\mathit{r}}_3$ is the third column of the rotation matrix ($\mathit{R}$), and it indicates the direction of the quadrotor’s z body-axis. As a result, if ${\mathit{r}}_3$ equals the desired thrust direction (${\mathit{r}}_{3d}$), the thrust force ($\mathit{T}$) will be equal to the desired thrust force (${\mathit{T}}_d$). Based on the desired thrust value ($T_d$) and thrust direction (${\mathit{r}}_{3d}$) in Equation (5), the thrust control law (T) is generated. Furthermore, the torque control law ($\tau \left(t\right)$) is calculated in Equation (6) according to the angular acceleration (${\dot{\mathsf{\omega }}}_c$). Moreover, an arbitrary function (${\dot{\omega }}_{3c}$) is used to define the dynamics of yaw angle.

$$\begin{array}{cc}\hfill {\dot{\omega }}_{3c}=& -l_2({\omega }_3-{\omega }_{3d}+l_1{\mathit{r}}_{2d}^T{\mathit{r}}_1)+{\dot{\omega }}_{3d}-l_1\frac{d}{dt}\left({\mathit{r}}_{2d}^T{\mathit{r}}_1\right)\hfill \\ \hfill {\dot{\mathsf{\omega }}}_c=& S\left({\mathit{u}}_3\right)(-k_4{\mathit{e}}_4-{\mathit{R}}^T{\mathit{r}}_{3d}+k_{3}S{\left({\mathit{u}}_3\right)}^2({\dot{\mathit{R}}}^T{\mathit{r}}_{3d}\hfill \\ \hfill & +{\mathit{R}}^T{\dot{\mathit{r}}}_{3d})+\frac{1}{m{k}_1}S{\left({\mathit{u}}_3\right)}^2({\dot{T}}_d{\mathit{R}}^T{\mathit{e}}_2+T_d{\mathit{R}}^T{\dot{\mathit{e}}}_2))\hfill \\ \hfill & +{\dot{\mathit{R}}}^T{\mathit{R}}_d{\mathsf{\omega }}_d+{\mathit{R}}^T{\mathit{R}}_d{\dot{\mathsf{\omega }}}_d+{\left[00{\dot{\omega }}_{3c}\right]}^T\hfill \\ \hfill \mathsf{\tau }=& \mathit{J}{\dot{\mathsf{\omega }}}_c+S\left(\mathsf{\omega }\right)\mathit{J}\mathsf{\omega }\hfill \end{array}$$

(6)

where $l_1$, $l_2$, $k_3$, and $k_4$ are positive gains; ${\mathit{r}}_1$ and ${\mathit{r}}_2$ are the first and second columns of the rotation matrix ($\mathit{R}$), respectively. The skew symmetric matrix is represented by $\mathit{S}(.)$; ${\mathit{R}}_d$ and ${\mathsf{\omega }}_d$ denote the desired rotation matrix and quadrotor’s desired angular speed, respectively. Based on the BS controller, four error vectors (${\mathit{e}}_1$ to ${\mathit{e}}_4$) are defined. Those error vectors and derivatives are expressed as follows:

$$\begin{array}{cc}\hfill {\mathit{e}}_1=& \mathit{p}-{\mathit{p}}_d\left(\gamma \right),{\dot{\mathit{e}}}_1=\mathit{R}\mathit{v}-{\dot{\mathit{p}}}_d,\hfill \\ & {\ddot{\mathit{e}}}_1=-\frac{T}{m}\mathit{R}{\mathit{u}}_3+g{\mathit{u}}_3-{\ddot{\mathit{p}}}_d\hfill \\ \hfill {\mathit{e}}_2=& \sigma \left({\mathit{e}}_1\right)+\frac{1}{k_1}{\dot{\mathit{e}}}_1,{\dot{\mathit{e}}}_2=\dot{\sigma }\left({\mathit{e}}_1\right)+\frac{1}{k_1}{\ddot{\mathit{e}}}_1\hfill \\ \hfill {\mathit{e}}_3=& {\mathit{r}}_3-{\mathit{r}}_{3d}\hfill \\ \hfill {\mathit{e}}_4=& -k_{3}S{\left({\mathit{u}}_3\right)}^2{\mathit{R}}^T{\mathit{r}}_{3d}+S\left({\mathit{u}}_3\right)(\mathsf{\omega }-{\mathit{R}}^T{\mathit{R}}_d{\mathsf{\omega }}_d)\hfill \\ \hfill & -\frac{T_d}{m{k}_1}S{\left({\mathit{u}}_3\right)}^2{\mathit{R}}^T{\mathit{e}}_2\hfill \end{array}$$

(7)

where ${\mathit{p}}_d$ and ${\mathit{v}}_d$ are the desired positions and velocities of the quadrotor, respectively. A time-dependent position error vector (${\mathit{e}}_1$) can influence the initial position of the vehicle significantly. As a result, the control structure is distinguished by the fact that it is not directly related to the time factor associated with the first error. In other words, thrust (T) and torque vector ($\mathsf{\tau }$) are not directly linked to (${\mathit{e}}_1$). In addition to the position error, the other errors are regulated by the saturation function ($\sigma$) of (${\mathit{e}}_1$), which allows for bounded actuation. A detailed discussion of the stability proofs for these control laws can be found in [23].

It is essential for the control algorithm to keep a well-defined control law to ensure that the vehicle follows the path. In order to achieve this condition, we have a timing law, which is obtained by adding the second time-derivative of the path parameter $\gamma$:

$$\begin{array}{cc}\hfill \ddot{\gamma }=& -k_{\gamma }\sigma (\dot{\gamma }-{\dot{\gamma }}_d)+{\mathit{u}}_1^T{\mathit{R}}^T(k_1^2{k}_2{\mathit{e}}_2+k_1\dot{\sigma }\left({\mathit{e}}_1\right)-\dot{\gamma }{\mathit{p}}_d^{{}^{″}})\hfill \\ \hfill {\ddot{\mathit{p}}}_d=& {\mathit{p}}_d^{{}^{″}}{\dot{\gamma }}^2+{\mathit{p}}_d^{{}^{\prime }}\ddot{\gamma }\hfill \end{array}$$

(8)

where ${\mathit{p}}_d^{{}^{\prime }}$ and ${\mathit{p}}_d^{{}^{″}}$ are the first and second partial derivatives of the desired path, respectively. The control law in Equation (8) drives $\dot{\gamma }$ to ${\dot{\gamma }}_d$, which is calculated based on the path’s geometry. Any vector in tangent frame $\left\{T\right\}$ can be represented in inertial frame $\left\{I\right\}$ by the rotation matrix ${\mathit{R}}^T$ at any points where $p^{{}^{″}}\ne 0$. For ease of use, most variables, which were previously defined, are listed in Table 1.

Table 1. Parameters used in backstepping controller of a quadrotor. Subscript “d” means “desired”.

Parameter	Description	Parameter	Description
$k_1,k_2,k_3,k_4,l_1,l_2,k_{\gamma },P_{max}$	controller gains	$\sigma \left(\mathit{x}\right)$	$P_{max}\frac{\mathit{x}}{1+\left|\right|\mathit{x}\left|\right|}$ Saturation function
$\mathit{p}$	${[x,y,z]}^T$	${\mathit{p}}_d$	Desired path position as a function of $\gamma$
$\mathit{v}$	${[u,v,w]}^T$	${\mathit{p}}_d^{\prime },{\mathit{p}}_d^{″}$	First and second partial derivatives of ${\mathit{p}}_d$
$\mathsf{\omega }$	${[P,q,r]}^T$	${\mathit{u}}_1,{\mathit{u}}_2,{\mathit{u}}_3$	${[1,0,0]}^T,{[0,1,0]}^T,{[0,0,1]}^T$
$\mathit{R}$	Rotation matrix of Euler angles $(\varphi ,\theta ,\psi )$	$S\left(\mathit{x}\right)$	Skew symmetric matrix

3.2. Ground Robot Control

A backstepping controller is adopted to accurately regulate the actuators that drive the robot from its current position $\mathit{p}={\left[xy\psi \right]}^T$ to the desired position ${\mathit{p}}_d={\left[x_d{y}_d{\psi }_d\right]}^T$. The aim of the control is to minimize the following error:

$$\mathsf{ϵ}=\mathit{R}\left(\mathsf{\psi }\right)({\mathit{p}}_{\mathit{d}}-\mathit{p})=\left[\begin{array}{c}(x_d-x)cos\psi +(y_d-y)sin\psi \\ -(x_d-x)sin\psi +(y_d-y)cos\psi \\ {\psi }_d-\psi \end{array}\right]$$

(9)

The error derivatives after substitutions from Equations (4) and (9) can be simplified as follows:

$$\dot{\mathsf{ϵ}}=\left[\begin{array}{c}\omega {ϵ}_2-v+v_{d}cos{ϵ}_3\\ -\omega {ϵ}_1+v_{d}sin{ϵ}_3\\ {\omega }_d-\omega \end{array}\right]$$

(10)

As discussed in [24], two control inputs (i.e., $v_c$ and ${\omega }_c$) are considered. Hence, the control law is implemented as follows:

$$\left[\begin{array}{c}{v}_c\\ {\omega }_c\end{array}\right]=\left[\begin{array}{c}{k}_1{ϵ}_1\left(\frac{F}{v_d}\right)+v_{d}cos{ϵ}_3\\ {\omega }_d-\left(\frac{\tau }{cos{ϵ}_3}\right)\end{array}\right]$$

(11)

where $v_d$ and ${\omega }_d$ are the desired linear velocity and angular velocity of the robot, respectively. The variables F and $\tau$ are calculated as follows:

$$\begin{array}{cc}\hfill F=& \frac{v_d}{k_5+1}\hfill \\ \hfill \tau =& -k_1(\delta sin{ϵ}_3-\delta \varpi )+\eta \sigma \hfill \\ \hfill \varpi =& \eta (k_{3}F{ϵ}_1^3+k_2{v}_d{ϵ}_1^2{ϵ}_2+\left(\frac{k_2{F}^2+k_5^{2}F}{v_d^2}\right){ϵ}_1)\hfill \\ \hfill \delta =& \frac{v_d+k_{5}F}{v_d}\hfill \\ \hfill \eta =& \frac{v_d}{v_d+k_{5}F}\hfill \\ \hfill \sigma =& \delta (-k_2{k}_5{v}_d-3k_{3}F){ϵ}_1^3+2\delta k_2{k}_5{v}_d{ϵ}_2^2{ϵ}_1\hfill \\ & +k_3\delta (-2F-3k_{5}F){ϵ}_1^2{ϵ}_2+\delta k_2{v}_d^2{ϵ}_1^{2}sin{ϵ}_3\hfill \\ & +\delta k_3\left(\frac{-F^3}{v_d^4}\right){ϵ}_1+\delta k_2{k}_5\left(\frac{-v_{d}F+F^2}{v_d^3}\right){ϵ}_2\hfill \end{array}$$

(12)

where $k_1,k_2,k_3,k_4$, and $k_5$ are the controller gains.

4. Motion Planning for Multi-Robot System

The purpose of this section is to introduce a new UV motion planning system based on potential forces. This system helps the robot (i.e., ground and aerial robots) follow a moving target within a dynamic environment containing obstacles. To achieve the desired target, an individual trajectory is generated for each robot, which has access to all the state information in one processor. In Section 3, the proposed control structure for each robot ensures that the tracking error will converge to zero, where tracking error of the ith robot is measured as:

$$\begin{array}{cc}\hfill & {\mathit{e}}_{ti}={\mathit{q}}_{di}-{\mathit{q}}_i\hfill \end{array}$$

(13)

where ${\mathit{q}}_{di}$ is a vector of the robot desired trajectory, and ${\mathit{q}}_i$ is its position with respect the inertial frame. The robot will continue to move towards its destination as long as there are no obstacles in its path. Alternately, the robot’s predefined path must be modified by feeding small variations in its motions. Essentially, the proposed approach generates a dynamic trajectory that allows the robot to track the target and avoid obstacles by generating attractive and repulsive potential forces.

4.1. Potential Attractive Force

The attractive force model is defined by the relative position and velocity vectors of two robots as follows:

$$\begin{array}{cc}\hfill & {\mathit{f}}_A=A_1\left(\mathit{p}\right)+A_2\left(\mathit{v}\right)\hfill \\ \hfill & A_1\left(\mathit{p}\right)=2k_p{\mathsf{\delta }}_{\mathit{p}}\hfill \\ \hfill & A_2\left(\mathit{v}\right)=2k_v{\mathsf{\delta }}_{\mathit{v}}\hfill \end{array}$$

(14)

The attractive force model includes two force components, $A_1$ and $A_2$, as shown in Figure 2. The first component, $A1$, propels the robot toward the target, shortening the distance between them. The second component, $A_2$, keeps the robot moves at the same velocity as the target. The relative position between the robot and the target is given by ${\mathsf{\delta }}_{\mathit{p}}$, and the relative velocity is given by ${\mathsf{\delta }}_{\mathit{v}}$. Each force component can be tuned correctly by the gains $k_p$ and $k_v$. Figure 2 illustrates the relationship between the attractive force components and the relative position and velocity of the robot to the target in a 2-dimensional space. Normally, this system is applied to a ground robot that is following another robot, and the concept can also be extended to cover UAVs in the third dimension. The attractive force for each motion direction (i.e., x, y, and z) can be given as follows:

$$\begin{array}{c}\hfill f_x=2k_{p_x}{\delta }_x+2k_{v_x}{\delta }_{v_x}\\ \hfill f_y=2k_{p_y}{\delta }_y+2k_{v_y}{\delta }_{v_y}\\ \hfill f_z=2k_{p_z}{\delta }_z+2k_{v_z}{\delta }_{v_z}\end{array}$$

(15)

Figure 2. Attractive force in 2D space.

4.2. Potential Repulsive Force

When a robot encounters an obstacle, it will also be provided with an extended potential force to detour and avoid collision. Considering $p_{obs}$ and $v_{obs}$ are the position and the velocity of an obstacle which can be obtained on-line, then the relative velocity between the robot and the obstacle is ${\mathsf{ϵ}}_{\mathit{v}}=\mathit{v}-{\mathit{v}}_{obs}$, and the relative position is ${\mathsf{ϵ}}_{\mathit{p}}={\mathit{p}}_{obs}-\mathit{p}$, as shown in Figure 3. Then, the value of ${\mathsf{ϵ}}_{\mathit{v}}$ in the direction from the robot to the obstacle is given by:

$$\begin{array}{cc}\hfill v_{RO}=& {\mathsf{ϵ}}_{\mathit{v}}^T{\mathit{n}}_{RO}\hfill \\ \hfill {\mathit{n}}_{RO}=& \frac{{\mathsf{ϵ}}_{\mathit{p}}}{\left|\right|{\mathsf{ϵ}}_{\mathit{p}}\left|\right|}\hfill \end{array}$$

(16)

where ${\mathit{n}}_{RO}$ is a unit vector pointing from the robot to the obstacle. As shown in Figure 3, in the case where the robot is moving away from the obstacle as $v_{RO}\le 0$, no avoidance action is needed from the robot. In contrast, if $v_{RO}>0$, the robot requires a force to push it away from the obstacle. As a result, a model is used to generate appropriate repulsive forces. These forces are computed using the following function:

$$\begin{array}{cc}\hfill \mathit{r}=& {\mathit{r}}_1+{\mathit{r}}_2.\hfill \\ \hfill {\mathit{r}}_1=& \frac{-\eta }{{({\rho }_s-{\rho }_m)}^2}(1+\frac{v_{RO}}{a_{max}}){\mathit{n}}_{RO}\hfill \\ \hfill {\mathit{r}}_2=& \frac{\eta v_{RO}{v}_{RO\perp }}{{\rho }_s{a}_{max}{({\rho }_s-{\rho }_m)}^2}{\mathit{n}}_{RO\perp }\hfill \end{array}$$

(17)

Figure 3. Repulsive force in 2D space.

It can be seen in Figure 3 that ${\mathit{r}}_1$ is a force in the opposite direction of $v_{RO}{\mathit{n}}_{RO}$, which keeps the robot away from the obstacle. Moreover, ${\mathit{r}}_2$ is in the same direction of the perpendicular vector $v_{RO\perp }{\mathit{n}}_{RO\perp }$ and acts as a steering force. Moreover, $\eta$ is a positive constant, ${\rho }_s$ is the shortest distance between the center of the robot and the center of the obstacle, and ${\rho }_m$ is the distance that the robot travels before it approaches zero, which is a function of a maximum deceleration $a_{max}$ and is given as follows:

$$\begin{array}{c}\hfill {\rho }_m=\frac{v_{RO}^2}{2a_{max}}.\end{array}$$

(18)

In order to make the repulsive force model valid for multiple obstacles $n_{obs}$, the following relation is established:

$$\begin{array}{c}\hfill \mathit{r}=\sum _{n=1}^{n_{obs}}{\mathit{r}}_i.\end{array}$$

(19)

Using Equations (14) and (17), the robot is subjected to an overall virtual force equal to the sum of the calculated attractive and repulsive forces as follows:

$$\begin{array}{c}\hfill {\mathit{f}}_{tot}=\mathit{f}+\mathit{r}.\end{array}$$

(20)

5. Navigation System and State Estimations

In real-time platforms, an estimation system is essential to compensate for noise and biases in sensors, and, in addition, to provide accurate attitude, position, and velocity information to the aerial and ground robots. One of the navigation solutions can be implemented with a typical inertial measurement unit (IMU) and a low-cost GNSS receiver for outdoor use or tracking systems for indoor use. This section discusses the design and implementation of attitude, position, and velocity estimations for both the ground robot and the quadrotor, addresses the lateral drifting, and improves the vertical position of the quadrotor platform.

5.1. Quadrotor State Estimations

Using a complementary filter, the attitude measurement is performed by fusing high-frequency gyroscope measurements with low-frequency magnetometer and accelerometer measurements. For the sake of direct implementation in embedded systems, the discrete quaternion representation of the attitude estimation is given as follows [32]:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\widehat{\mathit{q}}}_{k+1}\\ {\widehat{\mathit{b}}}_{k+1}\end{array}\right]=& \left[\begin{array}{cc}{\mathit{I}}_4& -\frac{T}{2}\mathsf{\Xi }\left({\mathit{q}}_k\right)\\ 0& {\mathit{I}}_3\end{array}\right]\left[\begin{array}{c}{\widehat{\mathit{q}}}_k\\ {\widehat{\mathit{b}}}_k\end{array}\right]\hfill \\ \hfill +& \left[\begin{array}{c}\frac{1}{2}\mathsf{\Xi }\left({\mathit{q}}_k\right){\mathit{k}}_{1q}\\ {\mathit{k}}_{2q}\end{array}\right]\sum _{i=1}^n({\mathit{y}}_{ik}\times {\widehat{\mathit{y}}}_{ik})\hfill \end{array}$$

(21)

where T as the sampling time interval and the sub-index k abbreviates the time instant $t=kT$; $\widehat{\mathit{q}}$ and $\widehat{\mathit{b}}$ are the estimated attitude in quaternion and the estimated gyro bias, respectively. In addition, the gains ${\mathit{k}}_{1q}$ and ${\mathit{k}}_{2q}$ are positive and tuned for best performance; ${\mathit{I}}_3$ and ${\mathit{I}}_4$ are $3\times 3$ and $4\times 4$ identity matrix, respectively. Furthermore, the term $({\mathit{y}}_{ik}\times {\widehat{\mathit{y}}}_{ik})$ represents the variation between the estimated inertial vector and the measured one in the body frame. In practice, two vectors are utilized, which are the earth’s gravity vector and the earth magnetic field vector, and can be measured in body frame by an accelerometer ($\mathit{a}$) and a magnetometer ($\mathit{m}$). Hence the term ${\sum }_{i=1}^n({\mathit{y}}_{ik}\times {\widehat{\mathit{y}}}_{ik})$ can be implemented as follows:

$$\begin{array}{cc}\hfill & \sum _{i=1}^2({\mathit{y}}_{ik}\times {\widehat{\mathit{y}}}_{ik})=\hfill \\ \hfill & {\mathit{k}}_a({\mathit{a}}_k\times \mathit{R}{\left({\mathit{q}}_k\right)}_k{\left[00g\right]}^T)+{\mathit{k}}_m({\mathit{m}}_k\times \mathit{R}\left({\mathit{q}}_k\right){\left[m_{xo}{m}_{yo}{m}_{z_o}\right]}^T)\hfill \end{array}$$

(22)

where the vectors ${\left[00g\right]}^T$ and ${\left[m_{xo}{m}_{yo}{m}_{z_o}\right]}^T$ are the earth’s gravity and magnetic field vectors, respectively, which are measured in inertial frame; $\mathit{R}\left({\mathit{q}}_k\right)$ is the quaternion rotation matrix.

This work also implements another complementary filter for accurate position and velocity estimations. Due to the low sampling rate and lack of accuracy, standalone GNSS systems are not reliable when they are used in high-dynamic systems such as quadrotors. Thus, GNSS data is integrated with accelerometer measurements by a complementary filter to provide continuous position and velocity estimations. For more reliable estimations in altitude, a barometer is used to provide the measurements of height. The position and velocity complementary filter is provided in discrete-time form as follows:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\widehat{\mathit{p}}}_{k+1}\\ {\widehat{\mathit{v}}}_{k+1}\\ {\widehat{\mathit{b}}}_{ak+1}\end{array}\right]=& \left[\begin{array}{ccc}\mathit{I}& T\mathit{I}& -\frac{T^2}{2}\mathit{R}\left({\mathit{q}}_k\right)\\ 0& \mathit{I}& -T\mathit{R}\left({\mathit{q}}_k\right)\\ 0& 0& \mathit{I}\end{array}\right]\left[\begin{array}{c}{\widehat{\mathit{p}}}_k\\ {\widehat{\mathit{v}}}_k\\ {\widehat{\mathit{b}}}_{ak}\end{array}\right]\hfill \\ \hfill +& \left[\begin{array}{c}\frac{T^2}{2}\mathit{I}\\ T\mathit{I}\\ 0\end{array}\right]\mathit{R}\left({\mathit{q}}_k\right)({\mathit{a}}_{mk}+{\left[00g\right]}^T)+\left[\begin{array}{c}{\mathit{k}}_p{\tilde{\mathit{p}}}_k\\ {\mathit{k}}_v{\tilde{\mathit{v}}}_k\\ {\mathit{k}}_b{\tilde{\mathit{v}}}_k\end{array}\right]\hfill \end{array}$$

(23)

where $\tilde{\mathit{p}}$ and $\tilde{\mathit{v}}$ are the errors in position and velocity, respectively, that represent the differences between the measured values (i.e., $\mathit{p}$ and $\mathit{v}$) and estimated ones (i.e., $\widehat{\mathit{p}}$ and $\widehat{\mathit{v}}$) at time point k; ${\widehat{\mathit{b}}}_a$ is the estimated accelerometer bias, and ${\mathit{k}}_p$, ${\mathit{k}}_v$, and ${\mathit{k}}_b$ are positive gains.

5.2. Ground Robot State Estimation

In a mobile robot navigating through an indoor environment, only the state in plane (i.e., x, y, and $\psi$) is needed assuming that the robot moves on a horizontal and flat surface. However, a robot’s motion could be significantly influenced by terrain characteristics when it is operating in an outdoor environment. Therefore, it can be oriented around the three axes of its reference frame in any way, resulting in six degrees of freedom (i.e., three positions and three orientations). Through a fusion of IMU, GNSS, and compass data, the work utilized the same technique as the quadrotor, which is discussed in the previous sub-section, to determine the localization and orientation of the ground robot. To enhance the estimations, the positions and orientation of the wheeled mobile robot is usually calculated based on an odometry according to measurements from motor encoders as follows [31]:

$$\begin{array}{c}\hfill \begin{array}{c}{x}_{k+1}=x_k+\delta s_{k}cos({\psi }_k+\frac{\delta {\psi }_k}{2})\\ y_{k+1}=y_k+\delta s_{k}sin({\psi }_k+\frac{\delta {\psi }_k}{2})\\ {\psi }_{k+1}={\psi }_k+\delta {\psi }_k\end{array}\end{array}$$

(24)

At each sampling time k, $\delta s_k$ and $\delta {\psi }_k$ are the increments in distance and angle.

6. Experimental Setup and Results

In this section, the proposed guidance, navigation, and control (GNC) system is implemented and evaluated by a multi-robot system. The performance of the adopted GNC system was evaluated in two different environments. The first environment was indoor and included three robotics platforms (i.e., two quadrotors and one ground robot), as shown in Figure 4. The second environment was implemented outdoor with another three drones and two car-like robots, as shown in Figure 4. An indoor system demonstrated robotic platforms with a motion capture system that provided real-time position and orientation. Based on several retro-reflective markers mounted on each robot, the motion capture system ran a software that defined rigid bodies. Each quadrotor executed the control algorithms with an onboard Intel Aero computer and the ground robot with a Raspberry Pi 3. In this study, quadrotor drones and ground robots were used to evaluate the performance of the backstepping controllers with the presence of disturbances. In addition in this stage, the drone’s ability to track a ground robot and avoid obstacles was tested in several experiments. Moreover, multi-robot missions were performed outdoors in different scenarios for surveying and monitoring, as illustrated in Figure 5. A Pixhawk autopilot was used to test the proposed control and navigation algorithms. The guidance system and communication between robots were built on a Raspberry Pi using the Robotics Operating System (ROS).

Figure 4. Multi-robot systems: (a) indoor and (b) outdoor.

Figure 5. Surveying mission at the campus of the Hashemite University: (a) a photo from the drone’s camera and (b) planned automatic mission on a map using sequence of way-points.

6.1. Backstepping and PID Comparison

Two experiments were implemented on each robot platform (i.e., quadrotor and ground robot). Well tuned PID controllers were adopted in the first experiment and compared with a similar scenario in the second experiment in which backstepping controllers were implemented, as discussed in Section 3. The performance of each platform was analyzed by computing the mean squared error (MSE) in positions and velocities. The results are listed in Table 2 for a quadrotor and Table 3 for a ground robot. The tables show the superiority of the backstepping controlled quadrotor and ground robot over PID controllers, particularly in high-speed paths. Figure 6 illustrates how the BS-controlled quadrotor was correctly tracking the predefined circular-path, whereas the PID-controlled quadrotor was diverging from the high-speed path. A similar performance was demonstrated by a BS-controlled ground robot on the squared-path. Figure 7 and Figure 8 clearly show the better performance of the backstepping controller for the quadrotor and the ground robot, respectively.

Table 2. Mean squared errors (MSEs) for PID-controlled and BS-controlled quadrotors at different velocities.

Parameter	v = 1 m/s		v = 3 m/s		v = 4 m/s
	PID	BS	PID	BS	PID	BS
$MS{E}_{xy}$	0.1074	0.0996	0.3712	0.1021	0.4900	0.1160
$MS{E}_{v_{xy}}$	0.1889	0.1189	0.4503	0.0839	1.1752	0.4881
$MS{E}_z$	0.0707	0.0247	0.0475	0.0295	0.0754	0.0444
$MS{E}_{v_z}$	0.0544	0.0415	0.0865	0.0824	0.0944	0.0997

Table 3. Mean squared errors (MSEs) for PID-controlled and BS-controlled ground robots at different velocities.

Parameter	v = 1 m/s		v = 3 m/s		v = 4 m/s
	PID	BS	PID	BS	PID	BS
$MS{E}_{xy}$	0.0764	0.0198	0.2614	0.1106	0.4312	0.1995
$MS{E}_{v_{xy}}$	0.1796	0.0845	0.7109	0.3108	1.2450	0.6754

Figure 6. A comparison between PID and BS controllers for quadrotors and ground robots (speed = 3 m/s).

Figure 7. A comparison between PID and BS controllers for quadrotors.

Figure 8. A comparison between PID and BS controllers for ground robots.

Figure 9 shows the performance of quadrotors using both PID and BS controllers in an experiment on a squared-path with the addition of fan air flow as a disturbance, located at ($x=0,y=-1.5$ m) and directed toward the y axis. The PID-controlled quadrotor’s highest deviation from the reference route as a result of the disturbance was 42.4 cm, whereas the BS-controlled quadrotor’s highest position divergence was 5 cm. Therefore, based on the BS controller’s good results, it was implemented in ground robots and quadrotors for the best stability and tracking in multi-robot missions.

Figure 9. A comparison between PID and BS controllers due to disturbance.

6.2. GNC System Performance

The backstepping controller was used along with the navigation system in Section 5 and the guidance system in Section 4 for a full guidance, navigation, and control (GNC) structure to demonstrate the performance of the quadrotors and ground robot in a network of multi-robots.

An indoor experiment was conducted to demonstrate a mission with two drones and one ground robot in order to to validate the overall GNC system. In this mission, a main drone was assigned to follow a ground robot that was traveling in a 4 × 4 squared path and followed with a 1.2 radius circle. A traveling drone was used to travel to point (−1.5, −1.5, 1.2) m and hover for a while. This point is one of the squared path’s corners that the tracking drone will pass through. Figure 10 depicts how the tracking drone avoided colliding with the traveling drone. Furthermore, it avoided the traveling drone once more while the last was approaching it, as shown in Figure 10.

Figure 10. Multi-robot mission of two drones and one ground robot.

The position and velocity of the two drones and the ground robot are illustrated in Figure 11. The figure shows that the tracking drone was precisely following the ground robot (i.e., $MS{E}_{xy}=0.173$ m, $MS{E}_z=0.046$ m, and $MS{E}_{v_{x}y}=0.125$ m/s), despite the fact that the obstacle avoidance task increased the error between them. The figure also shows how the tracking drone safely avoided the traveling drone on the 20th and 38th seconds and returned back to the desired path.

Figure 11. Positions and velocities of two drones and one ground robot mission.

6.3. Outdoor Missions

Different missions were carried out in an outdoor environment to demonstrate the performance of a network of several drones and ground robots. The first mission scenario was carried out with three drones, one of which was assigned to lead the others on a predefined path, as illustrated in Figure 12. In this mission, three drones were launched from the same location, and the leading drone was tasked with following the predefined way-points while the other two drones followed it safely. Figure 13 illustrates the performance of the 140-s mission. The average lateral speed (i.e., in the xy direction) of each drone was approximately 10 m/s at different heights. The three drones completed the mission precisely and without any collisions, with the follower drones maintaining a nearly constant distance from the leading drone throughout the mission.

Figure 12. Three-drone mission with one leading the others on a predefined path: (a) four way-points on a map (b) the trajectories of the three drones.

Figure 13. Positions and velocities for the three drone mission.

In the second scenario, the three drones were programmed to survey an area of approximately 87,000 ${\mathrm{m}}^2$ at the same time. Each drone was assigned to survey a portion of the area and photograph various locations, as shown in Figure 14. The mission was completed in approximately 160 s, with 24 photos of the entire area being taken, as illustrated in Figure 15. For sake of comparison, the same task was assigned to a single drone. The mission took approximately 330 s to complete using one drone.

Figure 14. Three drones surveying a large area: (a) surveyed area on a map, and (b) the path of each drone with the position of each picture (P).

Figure 15. Positions and velocities for the three drone mission.

The final outdoor scenario involved two ground robots performing a monitoring task. As shown in Figure 16, each robot was assigned to follow a different path on campus at the same time. As shown in Figure 17, each robot was moving at a speed of 5 m/s along a path that was approximately 1770 m long for the first robot and 2390 m for the second robot. As a result, the first ground robot completed its assignment in 390 seconds, whereas the second ground robot completed the mission in 490 s, taking into account the time it took to launch to the first way-point and return to the starting point.

Figure 16. Two ground robots in monitoring task: (a) robot’s path on a map using a sequence of way-points, and (b) the performed path of each robot.

Figure 17. Positions and velocities for the two ground robots mission.

The goal of the last two tasks was to evaluate the monitoring and inspection process using a group of aerial and/or ground robots rather than a single robot. The previous operations demonstrated that using a group of robots speeds up and improves the efficiency of the work. The robots demonstrated the ability to track way-points without colliding or intersecting with one another. These two experiments also demonstrated the ability of the navigation system to precisely locate the aerial and ground robots.

7. Conclusions

This work demonstrated the practical implementation of light-computing nonlinear algorithms for a full guidance, navigation, and control system for a network of quadrotor drones and ground robots. For the best path tracking and position and orientation stability, a backstepping controller was used. In order to implement a robust navigation system for control and guidance systems, nonlinear complementary filters were used to accurately estimate attitudes, positions, and velocities. For optimal robot guidance, a potential field method was used for tracking and obstacle-free path planning. Several indoor and outdoor experiments were conducted to assess the effectiveness of the GNC system on multi-robot missions. The results demonstrated the advantages of the proposed system in monitoring and inspection applications. This work paves the way for future efforts to improve the navigation system for higher-level perception in order to recognize and classify detected objects/events and infer some of their attributes. Another interesting aspect of the research is the use of the proposed GNC on the scales of swarms of quadrotors and ground robots in a challenging environment.