Leader–Follower Formation and Disturbance Rejection Control for Omnidirectional Mobile Robots

: This paper proposes a distance-based formation control strategy with real-time disturbance rejection for omnidirectional mobile robots. The introduced control algorithm is designed such that the leader tracks a desired trajectory while the follower keeps a desired distance and formation angle concerning the leader. In the ﬁrst step, the evolution of distance and formation angle is obtained from a perturbed second-order dynamic model of the robot, aided by a general proportional integral observer (GPIO), added to estimate unwanted disturbances. Then, the control law is designed for both robots via the active disturbance rejection control (ADRC) methodology, which only depends on the position, distance, and orientation measurements. A numerical simulation compared with a robust controller exhibits the system’s behavior. Furthermore, a set of laboratory experiments is conducted to verify the performance of the proposed control system, where a motion capture system is used as a proof of concept. In this context, this is considered a previous step for further experimentation with onboard sensors.


Introduction
Formation control is a fundamental motion coordination problem in mobile robots [1]. It can be found in transportation [2], surveillance [3], search and rescue [4], logistics [5], material handling in manufacturing cells [6], mobile sensor networks [7], and area coverage [8]. The primary purpose of formation control is to track group trajectories while maintaining desired inter-robot spacing defined by relative positions, distances, or angles [9,10]. From a technical point of view, the formation control is classified into displacement-based and distance-based approaches [11]. In the former, neighboring agents' states are obtained from a global coordinate system [12], while in the latter, neighboring agents' states (such as the distance and bearing angle) are obtained from a local reference system [13] through inertial measurement units, LiDARs, cameras, or the mixing of onboard sensors [14].
The most basic formation consists of two robots and is commonly referred to as the leader-follower scheme [15][16][17], in which the leader robot is assigned to follow a predefined • An inter-robot dynamical model, dependent on the distance, heading angle, and orientation angles is proposed using dynamical models of leader and follower robots.
The resulting equations are rewritten as an inter-robot perturbed dynamical model, where the conveniently aggregated single perturbation contains viscous and Coulomb frictions, centripetal forces, and other unmodeled dynamics. • A general proportional integral observer (GPIO), as seen in [43], is proposed to estimate the aggregated perturbation. A formation control law, based on the active disturbance rejection control (ADRC) approach, is then defined for the leader and fol-lower robots using the position, distance, formation angle, and estimated perturbation. The approach becomes a robust setup ready to overcome unmodeled dynamics in real time. • Experimental work utilizing laboratory-scale omnidirectional mobile robots and supported with VICON © motion capture system verifies the accuracy of the parameters of the assumed dynamical models. It validates the efficacy of the proposed control.
The rest of the work is organized as follows. The problem is formally stated in Section 2. The development of the leader-follower relative dynamic model is derived in Section 3. The proposed formation control strategy is presented in Section 4. Section 5 describes the details of the performed numerical simulations and real-time hardware experiments. Finally, Section 6 concludes the work and offers insights into potential future work.

Problem Statement
Let N = {R F , R L } be a set of two agents, illustrated in Figure 1, where R F is the follower and R L is the leader, both described with the following second-order dynamics: where q s = x s y s θ s ∈ R 3 is the state vector, in which coordinates (x s , y s ) represent the position in the plane and θ s is the orientation with respect to the horizontal axis X.
Additionally, M s ∈ R 3×3 is the non-singular inertia matrix; B s ∈ R 3×3 is the non-singular transformation matrix that maps the motor torques to the mobile robot forces and torques; C s ∈ R 3×3 is the Coriolis matrix and centripetal forces; τ s = τ s 1 τ s 2 τ s 3 ∈ R 3 is the control input; F e s represents the external forces and torques vector; F v s are the viscous robot friction coefficients related to linear, lateral, and angular velocities; and finally, F c s are the Coulomb friction coefficients. Subscript s = {F, L} helps to distinguish between the follower and leader agent model. Note that the model given in (1) is obtained from the X − Y inertial reference frame. After some algebraic manipulations, (1) can be simplified to a perturbed model with total disturbance ψ s = −M −1 s [C sqs + F v sq s + F c s + F e s ] and matrices M s and B s being where m s is the mass of the robot, r s is the wheels' radius, δ = π 6 rad is the orientation of each wheel concerning its longitudinal axis, D is the length from the center of the agent to each of the wheels, I w s is the inertia of the wheel, and I 0 s is the robot inertia.
The first considered problem is the development of a dynamic model for a pair of omnidirectional mobile robots based on the distance and the formation angle between them, i.e.,η where d LF ∈ R + is the distance measured from the geometrical center of agent R F to the geometrical center of the agent R L with R + as the set of all positive real numbers; α LF ∈ R is the formation angle measured from the distance vector d LF to a local frame attached to the agent R F ; θ F is the orientation of the follower concerning the horizontal axis; τ L and τ F are the control inputs of the leader and follower, respectively. Once the model is obtained, a GPIO is proposed to estimate the unmodeled disturbances.
Hence, the second considered problem is the design of a control strategy such that: • The leader robot follows a desired trajectory, that is, lim x * y * θ * , with x * , y * , and θ * being the leader's desired position in X, desired position in Y, and desired orientation, respectively; • The follower agent keeps a desired distance d * and a formation angle α * concerning the leader robot, and a desired orientation θ * F , that is, lim

Modeling Based on Distance and Formation Angle
From Figure 1, the distance d LF between the leader and the follower and formation angle α LF can be described as (4) can be expressed asḋ Taking the time-derivative of (5), one obtains the following equations: Now, by noting thatd LF x =ẍ L −ẍ F andd LF y =ÿ L −ÿ F , while the time-derivative of d LF x and d LF y arė the set of Equation (6) can be, after some simplifying actions, reduced tö The above can also be expressed in vector-matrix form as where By substituting (2) into (7), the simplified perturbed dynamic model based on distance and formation angle between the agents can be derived in the following compact form: . Note that the model given in (8) is obtained from the leader's reference frame.

Remark 1.
Although the approach is developed for a pair of robots (R L , R F ), the model (8) can be extended to multiple robots assuming that the follower is the leader of another robot. In this sense, open-chain or convoy-like topologies can be addressed.

Control Strategy
This Section presents the development process of two control strategies for omnidirectional mobile robots moving in a formation. First, a controller for the leader robot is designed to follow a desired trajectory. Then, a controller for the follower is designed to maintain a desired distance and formation angle concerning the leader.

Leader Controller Design
It is desired that the leader tracks, independently of the follower, a user-defined trajectory specified by the position reference signals q * (t) = x * y * θ * , which is at least twice differentiable. From (2), the dynamics of the leader is described as Consider the tracking trajectory error e q L = q L − q * , its dynamics, based on (9), is with Ψ L = ψ L −q * being the total disturbance [44].

Assumption 1.
The total disturbance Ψ L can be modeled with an element of a family of fixed-degree Taylor series polynomials of where r 1 (t) are residual vector terms (for details, see [45]).
In order to choose the observer gains, it is possible to match the characteristic polynomials of (13) with Hurwitz polynomials as in [46], resulting in where I 3 is the 3 × 3 identity matrix, while the gain matrices of the observer are selected as The proper selection of observer gains Λ j L can guarantee convergence of the estimated values, i.e.,ê 1 L → e 1 L ,ê 2 L → e 2 L , Based on the previous analysis, the ADRC for the leader is proposed as follows: where It is worth mentioning that control law (14) only depends on the position and orientation measurements, while the GPIO estimates the velocity errors and disturbances.
Theorem 1. Let the control law (14) be applied to the system (10). Therefore, the tracking trajectory error e q L and its time-derivativeė q L converge asymptotically to zero, that is, lim t→∞ e q L = lim t→∞ė q L = 0, and the leader robot reaches the desired trajectory.
Proof. Substituting (14) into (10), and remembering thatê 2 L = e 2 L −ẽ 2 L , one obtainṡ where e L = e 1 L e 2 L and matriceŝ where 0 3×1 is a 3 × 1 zero vector. Next, let us propose the following Lyapunov function: whose time-derivative along the trajectories (15) is given bẏ SinceÂ L is a Hurwitz matrix, P LÂL +Â L P L = −Q L with Q L = Q L > 0 andV L can be bounded asV Because Γ L ẽ 2 L ,z 1 L converges to zero, one can conclude thatV L < 0 and the tracking trajectory error e q L and its time-derivativeė q L also converge asymptotically to zero.

Follower Controller Design
In this case, the goal for the follower is to keep a certain distance and angle with respect to the leader as well as a desired orientation, given by the following reference vector: η * (t) = d * α * θ * F . Let us define the tracking error of the proposed scheme based on distance and formation angle as e η = η LF − η * (t), whose dynamics is given as Substituting (8) into (16), the error dynamics can be expressed as where is the total disturbance. Considering Assumption 1, an extended state space from (17) is defined, with e 1 η = e η , e 2 η =ė η , and z 1 η = Ψ LF , as A GPIO is proposed to estimate the velocities and disturbances of the proposed scheme as˙ê where Λ jF ∈ R 3×3 for j = {0, 1, 2} are diagonal matrices with elements λ Fi,i ∈ R + for i = {1, 2, 3}. Let us now define the estimation errors asẽ 1 η = e 1 η −ê 1 η ,ẽ 2 η = e 2 η −ê 2 η , andz 1 η = z 1 η −ẑ 1 η . Then, the dynamics ofẽ 1 η is obtained from (18) and (19) as follows In order to choose the observer gains, the matching between the characteristics polynomials of (20) and Hurwitz polynomials is given by The gain matrices of the observer are selected as The proper selection of observer gains Λ j F can guarantee convergence of estimates, i.e.,ê 1 η → e 1 η ,ê 2 η → e 2 η , andẑ 1 η → Ψ LF . Based on the previous calculus, the ADRC for the follower is proposed as where It is essential to point out that control law (21) only depends on the distance and orientation measurements, while the GPIO estimates the velocity errors and disturbances.

Theorem 2.
Let the control (21) law be applied to the system (17). Therefore, the tracking error e η and its time-derivativeė η converge asymptotically to zero, that is, lim t→∞ e η = lim t→∞ė η = 0, and the follower robot keeps a desired distance and formation angle with respect to the leader.
Proof. The proof of Theorem 2 is similar to the proof of Theorem 1, and therefore is omitted here.

Assumption 2.
The perturbations Ψ L and Ψ LF are bounded smooth signals such that sup t |Ψ L | ≤ K 1 and sup t |Ψ LF | ≤ K 2 with K 1 and K 2 as positive real numbers.

Numerical Simulations and Real-Time Experiments
This Section presents the simulations and real-time experiments that were carried out to exhibit the performance of the proposed control strategy. For the numerical simulations, a comparison with a robust controller is made. Then, the experimental setup is described, and two case studies are addressed to determine the performance of the control strategy. Finally, a discussion of the results is set out.

Numerical Simulation
The numerical simulations were conducted on Matlab/Simulink © with a sample time of 0.01 s. The parameters of the used robots are m s = 1.82 kg, r s = 0.03 m, δ = π 6 rad, D = 0.11 m, I w s = 3.06 × 10 −5 kg·m 2 , and I 0 s = 0.0071 kg·m 2 . The initial conditions are q L (0) = −0.6 −0.4 0 and q F (0) = −0.85 0.032 0 , while the perturbations are ψ L = 4 + cos(0.2t) −2 + 3 sin(0.2t) 2 − 3 sin(0.2t) , The leader robot is tracking a Lissajous curve defined as x * y * θ * = cos(wt) sin(2wt) 0 , with w = 2π 30 . For comparison purposes, a second-order sliding mode (SM) control is used, given by where s L =ê 2 L + β L e q L , s F =ê 2 η + β F e η are the sliding surfaces. The controller gains for the SM approach are set to K L = K F = diag{30, 30, 30},K L =K F = diag{0.01, 0.01, 0.01}, and β L = β F = diag{6, 6, 6}, while the control gains for the proposed strategy are set to K p L = diag{225, 225, 100}, K d L = diag{90, 90, 60}, K p F = diag{144, 100, 100}, and K d F = diag{72, 60, 60}. The observer gains are in this case set to Z O L = diag{2, 2, 2}, W O L = diag{30, 30, 30}, P O L = diag{120, 120, 120}, Z O η = diag{3, 3, 3}, W O η = diag{60, 60, 60}, and P O η = P O L . Such gains are computed using the methodology presented in [45]. The desired distance between the leader and the follower is set to d * = 0.4 m, with the desired formation angle α * = π 4 rad, and the desired orientation angle θ * F = θ L = 0 rad. Figure 2 compares the trajectory in the plane of both approaches. Note that the leader reaches the desired trajectory while the follower moves to maintain the desired distance and formation angle concerning the leader.  Figure 3 compares the distance, formation angle, and orientation errors. The distance error is closer to zero with the GPIO than when using the SM. On the other hand, the formation angle and orientation errors have a similar performance with both methodologies.  Figure 4, one can note that oscillations of higher amplitude appear in τ L 1 and τ L 2 when using the SM approach. On the other hand, from Figure 5, one can note that oscillations of higher amplitude appear in τ F 1 and τ F 3 when using the GPIO approach.   To assess the overall performance, a quadratic error index and a quadratic control index of the form were used to compare the performances obtained by the GPIO and SM approaches. As Figure 6 shows, the GPIO leads to a minimal performance index J in contrast to the results obtained with the SM technique. On the other hand, as Figure 7 presents, for the leader, the SM consumes more energy than the GPIO, while for the follower, the GPIO consumes more energy. Based on the above results, both approaches can deal with the external perturbations that affect the system; nevertheless, even though the GPIO approach needs more energy than the SM, the distance, formation angle, and orientation errors are closer to zero with the GPIO.

Real-Time Experiments
Two omnidirectional mobile robots were constructed for real-time experiments. Each robot uses three 12V POLOLU 37D geared motors, with a gear ratio of 1:70, and an encoder with a resolution of 64 counts per revolution (see Figure 8). For data acquisition, an STM32F4 Discovery board is implemented. At the same time, the communication between the computer and the robot is performed in real-time using a publicly available "waijung1504" Matlab/Simulink © library, using Bluetooth communication protocol which is programmed on ESP32 micro-controller with ARDUINO-ESPRESSIF (https: //docs.espressif.com/projects/arduino-esp32/en/latest/api/bluetooth.html, (accessed on 14 August 2023)) as it is shown in Figure 9. An STM32F4 Discovery board received the torque reference of each motor, and using a nonlinear function approximate the conversion of torque to PWM, as follows: The first term of the Equation (22) is the conversion of torque to PWM, and the second term compensates the dead zone of the motor due to the gearbox.
The experiments were carried out indoors with 10 infrared VICON© Bonita cameras with a precision of 0.5 mm [47], which measures the attitude of each robot in an area of 5 × 4 m 2 and with a sampling time of 0.005 s. For this purpose, each robot was placed with several reflective markers, which form different patterns and thus can be detected by the TRACKER© camera software.  The experiments have become a proof of concept to evaluate the control strategy's performance. In this context, this is considered a previous step for further experimentation with onboard sensors.
Two tests are carried out. For the former, the robots move in a horizontal plane: the leader tracks a circular trajectory with 0.5 m radius, while the follower maintains a desired distance and formation angle concerning the leader. In the latter, the robots are moving in a horizontal plane and in an inclined plane to evaluate the performance of the control strategy in the face of external disturbances. The robots' parameters; the controller and observer gains; and the desired distance, formation angle, and orientation are the same as in the numerical simulation.

First Case Study
For the first experiment, the initial conditions are q L (0) = 0.8794 0.0253 0.2061 and q F (0) = 0.6232 0.6837 0.0673 . The trajectory in the plane for both robots is in Figure 10, where the leader tracks the circular trajectory (red line). In contrast, the follower (blue line) maintains a desired distance and formation angle concerning the leader.  Figure 11 presents the distance, formation angle, and orientation errors between the two considered robots. Note that the errors are oscillating around zero, i.e., the desired distance d LF ≈ d * LF = 0.4 m, α LF ≈ α * LF = π 4 rad, and θ F ≈ 0 rad. Figure 11. Exp1: distance, formation angle, and orientation errors. Figure 12 illustrates the control inputs required to perform the desired motion of the robots. These signals correspond to the torques of each wheel. Note that such controls have higher-frequency oscillations due to selected higher observer gains needed to accomplish the formation tracking with relatively high accuracy. Furthermore, it is evident at the beginning that the robots are far away from the desired trajectory, so the control inputs reach their maximum value. Recall that the torques are saturated as τ s ∈ [−1.4, 1.4] N·m. The linear and angular velocities are shown in Figure 13a,b, respectively. Such velocities are calculated by taking the derivative of the position signals and adding a first-order Butterworth lowpass filter with a cutoff frequency of 12 π rad/s. Considering that the velocities of the robots are bounded by ±0.8 m/s, and the angular velocity is bounded by ±2 π rad/s, one can note that such velocities remain inside the allowed values.   Figure 14a presents the estimation of the linear disturbances. In the same context, Figure 14b estimates the angular disturbances. Note that such perturbations have a great magnitude initially. This comes from the fact that the robots are far away from the desired initial conditions.

Second Case Study
A second experiment was performed to verify the effectiveness of the proposed approach against unknown perturbations. In this sense, a platform with a tilt angle of 10 deg, as shown in Figure 8, is used as an unmodeled disturbance for both robots. The resultant motion of the robots is shown in Figure 15, from which one can note that the formation tracking is achieved regardless of the added perturbing surface tilt. The distance, formation angle, and orientation errors are shown in Figure 16, where it is clear that the errors are oscillating around zero, i.e., the desired distance d LF ≈ d * LF = 0.4 m, α LF ≈ α * LF = π 4 rad, and θ F ≈ 0 rad. Figure 16. Exp2: Distance, formation angle, and orientation errors. Figure 17 depicts the control inputs required to perform the desired robot motion. The robots are moving up the inclined plane when t ∈ [5,28] s. Note that the control inputs have more oscillations than Figure 12. This is due to the disturbance produced by the inclined plane. The linear and angular velocities are shown in Figure 18a,b, respectively. Again, note that such velocities remain inside the allowed values. Finally, Figure 19 illustrates the disturbance estimation. It is essential to point out that the estimated disturbances differ from those presented in Figure 14. This is due to the presence of the inclined plane. Furthermore, the disturbances increase when the robots start their motion on the inclined plane.
A video of the experimental results can be watched at https://drive.google.com/ drive/folders/11An29_Zuc_F4muxO2tVcYAg3e8qg5v0_?usp=sharing.

Discussion
To assess the overall performance, the same quadratic control index, presented in the numerical simulation, is used to compare the performance of the proposed technique in both scenarios. Furthermore, a quadratic observer index is defined as As Figure 20 shows, it is clear that more energy is needed when the robots are moving in the inclined plane. In a similar context, Figure 21 depicts that the estimated linear and angular perturbations increase when the robots move in the inclined plane. Based on those mentioned earlier, one concludes that even though the estimated perturbations are different, the control strategy can deal with them, and the robots can achieve tracking and formation control.

Conclusions and Future Work
This work developed a perturbed dynamic model based on the distance and the formation angle between a pair of omnidirectional mobile robots. The proposed control strategy needed information on the position, distance, and formation angle measurements while dedicated observers estimated the disturbances. From a mathematical point of view, it was proven that the tracking trajectory error of the leader converges asymptotically to zero. At the same time, the distance and formation angle errors also converge to zero, meaning the follower robot can maintain the desired distance and formation angle concerning the leader robot. The numerical simulation comparison of the proposed strategy with an SM controller shows that our control law can achieve smaller distance and formation angle errors than the SM approach. In the same context, real-time experiments validate the performance of the proposed control strategy in terms of accuracy and robustness, even when the robots needed to perform their given tasks under perturbing phenomena.
For further research, different tests will be carried out to determine (a) how fast the robots can be moving and (b) the performance of the robots for different trajectories where the centripetal/centrifugal acceleration changes both in magnitude and direction. On the other hand, we will tackle the time-varying formation control based on the distance and formation angle for a heterogeneous multi-agent system in a group of n robots, its implementation in cyber-physical multi-robot formations, and onboard sensors.