MEV Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN: 2088-6985 p-ISSN: 2087-3379 mev.lipi.go.id doi: https://dx.doi.org/10.14203/j.mev.2021.v12.57-67 2088-6985 / 2087-3379 ©2021 Research Centre for Electrical Power and Mechatronics - Indonesian Institute of Sciences (RCEPM LIPI). This is an open access article under the CC BY-NC-SA license (https://creativecommons.org/licenses/by-nc-sa/4.0/). MEV is Sinta 1 Journal (https://sinta.ristekbrin.go.id/journals/detail?id=814) accredited by Ministry of Research & Technology, Republic Indonesia Control of mobile robot formations using A-star algorithm and artificial potential fields Nelson Luis Manuel *, Nihat İnanç, Mustafa Yasin Erten Department of Electrical & Electronics Engineering, Kırıkkale University Kırıkkale City, 71450, Turkey Received 22 November 2021; Accepted 16 December 2021; Published online 31 December 2021 Abstract Formations or groups of robots become essential in cases where a single robot is insufficient to satisfy a given task. With an increasingly automated world, studies on various topics related to robotics have been carried out in both the industrial and academic arenas. In this paper, the control of the formation of differential mobile robots based on the leader-follower approach is presented. The leader's movement is based on the least cost path obtained by the A-star algorithm, thus ensuring a safe and shortest possible route for the leader. Follower robots track the leader's position in real time. Based on this information and the desired distance and angle values, the leader robot is followed. To ensure that the followers do not collide with each other and with the obstacles in the environment, a controller based on artificial potential fields is designed. Stability analysis using Lyapunov theory is performed on the linearized model of the system. To verify the implemented technique, a simulator was designed using the MATLAB programming language. Seven experiments are conducted under different conditions to show the performance of the approach. The distance and orientation errors are less than 0.1 meters and 0.1 radians, respectively. Overall, mobile robots are able to reach the goal position and maintaining the desired formation in finite time. ©2021 Research Centre for Electrical Power and Mechatronics - Indonesian Institute of Sciences. This is an open access article under the CC BY-NC-SA license (https://creativecommons.org/licenses/by-nc-sa/4.0/). Keywords: nonholonomic WMR; leader-follower approach; formation control; A-star algorithm; artificial potential fields. I. Introduction The formation of robots is an imitation of a group of behaviors observed in various creatures in the biological world that tend to work cooperatively in order to achieve a common goal [1]. While most industrial robots can only perform specific movements in the workspace, mobile robots can move freely within the predefined workspace [2]. This feature (mobility) expands the possibilities for the application of mobile robots in different environments [3][4][5]. There are certain circumstances in which, due to the complexity of the task, a single robot may not be able to satisfy several tasks simultaneously and/or efficiently. To overcome this, the formation of these robots can be considered [2]. Several advantages are pointed out with the implementation of robotic formations, such as better precision, increased efficiency and robustness against external agents [2]. The methods of controlling the formation of mobile robots can be divided into three classes: behavioral approach, virtual structure approach, and leader-follower approach [6][7]. In the leader-follower approach, one of the robots plays the role of leader and the rest are followers. The idea behind this approach is that the followers adjust their states according to the position of the leader robot. There are different leader-follower topologies, for example, approaches that consider multi-leaders, forming a chain (vehicle tracks vehicle), among others [2]. It is possible to find different control techniques for leader-follower robotic formations in the literature. Fuzzy logic controller is presented for the formation of wheeled mobile robots (WMRs) [8]. Input-output (I/O) feedback linearization along with potential fields is used to control the formation of robots in the leader-follower approach [9]. Linear model predictive control (MPC) along with feedback linearization is applied to achieve the task of a formation of WMRs [10]. A non-linear MPC based on neurodynamic-optimization is presented to control the formation of WMRs in a leader-follower architecture [11][12]. A control strategy based on Lyapunov theory and Sliding Mode is applied to * Corresponding Author. Tel: +90-552-5824071 E-mail address: nelsonluismanuel@gmail.com https://dx.doi.org/10.14203/j.mev.2021.v12.57-67 http://u.lipi.go.id/1436264155 http://u.lipi.go.id/1434164106 https://mev.lipi.go.id/mev/index https://dx.doi.org/10.14203/j.mev.2021.v12.57-67 https://creativecommons.org/licenses/by-nc-sa/4.0/ https://sinta.ristekbrin.go.id/journals/detail?id=814 https://crossmark.crossref.org/dialog/?doi=10.14203/j.mev.2021.v12.57-67&domain=pdf https://creativecommons.org/licenses/by-nc-sa/4.0/ N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 58 multiple WMRs in a leader-follower structure [13]. The leader-follower approach has advantages such as simplicity, no need for global knowledge, and consequently, reduced computerization cost [13]. In this paper, a technique for controlling differential mobile robot formations based on the A- star and artificial potential fields algorithms is presented. The approach used is leader-follower, where the leader receives information about the safest and lowest cost route in an environment with the presence of obstacles from the A-star algorithm. The leader's position and orientation are constantly monitored by the followers. Using a controller based on artificial potential fields, the leader's trajectory is followed, maintaining the desired separation and orientation while avoiding collisions with each other and the obstacles present in the environment. II. Materials and Methods A. Kinematic model of differential mobile robot With the exception of a few, most control techniques depend on the mathematical model of the system to be controlled. Therefore, the first step before deducing the control laws is to define the mathematical model of the differential mobile robot. From Figure 1, applying some trigonometry concepts, the system of equations (1) can be determined. cos( ) 0 sin( ) 0 0 1 I x v y θ θ ω θ         =                 (1) Here 𝑣 is the linear velocity, 𝜔 is the angular velocity, �̇� is the velocity with respect to the x-axis, �̇� is the velocity with respect to the y-axis, and �̇� is the bearing angle. A differential drive wheeled mobile robot is usually composed of two independently controlled wheels and an additional wheel (or caster) for balance purposes. The direction of movement, speed, and orientation of the robot is defined by the speed ratios between its wheels [14]. Figure 1 is a schematic of a differential drive wheeled mobile robot in an inertial frame, where 𝑋𝐼 and 𝑌𝐼 represent the inertial (or global) frame axes and 𝑋𝑅 and 𝑌𝑅 represent the robot frame (or local frame) axes. The robot's linear velocity 𝑣(𝑡) is always parallel to the 𝑋𝑅 axis of the robot frame due to the nonholonomic constraint. The robot's orientation is measured between the 𝑋𝐼 and 𝑋𝑅 axes. The change in orientation 𝜃(𝑡) of the robot represents the angular speed 𝜔(𝑡). The robot's equivalent position on the inertial frame is shown by the point P. System (1) represents the mathematical model of the differential mobile robot shown in Figure 1. In general terms, it can be said that to design a controller for this mobile robot system, it is necessary to find the law that provides the relationship between the output vector (the robot position) and the input vector of the system (linear and angular velocities). Such a relationship can be considered as a transformation matrix. The purpose of the controller layout is to find, if it exists, a 2×3 control matrix 𝐾, as shown in equation (2) [15]. 11 12 12 21 22 23 , ( , )ij k k k K k k t e k k k   = =    (2) Because of the non-linearity of the robot model, the elements (𝑘𝑖𝑖) of the control matrix (𝐾) are time- varying and error dependent, as expressed in equation (2). The matrix 𝐾 has to be able to generate control signals 𝑣(𝑡) and 𝜔(𝑡) in such a fashion that the error 𝑒(𝑡) tends to zero as time 𝑡 tends to infinity, i.e., equation (3) has to be satisfied. ( ) . , lim ( ) 0 ( ) R t x v t K e K y e t tω θ →∞      = = =         (3) Finding the matrix 𝐾 in (2) is not that simple as it depends on the current value of the error and its elements can change over time. To overcome this, the approach used in [15] will be considered. A schematic of a robot and a desired position (or goal) is illustrated in Figure 2. The frame of the desired position (𝑋𝐺, 𝑌𝐺) is set to match the inertial frame (𝑋𝐼, 𝑌𝐼) . The Euclian distance between the robot and the desired position is symbolized by 𝜌; ∆𝑥 and ∆𝑦 are the differences between the 𝑥 and 𝑦 coordinates of the robot's position and the desired position, respectively. The bearing angle which orients the robot towards the goal position is 𝑿𝑰 𝒀𝑰 𝜔(𝑡) 𝑣(𝑡) 𝜃 Castor wheel 𝒀𝑹 𝑿𝑹 P Figure 1. Differential drive mobile robot in an inertial frame 𝜔 𝑣 𝜃𝑅 𝛼 𝜌 Δ𝑥 Δ𝑦 YR Y𝐺 = 𝑌𝐼 X𝐺 = 𝑋𝐼 XR 𝛽 𝜃𝐺 𝛾 Goal Robot Figure 2. Kinematics of differential mobile robot N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 59 indicated by the angle 𝛼. The difference between the desired orientation (𝜃𝐺) and the robot orientation (𝜃𝑅), when 𝛼 = 0 is represented by the angle 𝛽. From Figure 2, the following closed-loop parameters can be determined: 2 2 R G x yρ α θ γ β θ γ = ∆ + ∆ = − + = − (4) where 𝛾 = 𝑎𝑡𝑎𝑎2(Δ𝑦, Δ𝑥), as can be clearly concluded from Figure 2. Unlike the arctangent (atan) function, arctangent2 (atan2) function operates in all four quadrants [16]. Since the robot has orientations in all quadrants, the atan2 function is preferred over atan, which only returns results in two quadrants. To analyze the behavior of the parameters (𝜌, 𝛼, 𝛽) presented in equation (4) as time passes, their time derivatives are computed. Since these parameters are not directly expressed as functions of time, their derivatives are obtained through the chain rule. Therefore, the dynamics of the Euclidean distance ( 𝜌 ), applying the chain rule, can be expressed as in equation (5). R R R R R R dx dy dd d d d dt dx dt dy dt d dt θρ ρ ρ ρ ρ θ = = + + (5) The �̇� on the left side of equation (5) represents the time derivative of the Euclidean distance (𝜌), and the addends on the right side are the partial derivatives of 𝜌 with respect to 𝑥 , 𝑦 , and 𝜃 , respectively. In equations (6), (7), and (8), the derivatives of the addends present on the right side of equation (5) are demonstrated. ( ) ( )2 2 ;G R R G R G R R R x xd x dx x x y y dx x dt ρ ρ − ∆ = − = − − + − =  (6) ( ) ( )2 2 ;G R R G R G R R R y yd y dy x x y y dy y dt ρ ρ − ∆ = − = − − + − =  (7) 0 ; dd R Rd dtR θρ θ θ = =  (8) where 𝑥𝑅 , 𝑦𝑅 represent the robot position coordinates on the 𝑋𝐼 and 𝑌𝐼 axes and 𝑥𝐺, 𝑦𝐺 are the goal position coordinates on the 𝑋𝐼 and 𝑌𝐼 axes of the inertial frame. Substituting the results obtained in equations (6), (7), and (8) in equation (5), the equation (9) is derived. . .R Rx x y yρ ρ ∆ + ∆ = −    (9) Furthermore, replacing 𝛥𝑥 by 𝜌cos (𝛾) , Δy by 𝜌sin (𝛾) , 𝑥�̇� by 𝑣cos (𝜃) , 𝑦�̇� by 𝑣sin (𝜃) , and then applying the trigonometric identity cos(𝑎 − 𝑏) = cos(𝑎) . cos(𝑏) + sin(𝑎) . sin(𝑏), equation (9) becomes equation (10), which represents the rate of change of the Euclidean distance (𝜌) with respect to time. cos( )vρ α= − (10) Applying the same idea used to obtain the equation (10) to the other two parameters presented in equation (4), the system (11) can be derived. cos( ) 0 sin( ) 1 sin( ) 0 v α ρ α α ωρ β α ρ    −        = −            −       (11) B. Stability analysis using Lyapunov The feedback system given in (11), presents a discontinuity at 𝜌 = 0. To solve this, the inputs (𝑣 and 𝜔) are selected as shown in the system (12). cos( ) ( ) sin( ) sin( ) k V x k k k k ρ ρ α β ρ ρ ρ α α α α β β α  −    = = − −      −        (12) where 𝑉(𝑥) is the Lyapunov function applied to the differential mobile robot model, 𝑘𝜌 is the linear velocity adjustment gain, 𝑘𝛼 and 𝑘𝛽 are the robot orientation adjustment gains. The system (12) is now continuous at the point [𝜌, 𝛼, 𝛽] = [0, 0, 0], which is the point of interest (the goal). However, as this system is not linear, the stability analysis becomes complex. To facilitate this, the analysis is carried out at the equilibrium point of interest. Local stability analysis allows the approximation of the behavior of a non-linear system to an equivalent linear system at the point of interest. Applying Jacobian to the system (12) in the goal position ( [𝜌, 𝛼, 𝛽] = [0, 0, 0]) , system (13) is derived. The matrix A of the system (13) is linear and time-invariant, therefore, the stability analysis becomes simplified. A 0 0 0 ( ) 0 0 k k k k k ρ α ρ β ρ ρ ρ α α β β  −        = − − −         −        (((((((( (13) The system is locally exponentially stable, if all eigenvalues of the matrix A presented in (13) have a negative real part. From the matrix A the characteristic polynomial presented in equation (14) is obtained. ( ) 2 ( )k k k k kρ α ρ ρ βλ λ λ + + − −  (14) The 𝜆 in equation (14) represents the eigenvalues of matrix A. Therefore, in order for the system to be stable after reaching the destination, the conditions presented in (15) must be met. 0; 0;k k k kρ β α ρ> < > (15) N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 60 C. A-star algorithm A-star, is one of the widely applied computer science algorithms, which takes some inputs (the starting sector and the destination sector), calculates the costs of different possible routes and returns the path with the lowest cost between the starting sector and the destination sector [17]. A-star is based on equation (16): ( ) ( ) ( )F x g x h x= + (16) where 𝑥 is a sector on the map, 𝑔(𝑥) is the total distance from the starting sector to the current sector 𝑥 , ℎ(𝑥) is the heuristic function used to calculate the distance from the current position to the desired position, and 𝐹(𝑥) is the cost function. Figure 3 is a flowchart that gives a brief explanation of how the applied A-star algorithm works. D. Artificial potential fields In artificial potential fields (APF), objects in the workspace are modeled as potential fields that generate forces of attraction and repulsion [18]. Target points are modeled to generate attraction forces, while obstacles are modeled as points that generate repulsion forces to the robot [19]. In this manner, the robot tends to move towards the target while avoiding obstacles present in the environment. The function of the attractive potential field can be expressed by equation (17). ( )21(X ) 2att R att R D U K X X= − (17) where 𝑈𝑎𝑎𝑎(𝑋𝑅) represents the attractive potential field function, 𝐾𝑎𝑎𝑎 is a positive constant (defines the influence of the attractive field), 𝑋𝑅 is the vector of the robot's position and 𝑋𝐷 is the vector of the desired (or goal) position. The negative gradient of the attractive potential field function results in the corresponding attractive force, as shown in equation (18). ( ) ( )att att att D R D R att D R F U K X X x x K y y = −∇ = − −  =  −  (18) where 𝑥𝐷 and 𝑦𝐷 are the desired position ( 𝑋𝐷) components and 𝑥𝑅 and 𝑦𝑅 are the robot position (𝑋𝑅) components. Similarly, the repulsive potential field function is defined according to equation (19). 2 1 1 1 , ( ) 2 0 , rep o rep R o o K U X e e e e e e     − ≤ =     > (19) where 𝑈𝑟𝑟𝑟(𝑋𝑅) is the repulsive potential field Start Specify starting, ending and obstacles positions Compute cost function (using E q. 16). Is open list null? Select the node (x) with the lowest F(x) as start node. Save its index and place it in the closed list No Feasible path not found Yes End Is (x) the goal point? Yes A No A Expand the current node (x) and create a new node (x) which is not on the closed list Is the new node already in the open list? Compare the new g(x) and the old g(x) of the new node (x) and update g(x) to the smaller one Yes Place the new node (x) into the open list No B B Use saved index pointers to get the shortest path. Send this path to the leader robot Create an open list and a closed list; place the start node and its adjacents in the open list; put obstacles in the closed list Figure 3. Flowchart of A-star algorithm N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 61 function, 𝐾𝑟𝑟𝑟 is a positive parameter which defines the impact of the repulsive potential, 𝜀 is the distance between the robot and the obstacle and 𝜀0 represents the threshold distance of the repulsive potencial field influence. The repulsive force ( 𝐹𝑟𝑟𝑟 ) is determined by computing the negative gradient of the repulsive field, as shown in equation (20). ( ) 3 1 1 , 0 , rep rep rep o R o o Ro o F U K x x y y e e e ee e e = −∇  −   − ≤   −=      > (20) where 𝑥𝑜 and 𝑦𝑜 are the components of the obstacle vector. Therefore, the resulting total force (𝐹𝑎𝑜𝑎) that moves the robot towards the desired position will have the influences of both attractive and repulsive forces as expressed in equation (21). Figure 4 is a simplified representation of the resulting force on the follower robot in the presence of the attractive field (which pulls the robot towards the leader) and the repulsive field (which arises due to the presence of obstacles). ( ) ( ) (X )tot R att R rep RF X F X F= + (21) In Algorithm 1, the pseudocode of the controller applied in this paper based on artificial potential fields is presented. Algorithm 1: Pseudocode of the APF Input: leader’s position, followers’ positions, desired position(s), stopping criterion (𝛿), max. linear and angular speeds (𝑣𝑚𝑎𝑥 and 𝜔𝑚𝑎𝑥 ). Output: linear and angular speeds (𝑣 and 𝜔). 1: while (𝜌 > 𝛿) do 2: 𝑥𝑅, 𝑦𝑅, 𝜃𝑅 ←Sense the robot position; 3: 𝐹𝑎𝑡𝑡 = 𝐾𝑎𝑡𝑡 � 𝑥𝐷 − 𝑥𝑅 𝑦𝐷 − 𝑦𝑅 �; 4: 𝐹𝑟𝑒𝑟 = � 0 0 �; 5: 𝑁𝑜 ← Sense the obstacle(s); 6: if 𝑁𝑜 > 0 then 7: 𝐹𝑟𝑒𝑟 = 𝐾𝑟𝑒𝑟 ∑ 1 𝜖𝑖 3 � 1 𝜖0 − 1 𝜖𝑖 �� 𝑥𝑜𝑖 − 𝑥𝑅 𝑦𝑜𝑖 − 𝑦𝑅 �𝑁0𝑖=1 ; 8: end 9: 𝐹𝑡𝑜𝑡 = 𝐹𝑎𝑡𝑡 + 𝐹𝑟𝑒𝑟 ; 10: 𝑣 ← 𝑚𝑖𝑎(‖𝐹𝑡𝑜𝑡‖2 , 𝑣𝑚𝑎𝑥 ); 11: 𝜔 ← �𝑎𝑡𝑎𝑎2�𝐹𝑡𝑜𝑡 {𝑦}, 𝐹𝑡𝑜𝑡 {𝑥}� − 𝜃𝑅�; 12: 𝜔 ← 𝑠𝑖𝑔𝑎(𝜔)𝑚𝑖𝑎(|𝜔|, 𝜔𝑚𝑎𝑥 ); 13: 𝜌 ← �(𝑥𝐷 − 𝑥𝑅)2 + (𝑦𝐷 − 𝑦𝑅)2 ; 14: return 𝑣 and 𝜔; 15: end III. Results and Discussions To demonstrate that the designed controllers work properly, a simulator was designed using the MATLAB programming language. The simulations were carried out using MATLAB R2019a software, installed in a personal computer equipped with an Intel® Core™ i3 2.20 GHz processor and 4.00 GB of RAM. The A-star algorithm is used to obtain the least cost path. This path is sent to the leader. The leader's position is communicated to the followers. Based on this information and according to the values of the desired distances and angles, the followers track the leader robot. Followers are controlled based on the artificial potential fields algorithm, thus avoiding obstacles in real time. As shown in the next subsections, seven experiments were conducted under different conditions. In all experiments, the leader is colored red, and the followers are colored yellow. The robots' positions are represented by a 1×3 vector, that is, [𝑥 (𝑚), 𝑦 (𝑚), 𝜃(𝑟𝑎𝑟)], while the obstacles are represented by a 1× 2 vector, i.e., [𝑥 (𝑚), 𝑦 (𝑚)]. A. First experiment One follower and one leader are used for this experiment. A vertical obstacle is placed in the environment. The leader robot is asked to follow the safe route obtained by A-star. The follower robot is asked to keep an angle of 0 radians and a distance of 1 meter from the leader. The starting positions of leader and follower are [2.50, 6.50, -0.524], and [2.00, 8.50, 0.175], respectively. The goal position is [10.5, 6.5, 0.873], while the positions of the obstacles are [6.5, 5.5], [6.5, 6.5], and [6.5, 7.5]. The leader’s final position is [10.44, 6.43, 0.877] and the follower’s final position is [9.73, 5.60, 0.858]. The trajectories of this experiment, which lasted 122.623 seconds to complete, are illustrated in Figure 5. B. Second experiment In this experiment, one leader and one follower are taken again. But this time, a horizontal obstacle was placed in the environment. Under these conditions, the leader is asked to reach the target point by following the path obtained by A-star. The follower robot is requested to follow the leader by maintaining a distance of 1.5 meters and an angle of -0.5236 radians relative to the leader. The starting positions of leader and follower are [1.50, 6.50, - 0.524], and [1.00, 8.50, 0.175], respectively. The goal position is [10.5, 6.5, -0.767], while the position of the obstacle is [5.5, 6.5]. The leader’s final position is [10.43, 6.56, -0.763] and the follower’s final position is [8.95, 6.78, -0.753]. Figure 6 shows this experiment, which lasted 156.099 seconds. Leader Follower 𝐹𝑎𝑡𝑡 𝐹𝑡𝑜𝑡 𝐹𝑟𝑒𝑟 Obstacle Figure 4. Interaction of forces in the APF N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 62 C. Third experiment Two followers were used in this experiment. A vertical obstacle is included in the environment. Both follower robots are required to be 1 meter away from the leader. At the same time, one of the follower robots is asked to keep -0.5236 radians away from the leader. The other one is asked to keep it at 0.5236 radians. The leader’s starting position is [1.50, 5.50, 0.000] and the followers’ starting positions are [1.00, 7.50, 0.087], and [1.00, 2.50, 0.175]. The goal position is [10.5, 6.5, 0.775], while the positions of the obstacles are [5.5, 5.5], and [5.5, 6.5]. The leader’s final position is [10.43, 6.44, 0.778] and the followers’ final positions are [10.19, 5.39, 0.782], and [9.45, 6.28, 0.759]. The trajectories of this experiment, which lasted 124.426 seconds to complete, are illustrated in Figure 7. D. Fourth experiment Two followers and a horizontal obstacle were used in this experiment. The two followers must maintain 0 radian in relation to the leader. To prevent followers from colliding with each other, followers are commanded to have different separation distances from the leader. Thus, the first Figure 5. Result of the first experiment obtained using the simulator made in MATLAB Figure 6. Result of the second experiment obtained using the simulator made in MATLAB Figure 7. Result of the third experiment obtained using the simulator made in MATLAB N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 63 follower is commanded to keep 1 meter away from the leader and the other one is asked to keep 2 meters away from the leader. The leader’s starting position is [1.50, 6.50, 0.000] and the followers’ starting positions are [1.00, 8.50, 0.000] and [1.00, 3.50, 0.000]. The goal position is [10.5, 6.5, -0.768], while the position of the obstacle is [5.5, 6.5]. The leader’s final position is [10.43, 6.56, -0.765] and the followers’ final positions are [9.71, 7.26, -0.771] and [8.99, 7.95, -0.755]. The trajectories of this experiment, which lasted 245.220 seconds to complete, are illustrated in Figure 8. E. Fifth experiment In this experiment, 3 followers and a vertical obstacle were considered. The 3 followers are requested to hold 0.3491, 0, and -0.3491 radians from the leader robot, respectively. Follower 1 should be 2.5 meters away from the leader. Follower 2 should be 1 meter away from the leader. Follower 3 should be 2.5 meters away from the leader. The leader’s starting position is [1.50, 6.50, 0.000] and the followers’ starting positions are [-0.50, 8.50, 0.00], [0.50, 6.00, 0.000], and [-0.50, 4.50, 0.000]. The goal position is [10.5, 6.5, -0.785] and the obstacles’ positions are [5.5, 5.5], [5.5, 6.5], and [5.5, 7.5]. The leader’s final position is [10.44, 6.57, -0.803] and the followers’ final positions are [9.47, 8.88, -0.894], [9.74, 7.29, -0.802], and [8.16, 7.61, -0.804]. Figure 9 shows this experiment, which lasted 262.800 seconds. F. Sixth experiment In this experiment, 4 followers and a horizontal obstacle were selected. The vector [0.6981, 0, 0, -0.6981] was used to define the desired angles of the four followers. The vector [2.5, 1, 2, 2.5] was used to define the desired distances of the four followers relative to the leader. The leader’s starting position is [1.50, 6.50, 0.000] and the followers’ starting positions are [-3.50, 8.50, 0.000], [-1.50, 6.00, 0.000], [0.50, 3.50, 0.000], and [-2.50, 4.50, 0.000]. The goal position is [10.5, 6.5, -0.766] and the obstacle’s position is [5.5, 6.5]. The leader’s final position is [10.43, 6.56, -0.765] and the followers’ final positions are [10.91, 9.02, -0.981], [9.71, 7.26, -0.859], [8.99, 7.95, -0.773], and [7.998, 5.99, -0.601]. The trajectories of this experiment, which lasted 277.587 seconds to complete, are illustrated in Figure 10. G. Seventh experiment Four follower robots and vertical obstacles were used to simulate this experiment. The vector [0.5236, 0, 0, -0.5236] is used to define the desired angles of the follower robots and the vector [2.5, 1, 2, 2.5] is used to define the distance of the follower robots relative to the leader robot. The leader’s starting position is [1.50, 5.50, 0.000] and the followers’ Figure 8. Result of the fourth experiment obtained using the simulator made in MATLAB Figure 9. Result of the fifth experiment obtained using the simulator made in MATLAB N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 64 starting positions are [-3.50, 7.50, 0.000], [-1.50, 5.00, 0.000], [0.50, 2.50, 0.000], and [-2.50, 3.50, 0.000]. The goal position is [10.5, 6.5, 0.767] and the obstacles’ positions are [5.5, 5.5], and [5.5, 6.5]. The leader’s final position is [10.43, 6.44, 0.768] and the followers’ final positions are [7.96, 6.05, 0.778], [9.71, 5.74, 0.743], [8.99, 5.05, 0.735], and [9.96, 3.98, 0.761]. Figure 11 shows this experiment, which lasted 185.760 seconds. H. Comparison of distance and orientation errors In this subsection, the distance and orientation errors measured in each experiment are presented. The errors of the leaders were measured in relation to the goal position, while the errors of the followers were measured using as reference the desired values of angle and distance in relation to the leaders. Figure 12 to Figure 18 depict the distance and orientation errors for each experiment. The leaders’ distance errors remained practically constant in all experiments. The leader’s orientation errors are smaller than those of followers in all experiments, except in experiments four and five. With the exception of experiment 1, the followers’ distance errors are relatively smaller than the leaders’ ones in all experiments. Overall, it can be noticed that the values of both distance and orientation errors do not exceed 0.1 m and 0.1 rad, respectively. Figure 10. Result of the sixth experiment obtained using the simulator made in MATLAB Figure 11. Result of the seventh experiment obtained using the simulator made in MATLAB Figure 12. Distance and orientation errors corresponding to the first experiment N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 65 Figure 13. Distance and orientation errors corresponding to the second experiment Figure 14. Distance and orientation errors corresponding to the third experiment Figure 15. Distance and orientation errors corresponding to the fourth experiment Figure 16. Distance and orientation errors corresponding to the fifth experiment N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 66 IV. Conclusion In this paper, the formation control of differential mobile robots using the leader-follower approach was presented. As seen in the simulations, the A-star algorithm was used to obtain the safe path, then this path was sent to the leader. Follower robots receive information about the leader's position and maintain the desired distance and angles in relation to the leader robot. A controller that allows real-time obstacle avoidance (artificial potential fields) is used to allow followers to travel on safe paths. It is also important to emphasize that the conclusions made here are based on simulations and theoretical knowledge. Although the results obtained are successful in the virtual environment, it should be noted that, in the real world, some parameters of the applied controllers may need to be modified to achieve the same performance as in the virtual environment. This is because in the real world, there are many uncertainties that may not have been considered during the simulations. Declarations Author contribution All authors contributed equally as the main contributor to this study. Funding statement This research did not receive any specific grant from funding agencies in the public, commercial, or not-for- profit sectors. Conflict of interest The authors declare no known conflict of financial interest or personal relationships that could have appeared to influence the work reported in this paper. Additional information Reprints and permission information is available at https://mev.lipi.go.id/. Publisher’s Note: Research Centre for Electrical Power and Mechatronics - Indonesian Institute of Sciences remains neutral with regard to jurisdictional claims and institutional affiliations. References [1] K. H. Kowdiki, R. K. Barai, and S. Bhattacharya, “Leader- follower formation control using artificial potential functions: A kinematic approach,” IEEE-International Conf. Adv. Eng. Sci. Manag. ICAESM-2012, pp. 500–505, 2012. [2] A. Alouache and Q. Wu, “Performance comparison of consensus protocol and l-phi approach for formation control of multiple nonholonomic wheeled mobile robots,” J. Mechatronics, Electr. Power, Veh. Technol., vol. 8, no. 1, pp. 22–32, Jul. 2017. Figure 17. Distance and orientation errors corresponding to the sixth experiment Figure 18. Distance and orientation errors corresponding to the seventh experiment https://mev.lipi.go.id/ https://ieeexplore.ieee.org/document/6216054 https://ieeexplore.ieee.org/document/6216054 https://ieeexplore.ieee.org/document/6216054 https://ieeexplore.ieee.org/document/6216054 https://www.ijrer.org/10.14203/j.mev.2017.v8.22-32 https://www.ijrer.org/10.14203/j.mev.2017.v8.22-32 https://www.ijrer.org/10.14203/j.mev.2017.v8.22-32 https://www.ijrer.org/10.14203/j.mev.2017.v8.22-32 https://www.ijrer.org/10.14203/j.mev.2017.v8.22-32 N.L. Manuel et al. / Journal of Mechatronics, Electrical Power, and Vehicular Technology 12 (2021) 57-67 67 [3] L. Pacheco and N. Luo, “Testing PID and MPC performance for mobile robot local path-following,” Int. J. Adv. Robot. Syst., vol. 12, no. 11, p. 155, Nov. 2015. [4] S. Hiroi and M. Niitsuma, “Building a map including moving objects for mobile robot navigation in living environments,” in 2012 Ninth International Conference on Networked Sensing (INSS), Jun. 2012, pp. 1–2. [5] W.-Y. Lee, K. Hur, K. Hwang, D.-S. Eom, and J.-O. Kim, “Mobile robot navigation using wireless sensor networks without localization procedure,” Wirel. Pers. Commun., vol. 62, no. 2, pp. 257–275, Jan. 2012. [6] S.-L. Dai, S. He, X. Chen, and X. Jin, “Adaptive leader–follower formation control of nonholonomic mobile robots with prescribed transient and steady-state performance,” IEEE Trans. Ind. Informatics, vol. 16, no. 6, pp. 3662–3671, Jun. 2020. [7] J. Hirata-Acosta, J. Pliego-Jiménez, C. Cruz-Hernádez, and R. Martínez-Clark, “Leader-follower formation control of wheeled mobile robots without attitude measurements,” Appl. Sci., vol. 11, no. 12, p. 5639, Jun. 2021. [8] T. Dewi, C. Sitompul, P. Risma, Y. Oktarina, R. Jelista, and M. Mulyati, “Simulation analysis of formation control design of leader-follower robot using fuzzy logic controller,” in 2019 International Conference on Electrical Engineering and Computer Science (ICECOS), Oct. 2019, pp. 68–73. [9] J. Dong, S. Liu, and K. Peng, “Formation control of multirobot based on I/O feedback linearization and potential function,” Math. Probl. Eng., vol. 2014, pp. 1–6, 2014. [10] M. A. Kamel and Y. Zhang, “Decentralized leader-follower formation control with obstacle avoidance of multiple unicycle mobile robots,” in 2015 IEEE 28th Canadian Conference on Electrical and Computer Engineering (CCECE), May 2015, pp. 406–411. [11] H. Xiao and C. L. P. Chen, “Leader-follower consensus multi- robot formation control using neurodynamic-optimization- based nonlinear model predictive control,” IEEE Access, vol. 7, no. 9, pp. 43581–43590, 2019. [12] H. Xiao, Z. Li, and C. L. Philip Chen, “Formation control of leader–follower mobile robots’ systems using model predictive control based on neural-dynamic optimization,” IEEE Trans. Ind. Electron., vol. 63, no. 9, pp. 5752–5762, Sep. 2016. [13] Y. Zhao, Y. Zhang, and J. Lee, “Lyapunov and sliding mode based leader-follower formation control for multiple mobile robots with an augmented distance-angle strategy,” Int. J. Control. Autom. Syst., vol. 17, pp. 1314–1321, 2019. [14] J. Heikkinen, T. Minav, and A. D. Stotckaia, “Self-tuning parameter fuzzy PID controller for autonomous differential drive mobile robot,” Proc. 2017 20th IEEE Int. Conf. Soft Comput. Meas. SCM 2017, pp. 382–385, 2017. [15] R. Siegwart, I. R. Nourbakhsh, and D. Scaramuzza, Introduction to autonomous mobile robots, 2nd ed. Cambridge, MA: MIT press, 2011. [16] T. Zhang, P. W. Stackhouse, B. Macpherson, and J. C. Mikovitz, “A solar azimuth formula that renders circumstantial treatment unnecessary without compromising mathematical rigor: Mathematical setup, application and extension of a formula based on the subsolar point and atan2 function,” Renew. Energy, vol. 172, pp. 1333–1340, 2021. [17] I. M. Zidane and K. Ibrahim, “Wavefront and a-star algorithms for mobile robot path planning,” Adv. Intell. Syst. Comput., vol. 639, pp. 69–80, 2018. [18] R. L. Galvez et al., “Obstacle avoidance algorithm for swarm of quadrotor unmanned aerial vehicle using artificial potential fields,” in TENCON 2017 - 2017 IEEE Region 10 Conference, Nov. 2017, pp. 2307–2312. [19] A. Azzabi and K. Nouri, “Path planning for autonomous mobile robot using the potential field method,” in 2017 International Conference on Advanced Systems and Electric Technologies (IC_ASET), Jan. 2017, pp. 389–394. https://doi.org/10.5772/61312 https://doi.org/10.5772/61312 https://doi.org/10.5772/61312 https://doi.org/10.1109/INSS.2012.6240564 https://doi.org/10.1109/INSS.2012.6240564 https://doi.org/10.1109/INSS.2012.6240564 https://doi.org/10.1109/INSS.2012.6240564 https://doi.org/10.1007/s11277-010-0052-2 https://doi.org/10.1007/s11277-010-0052-2 https://doi.org/10.1007/s11277-010-0052-2 https://doi.org/10.1007/s11277-010-0052-2 https://doi.org/10.1109/TII.2019.2939263 https://doi.org/10.1109/TII.2019.2939263 https://doi.org/10.1109/TII.2019.2939263 https://doi.org/10.1109/TII.2019.2939263 https://doi.org/10.3390/app11125639 https://doi.org/10.3390/app11125639 https://doi.org/10.3390/app11125639 https://doi.org/10.3390/app11125639 https://doi.org/10.1109/ICECOS47637.2019.8984433 https://doi.org/10.1109/ICECOS47637.2019.8984433 https://doi.org/10.1109/ICECOS47637.2019.8984433 https://doi.org/10.1109/ICECOS47637.2019.8984433 https://doi.org/10.1109/ICECOS47637.2019.8984433 https://doi.org/10.1155/2014/874543 https://doi.org/10.1155/2014/874543 https://doi.org/10.1155/2014/874543 https://doi.org/10.1109/CCECE.2015.7129312 https://doi.org/10.1109/CCECE.2015.7129312 https://doi.org/10.1109/CCECE.2015.7129312 https://doi.org/10.1109/CCECE.2015.7129312 https://doi.org/10.1109/CCECE.2015.7129312 https://doi.org/10.1109/ACCESS.2019.2907960 https://doi.org/10.1109/ACCESS.2019.2907960 https://doi.org/10.1109/ACCESS.2019.2907960 https://doi.org/10.1109/ACCESS.2019.2907960 https://doi.org/10.1109/TIE.2016.2542788 https://doi.org/10.1109/TIE.2016.2542788 https://doi.org/10.1109/TIE.2016.2542788 https://doi.org/10.1109/TIE.2016.2542788 https://doi.org/10.1109/TIE.2016.2542788 https://doi.org/10.1007/s12555-018-0194-7 https://doi.org/10.1007/s12555-018-0194-7 https://doi.org/10.1007/s12555-018-0194-7 https://doi.org/10.1007/s12555-018-0194-7 https://doi.org/10.1109/SCM.2017.7970592 https://doi.org/10.1109/SCM.2017.7970592 https://doi.org/10.1109/SCM.2017.7970592 https://doi.org/10.1109/SCM.2017.7970592 https://dl.acm.org/doi/10.5555/1971970 https://dl.acm.org/doi/10.5555/1971970 https://dl.acm.org/doi/10.5555/1971970 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1016/j.renene.2021.03.047 https://doi.org/10.1007/978-3-319-64861-3_7 https://doi.org/10.1007/978-3-319-64861-3_7 https://doi.org/10.1007/978-3-319-64861-3_7 https://doi.org/10.1109/TENCON.2017.8228246 https://doi.org/10.1109/TENCON.2017.8228246 https://doi.org/10.1109/TENCON.2017.8228246 https://doi.org/10.1109/TENCON.2017.8228246 https://doi.org/10.1109/ASET.2017.7983725 https://doi.org/10.1109/ASET.2017.7983725 https://doi.org/10.1109/ASET.2017.7983725 https://doi.org/10.1109/ASET.2017.7983725 Introduction II. Materials and Methods A. Kinematic model of differential mobile robot B. Stability analysis using Lyapunov A-star algorithm D. Artificial potential fields III. Results and Discussions A. First experiment B. Second experiment Third experiment D. Fourth experiment E. Fifth experiment F. Sixth experiment G. Seventh experiment Comparison of distance and orientation errors IV. Conclusion Declarations Author contribution Funding statement Conflict of interest Additional information References