Hybrid Controller based on Null Space and Consensus Algorithms for Mobile Robot Formation

This work presents a novel hybrid control approach based on null space and consensus algorithms to solve the scalability problems of mobile robot formation and improve leader control through multiple control objectives. In previous works, the training of robots based on the null space requires a rigid training structure based on a geometric shape, which increases the number of agents in the formation. The scheme of the control algorithm, which does not make formation scalability possible, must be changed; therefore, seeking the scalability of training based on null space is a challenge that could be solved with the inclusion of consensus algorithms, which allow the control structure to be maintained despite increasing or decreasing the number of robot followers. Another advantage of this proposal is that the formation of the followers does not depend on any geometric figure compared to previous works based on the null space; this new proposal does not present singularities as if the structure is based on geometric shape, the latter one is crucial since the formation of agents can take forms that cannot be achieved with a geometric structure, such as collinear locations, that can occur in many environments. The proposed hybrid control approach presents three tasks: i) leader position task, ii) leader shape task, and iii) follower formation task. The proposed algorithm is validated through simulations, performing tests that use the kinematic model of non-holonomic mobile robots. In addition, linear algebra and Lyapunov theory are used to analyze the stability of the method.

Implementing several tasks in a single control algorithm is an advantage from the control point of view, which is why the implementation of algorithms based on null space has taken on great relevance. An algorithm of behaviors is used to execute tasks and sub-tasks of an autonomous robot system. These tasks are achieved simultaneously without generating conflict between them using the null space approach to execute lower hierarchy tasks [7]. The experimental results of the formation of multi-robot systems based on null spaces are shown in Antonelli et al. (2009) [8]. However, it does not consider the scalability of the formation. The null-space-based controller implementation is introduced to achieve multiple tasks within the heterogeneous formation of aerial and terrestrial robots [9]. However, in this proposal, the robot formation depends on a geometric shape. The formation of N-holonomic agents is solved using the hierarchical task-based scheme [10]; although, in that work, obstacle avoiding and agent formation are considered. Besides, a control algorithm with rigid and flexible formations to avoid static and dynamic obstacles is implemented in Leica et al. (2018) [11]. As proposed in Arevalo et al. (2018) [12], these do not consider human-robot interaction can control the formation. Previous works are applications with distributed control, where the control strategy must contemplate that the follower robots can move in a geometric space formed by other robots known as leaders. An adaptive null-space-based behavioral (NSB) method to deal with the problems of saturation planning and lack of adaptability when the traditional NSB method is applied to the formation control of multiple unmanned surface vehicles [13], a control strategy based on null-space for monitoring the trajectory of training of aerial manipulators in congested environments is shown in Camacho et al. (2019) [14]. By means of the null-space control, several control objectives can be achieved, assigning a priority to each of the objectives.
The strategy proposes controlling a homogeneous formation of three aerial manipulators for trajectory tracking and the evasion of obstacles. A null-space technique is used to guide the formation of two mobile robots autonomously; a differential drive wheeled ground vehicle and an unmanned aerial vehicle [15]. A controller based on Null-Space is used for the trajectory tracking of a formation of three quadcopters to evade grounded and aerial mobile obstacles [16]. A formation of robots based on null space, where the robot formation depends on a geometric figure, hinders the scalability of the robot formation [17]. An unmanned ground vehicle (UGV) and an unmanned aerial vehicle (UAV) use the concepts of null space control by robot formation [18]. A controller to autonomously guide a rigid formation of two mobile robots, a wheeled land vehicle with a differential drive, and an unmanned aerial vehicle, based on null space, is presented in [15,19]. The problem of multi-agent formation with trajectory tracking and obstacle avoidance, the formation control based on null-space, is considered in [20,21]. In the previous works based on Null-Space, the robot formation depends on a geometric figure, which hinders the scalability of the robot formation.
Null-space algorithms solve the inconvenience of multiple tasks, however with this control strategy, performing the training of several robots implies several disadvantages: i) it is necessary to change the shape and posture matrices, as well as their Jacobians, ii) there are configurations in the formation that present singularities, for example, the robots cannot be located collinearly with each other, iii) the calculation of the centroid of the formation becomes more complex, and in the cases of collinear robots, it cannot be calculated. For all the above, it is necessary to look for an alternative that improves the characteristics of the robot formation. The consensus algorithms allow avoiding singularities in the formation. Using leading virtual robots allows calculating the centroid of the formation simply and does not require the change of the posture and form matrices as it happens in the null space algorithm.
Consensus algorithms have been implemented in multi-vehicle cooperative control as presented in [22][23][24]. A robot could be represented by a double integrator dynamic model [25]. A distributed control algorithm is developed to drive the multi-robot system to follow a group of dynamic leaders with containment and group dispersion behaviors. A distributed containment control for double-integrator dynamics in the presence of both stationary and dynamic leaders, where the control of leaders becomes complex and does not allow to control the orientation or form of the formation [26,27]. Consensus algorithms are implemented to track the trajectory of a group of robots; however, neither the shape nor the position of the formation can be controlled [28]. A decentralized adaptive controller to cooperatively manipulate a payload of unknown mass employing multiple aerial robots is presented by Moreira et al. (2019) [15], where consensus algorithms are used to ensure that the manipulated load can be evenly distributed in each agent; its speed is constant converges to the given reference. The multi-agent system formation scaling control problem is studied in [13], where only one agent knows the desired formation size. In that work, the authors design a formation scaling control for universally rigid frameworks using the stress matrix; however, this work does not consider that robot formation can perform multiple subtasks.
The consensus algorithm is used for the coordinated motion control of a wheel-leg robot [29]. A formation control strategy merges the formation control algorithm, consensus control algorithm, and artificial potential field algorithm [30]. The formation control algorithm controls the formation of multiple robot formations. A consensus algorithm for distributed cooperative formation trajectory planning is proposed by combining improved artificial potential field and consensus theory [31]. A quadrotor variable formation control method based on a consensus control algorithm is proposed by Xu et al. (2020) [32]. A comparison between sliding mode and consensus approaches for quadrotor leaderfollower formation flight [33]. Finally, a distributed consensus algorithm for second-order nonlinear multi-agent systems is formulated under the leader-following approach [34]. The consensus algorithms allow formations without singularities; another advantage is that they do not require a geometric shape to perform the formation of the followers; however, they require algorithms for the formation of the leading robots, and it is complex to perform a posture and shape control to the leaders. Therefore, it is necessary to have several control objectives. That is why a novel hybrid combination of consensus and null space algorithms is carried out to take advantage of both control strategies.
On the one hand, the consensus algorithms allow eliminating the dependence on geometric shapes to form followers and avoid singularities that occur in geometric shapes. In contrast, the null space allows several control objectives to be carried out in a single control strategy, such as controlling the leader form and posture and controlling the formation of the follower robots. Another advantage of this hybrid algorithm is the scalability given to the null space.
The main contributions of this manuscript can be enumerated as follows: • A hybrid control topology combining the null space approach and consensus algorithms is presented.
• The hybrid control topology enhances multitasking in the formation of mobile robots regardless of the number of robots.
• The formation of the follower robots as a task within the null space does not depend on a geometric figure, so it is not necessary to redesign the controller, introducing scalability to the null space, adding advantages to the hybrid proposal.
• With this proposal, the singularities generated in certain configurations of the formation of the robot followers are avoided.
This paper is organized as follows: In section 2, the modeling of mobile robot formation is presented. Then, the control design based on the consensus algorithm and null-space approach for robot mobile formation is developed in section 3. Then, in section 4, the simulation results are shown. Finally, the conclusions are given in section 5.

2-Research Methodology
This paper presents a hybrid control strategy based on consensus and null space algorithms. Figure 1 shows the outline of the research methodology. The system initializes when the shape and posture parameters of the leader robots are defined. These parameters are used in the null space algorithm. This algorithm performs three tasks: i) Task 1: performs a posture control of the leader robots. They form a square geometric figure; the centroid of this geometric figure is identified, and the Jacobian of posture is determined. After this, the control actions are generated so that the centroid of the formation of the leader robots follows the desired trajectory. ii) Task 2: a shape control is applied to the leader robots; the geometric shape of these robots is described; in this case, the shape is a square. Then the Jacobian of shape is defined, and the control actions are generated so that the leader robots form the desired geometric figure. iii) Task 3: A consensus algorithm is implemented for the formation of the robot followers. The Laplacian formation matrix is defined according to the number of existing robot followers. Finally, control actions are produced for the follower robots to take the desired positions in the robot formation. Consider that the control algorithms constantly require information about the positions of the leader and follower robots.

3-Model Robot
In Figure 2, a mobile differential traction robot is shown.

Figure 2. Differential traction mobile robot
In the present work differential drives, mobile robots with non-holonomic restriction are used; the kinematic model is given by [6]: where ̇= [̇ ̇] are the temporal variations of the position X and Y of the i-th robot, is its orientation ui, and ωi are its linear and angular velocities respectively, and JRi is its Jacobian. Thus, for n robots, the kinematics model is defined by:   ] is the position of the centroid of the formation; d1, d2, d3, and d4 are the distances between robots. The shape variable qf is represented by:

3-1-Null-Space Algorithms
The posture variable qp is defined by: The control law is defined by: where where, Kf, Kf1, Kp, and Kp1 are positive constants for the tuning of the controller. and are the form the shape errors, respectively. ̇ and ̇ are the time derivates of the desired variables of form and posture.

3-2-Consensus Algorithms
In Figure 5, a control algorithm structure is shown.

Figure 5. Control algorithm structure
The consensus algorithm control law according to [20] is expressed as: where Γ is the Laplacian matrix, X and Y are the robot positions ̇= [̇1 ̇2 …̇4 ̇1 ̇2 ⋯̇] , ̇= [̇1 ̇2 …̇4 ̇1 ̇2 ⋯̇] are the derivatives of the position of the robots, ̈ and ̈ are the second derivatives in the x-y axis of the leader and followers robots. α, β and γ are controller scaling gains Ψx and Ψy accelerations in the leader robots x-y axis. Due to the robots input being input with only one derivative and the consensus control law having a double derivative, the control law is integrated through the Euler method.
where ̇− 1 and ̇− 1 re the previous state, ts is the sampling time, the control action for each robot is given by the inverse kinematics expressed as: ( 2 , 2 ) ( 1 , 1 ) Replacing Equations 8 and 9 in Equation 12, the control law is defined by: Figure 6 presents the scheme hybrid algorithm, where the system must carry out three tasks:

3-3-Hybrid Algorithm (Consensus and Null-Space)
• Task 1: To maintain the position of leader robots.
• Task 2: To maintain the shape of leader robots.
• Task 3: To maintain the formation of follower robots.

NULL-SPACE ALGORITHM
Taking into consideration the control objectives, the null space is defined by: where 2 = ( − + ); 2 where, Kf, Kf1, Kp and Kp1 are positive constants for the tuning of the controller. and are the form and the shape errors, respectively.
The stability of the second task is analyzed, the shape kinematics (shape of the leaders) is given by ̇= ℎ̇. Replacing the robot model: Assuming perfect velocity tracking U=Uc; in closed loop:   Multiplying both members of the equation by J2: Multiplying both members of Equation 40 by J2, and by orthogonality 2 + = 0, then: The control law UD is: where ℎ̇= [̇ ̇] replacing and expanding, thus ℎ̇ can be rewritten as: Replacing Equations 10 and 11, and expanding Equation 42, thus can be rewritten as: It is known that ΓX the position error can be defined as =̃ where ̃ is the position error on the x-axis. Therefore, it can be rewritten as:

By replacing Equation 42 and its temporal derivatives in Equation 41
: For the stability analysis, it can be considered that the first n-m inputs corresponding to the four leaders are equal to zero since the consensus algorithm will be used to carry out the formation of the followers. The formation of the leaders is carried out in the second task. Thus, it can be rewritten in a simplified form as: where ΨF=ΓΨx are position vector error in X, Y., velocity vectors error in x and y. , and leader acceleration vectors in x and y. To find a candidate function to test the stability criterion, the matrices P and Q are defined as: where λmin(M) is the smallest eigenvalue of M. According to which the Lyapunov candidate function is defined as follow: Deriving V, it is obtained: The product (̃+̃) (̃+̃) is: To guarantee that V<0, the following condition must be met: Thus ̃→ 0 when → 0. Considering the same analysis for ̃, it is obtained that ̃→ 0 the same stability analysis is developed for the Y-axis.

4-Test and Results
To validate the hybrid control, two experiments have been developed: i) formation of 7 robots (4 leaders and three followers), and ii) formation of 12 robots (4 leaders and eight followers). With these experiments, it is intended to demonstrate that with the proposed hybrid controller, it is possible to make the scalability of a null space algorithm for a formation of mobile robots, maintaining the structure of the controller despite increasing the number of robots, it is also intended to verify that there are no singularities in the formation of the robot followers. And the convergence to zero of the control errors will be verified. The main advantage of the proposal is that the formation of the followers does not depend on any geometric shape. The controller parameters are Kf=3.8; Kf1=5.5; Kp=2.8; kp1=10.5; α=10; γ=30; β=7, these values were obtained empirically through experimentation. In the first experiment, seven robots are considered. The distribution of robots is shown in Figure 8. The trajectory is defined by ( ) = ( /4), the sampling time is = 0.02 . Figure 9 shows the trajectory tracking for a group of robots, four leader robots, and three follower robots. The leader robots start from an initial position; the null space control brings the leading robots to a square shape. The follower robots also start from an initial position, and the consensus algorithm control allows the followers robots to be in the desired positions. The centroid of the square formed by the leaders follows the desired sinusoidal trajectory. It can be seen in Figure 9 that the leader robots form a square, and the follower robots are positioned in the desired position of the formation.  Figure 10 shows the posture error in X, and the posture error in Y, the initial position of the centroid of the formation of the leader robots starts outside the desired trajectory, for which an error is generated at the time 0 , task 1 of the null_space control takes the centroid of the formation to the desired trajectory; it is observed in the figure that the posture errors converge to zero, verifying the good performance of the null space controller.  Figure 11 shows the formation errors of the four leader robots ̃1 , ̃2 , ̃3 and ̃4 . It can be seen that the robots start from an initial position, which generates the formation errors at the time 0 . Task 2 of the null space controller allows the leader robots to form a square figure; it is verified in Figure 11 that the four shape errors converge to zero, which confirms the good performance of the null space controller Figure 11. Shape errors of the leader Figure 12 shows the position errors of the three follower robots, where 1 , 2 and 3 are the position errors in X of the formation of the follower robots 1 , 2 and 3 are the position errors in Y of the formation of the follower robot. To control these errors, the consensus algorithm of the hybrid controller was implemented; it can be seen that the robots start from an initial position different from the desired one, and the consensus algorithm takes the robots to the desired positions. It is verified that the position errors of the follower robots converge to zero, affirming the good behavior of the proposed controller.

Figure 12. Position errors of the follower robots
In this first experiment, a hybrid controller has been implemented for the formation of 7 robots (4 leader robots and three follower robots); the hybrid controller is structured of the null space controller and a consensus algorithm. As a result, it has been observed that the null space controller meets the posture objectives, the consensus algorithm meets the control objective of the formation of the follower robots. Therefore, the first experiment can verify that the control objectives are met since the errors converge to zero.
Twelve robots (4 leaders and eight followers) are considered for the second experiment. The leader robots maintain the same square shape as in experiment one. The controller structure does not change, task 1 is maintained as a posture control, and task 2 for the formation of the leader and follower robots, the follower robots use consensus algorithms to maintain their shape. The Jacobian of the robots and the Laplacian matrix are changed to increase the number of robots. The location of the follower robots depends on the Laplacian matrix, and they are located as shown in Figure 13.  Figure 14 shows the trajectory tracking for a group of robots, four leader robots, and eight follower robots. The leader robots start from an initial position; the null space control brings the leading robots to a square shape. The follower robots also start from an initial position, and the consensus algorithm control allows the follower robots to be in the desired positions. The centroid of the square formed by the leaders follows the desired sinusoidal trajectory. It can be seen in Figure 14 that the leader robots form a square, and the follower robots are positioned in the desired position of the formation. Figure 15 shows the posture error in X, and the posture error in Y, the initial position of the centroid of the formation of the leader robots starts outside the desired trajectory, for which an error is generated at the time 0 Task 1 of the null space control takes the centroid of the formation to the desired trajectory; it is observed in the figure that the posture errors converge to zero, verifying the good performance of the null space controller. Figure 16 shows the formation errors of the four leader robots ̃1 , ̃2 , ̃3 and ̃4 . It can be seen that the robots start from an initial position, which generates the formation errors at the time 0 . Task 2 of the null space controller allows the leader robots to form a square figure; it is verified in the figure that the four shape errors converge to zero, which verifies the good performance of the null space controller.  are the position errors in X of the formation of the follower robots 1 , 2 , 3 , 4 , 5 , 6 , 7 8 are the position errors in Y of the formation of the follower robot. To control these errors, the consensus algorithm of the hybrid controller was implemented; it can be seen that the robots start from an initial position different from the desired one, and the consensus algorithm takes the robots to the desired positions. It is verified that the position errors of the follower robots converge to zero, verifying the good behavior of the proposed controller. In this second experiment, a hybrid controller has been implemented for the formation of 12 robots (4 leader robots and eight follower robots); the hybrid controller is structured around the null space controller and a consensus algorithm. The structure of the controller is maintained; what changes is the Laplacian matrix, and it has been observed that the null space controller meets the posture objectives, the consensus algorithm meets the control objective of the formation of the eight follower robots. The second experiment can verify that the control objectives are met since the errors converge to zero. The scalability of the controller is demonstrated in this second experiment, where it can be verified that the control objectives are met since the errors converge to zero.
Based on the experiments carried out, it can be determined that the hybrid control topology enhances multitasking in the formation of mobile robots regardless of the number of robots. The implementation of the null space algorithm allows a controller with multiple tasks, such as i) the posture task, which allows the formation centroid of the leading robots to follow the desired trajectory, ii) the shape task, which allows the leading robots to form a square, and iii) in the task of forming the following robots, the advantages of the consensus algorithm are included, such as the formation of the follower robots as a task within the null space does not depend on a geometric figure, so it is not necessary to redesign the controller, introducing scalability to the null space, adding advantages to the hybrid proposal, singularities that can be generated in certain configurations are avoided. This proposal combines the best features of null space control and consensus algorithms.

5-Conclusion
In the present work, a novel proposal for a hybrid control based on null-space and consensus algorithms to solve the scalability problem of robot formation was developed. The main advantage of the proposal is that the formation of the followers does not depend on any geometric shape. Simulation tests have shown the outstanding performance of the proposed controller in the formation of non-holonomic mobile robots, and the algorithm scalability has been validated. Furthermore, the performed tests showed that the errors of the three tasks in the null space converged to zero: i) the leader position task, ii) the leader shape task, and iii) the follower formation task. Furthermore, the results confirmed that the controller structure did not change despite increasing or decreasing the number of follower robots in the formation. Based on the experiments carried out, it can be determined that the hybrid control topology enhances multitasking in the formation of mobile robots regardless of the number of robots.
Furthermore, the formation of the follower robots as a task within the null space does not depend on a geometric figure, so it is not necessary to redesign the controller, introducing scalability to the null space, adding advantages to the hybrid proposal. Another advantage of implementing this structure is that it differs from algorithms based on geometric figures in that it does not introduce singularities in the formation of mobile robots. Therefore, the approach presented a control algorithm that allows the null space and consensus algorithms to solve the scalability problem and the geometric shape dependence for the follower robots' formation. In future work, it is proposed to analyze a complete model of the robots in which changes in the dynamics are considered to develop an in-depth study of the performance of the proposed controller.

6-2-Data Availability Statement
The data presented in this study are available on request from the corresponding author.

6-3-Funding and Acknowledgements
This work was sponsored by Universidad Internacional del Ecuador, Escuela Politécnica Nacional and Universidad San Francisco de Quito. PL, MH, and LM thank the PIS-19-10 Project of the Escuela Politécnica Nacional for their sponsorship to realize this work.

6-4-Conflicts of Interest
The author declares that there is no conflict of interest regarding the publication of this manuscript. In addition, the ethical issues, including plagiarism, informed consent, misconduct, data fabrication and/or falsification, double publication and/or submission, and redundancies have been ultimately observed by the authors.