MEV J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN: 2088-6985 p-ISSN: 2087-3379 www.mevjournal.com © 2015 RCEPM - LIPI All rights reserved. Open access under CC BY-NC-SA license. Accreditation Number: 633/AU/P2MI-LIPI/03/2015. doi: 10.14203/j.mev.2015.v6.67-74 OBSTACLE AVOIDANCE METHOD FOR A GROUP OF HUMANOIDS INSPIRED BY SOCIAL FORCE MODEL Ali Sadiyoko a, b, *, Bambang Riyanto Trilaksono b, Kusprasapta Mutijarsa b, Widyawardana Adiprawita b aMechatronics Engineering Department, Universitas Katolik Parahyangan Jl. Ciumbuleuit no 94, Bandung, Indonesia bSchool of Electrical Engineering & Informatics, Institut Teknologi Bandung Jl. Ganesha no 10, Bandung, Indonesia Received 01 September 2015; received in revised form 11 October 2015; accepted 19 October 2015 Published online 30 December 2015 Abstract This paper presents a new formulation for obstacle and collision behavior on a group of humanoid robots that adopts walking behavior of pedestrian crowd. A pedestrian receives position information from the other pedestrians, calculate his movement and then continuing his objective. This capability is defined as socio-dynamic capability of a pedestrian. Pedestrian’s walking behavior in a crowd is an example of a sociodynamics system and known as Social Force Model (SFM). This research is trying to implement the avoidance terms in SFM into robot’s behavior. The aim of the integration of SFM into robot’s behavior is to increase robot’s ability to maintain its safety by avoiding the obstacles and collision with the other robots. The attractive feature of the proposed algorithm is the fact that the behavior of the humanoids will imitate the human’s behavior while avoiding the obstacle. The proposed algorithm combines formation control using Consensus Algorithm (CA) with collision and obstacle avoidance technique using SFM. Simulation and experiment results show the effectiveness of the proposed algorithm. Keywords: humanoid robots; formation control; obstacle avoidance; social force model; consensus algorithm. I. INTRODUCTION This paper propose a new approach to solve obstacle avoidance problem on a group of humanoid robots by combining of consensus algorithm and sociodynamic approach. Sociodynamics is a systematic approach to mathematical modeling in the social sciences. Sociodynamics has been developed starting from interdisciplinary approach that attempts to model the dynamic behavior of the social system of stochastic and quasi-deterministic models into more structured physical-mathematical system. The term of socio-dynamic is introduced by Weidlich, as quoted in [1]. The goal of this new approach is to make a group of humanoid robots can walk to desired position and still able to avoid obstacle while still maintaining their path to their desired position. The new approach is using Social Force Model (SFM) approach to make robots able to avoid obstacle and collision. SFM itself is a pedestrian’s walking behavior dynamic mathematical model developed by Helbing and Molnar [2]. The implementation of human behavior in humanoid’s behavior is based on the premise that, in the next few years, a humanoid robot will be placed on the human environment. So, if a robot will be placed in a human environment/crowd, it must have some knowledge of human behavior and capable to imitate and calculate it into its behavior. By using the SFM, the robot’s walking behavior is expected to be able to imitate the behavior of pedestrians in a crowd. The social force captures the effect of the neighboring pedestrians and the environment on the movement of individuals in the crowd. Helbing [2] used the SFM approach into collective model of social panic to simulate the behavior of an escape panic of a crowd. In this model, both * Corresponding Author. Tel: +62-8562156807 E-mail: asadiyoko@yahoo.com/alfa51@unpar.ac.id http://dx.doi.org/10.14203/j.mev.2015.v6.67-74 A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 68 psychological and physical effects are considered in formulating the behavior of the crowd. Since our research’s aim is implementing the obstacle/collision avoidance algorithm on a group of humanoid robots, a capable algorithm is needed to assemble the robots into a group. For this purpose, an algorithm called the Consensus Algorithm was used [3]. Consensus Algorithm (CA) is a distributed algorithm for multi-agent system to achieve an agreement on the information states of each agent. Its implementation in the field of robotics today is very much developed. By using consensus algorithm, a robot group can perform various tasks together including formation control, attitude alignment, foraging, rendezvous and cooperative search. When multiple robots agree on the value of a variable of interest, they are said to have reached consensus. To achieve consensus there must be a shared variable of interest, called the information state, which represents an instantiation of the coordination variable for the team. For this research, the information states are robot’s position, the center and shape of a formation and the direction of motion. Robots update the value of their information states based on the information states of their neighbors. The aim of consensus algorithm is to design an update law so that the information states of all the robots in the network converge to a common value [4]. This paper is organized as follows, the problem statement and formulation are described in Section II. The method and basic theory of SFM, CA for formation control, obstacle and collision avoidance techniques and stability analysis of the proposed algorithm, and system architecture are described in Section III. Some simulation and experiment results are shown in Section IV. Finally, Section V concludes this paper. II. PROBLEM STATEMENTS AND FORMULATION Given is a formation composed of three robots with a known virtual center as illustrated in Figure 1. In Figure 1, R1, R2, R3, R4 stand for Robot1, Robot2, Robot3, and Robot4. A position variable 𝑟𝑖 = [𝑥𝑖,𝑦𝑖] 𝑇, 𝑟𝑖 𝑑 = [𝑥𝑖 𝑑,𝑦𝑖 𝑑] 𝑇 and 𝑟𝑜𝑏𝑠 = [𝑥𝑜𝑏𝑠,𝑦𝑜𝑏𝑠] 𝑇 represents, respectively, the i-th robot’s actual position, robot’s desired position and obstacle’s position. The variable 𝑟𝑗 𝑑 = [𝑥𝑗 𝑑,𝑦𝑗 𝑑] 𝑇 and 𝑟𝑗 = [𝑥𝑗,𝑦𝑗] 𝑇 represent the j-th robot’s actual and desired position, and 𝑟𝑗𝐹 𝑑 = [𝑥𝑗𝐹 𝑑 ,𝑦𝑗𝐹 𝑑 ] 𝑇 represents the desired deviation of the j-th robot relative to 𝐶𝐹, where: [ 𝑥𝑗 𝑑(𝑡) 𝑦𝑗 𝑑(𝑡) ] = [ 𝑥𝑐(𝑡) 𝑦𝑐(𝑡) ]+ [ 𝑐𝑜𝑠[𝜃𝑐(𝑡)] −𝑠𝑖𝑛[𝜃𝑐(𝑡)] 𝑠𝑖𝑛[𝜃𝑐(𝑡)] 𝑐𝑜𝑠[𝜃𝑐(𝑡)] ][ 𝑥𝑗𝐹 𝑑 (𝑡) 𝑦𝑗𝐹 𝑑 (𝑡) ] (1) 𝐶𝐹 is coordinate frame on position 𝑟𝑖, where the x-axis is coincide with the orientation of the robot. This coordinate frame transformation is illustrated in Figure 2. Since research is focused only on robots and group behavior, the dynamics of robots as a single integrator system were considered, which is given by: 𝑢𝑖 = �̇�𝑖 (2) where 𝑟𝑖 ≜ [𝑥𝑖,𝑦𝑖] 𝑇 and �̇�𝑖 denote the position and velocity of the i-th robot, and 𝑢𝑖 is the control input and 𝑟𝑖 𝑑 ≜ [𝑥𝑖 𝑑,𝑦𝑖 𝑑] 𝑇 as target or desired position of 𝑟𝑖 . The static obstacle is defined as 𝑟𝑜𝑏𝑠 ≜ [𝑥𝑜𝑏𝑠,𝑦𝑜𝑏𝑠] 𝑇 and 𝑅𝑠𝑎𝑓𝑒, respectively as the position of an obstacle and its radii. All robots are connected with a communication topology as describe with graph )( nnn EVG  , where },...,1{ nVn  is the node set and nnn VVE  is the edge set, representing robots and its communication links. The communication topology among robots is illustrated in Figure 3. Figure 1. A formation composed of three robots with a known virtual center & an obstacle Figure 2. Frame coordinate transformation from global into robot’s frame coordinate A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 69 Given the initial configuration as shown in Figure 1, the objective of the system are: • All robot can walk from an initial position to their desired position, in a certain formation. R1, R2, and R3 are placed on the left side, and R4 is placed on the right side of the experimental platform. A control input 𝑢𝑖 will make i-th robot walks from 𝑟𝑖 to 𝑟𝑖 𝑑 as 𝑡 → ∞. • All robots can avoid obstacle while they walk along the way to reach their desired destination. • For obstacle avoidance, collision and obstacle avoidance factor from SFM equation were used. By using the observation results of Moussaïd et al. [5] as a comparison, the expected results of the experiment of this new algorithm will resemble the behavior as depicted in Figure 4. Figure 4 shows the results of computer simulations for the heuristic pedestrian model (solid lines) compared with experimental results (shaded lines) during simple avoidance maneuvers in a corridor. Part (A) shows the average trajectory of a pedestrian passing a static obstacle in the middle of the corridor; and part (B) shows the average trajectory of a pedestrian passing another individual moving in the opposite direction. III. METHOD This section describes the method that is used to solve the problem, begin with the basic theory of SFM, CA for formation control, obstacle/collision avoidance techniques and stability analysis. A. Social Force Model According to Helbing et al. [2], the motion behavior of a pedestrian is determined by some factors, which are: (i) individual desired direction, (ii) some influences from other pedestrians, (iii) some influences from obstacles, walls or other objects, and (iv) influence of an attractive object. The general SFM equation can be written as: 𝐹𝑖(𝑡) = 𝐹𝑖 0(𝑣𝑖,𝑣𝑖 0𝑒𝑖) + ∑ 𝐹𝑖𝑗(𝑒𝑖,𝑟𝑖 −𝑗 𝑟𝑗) + ∑ 𝐹𝑖𝑂(𝑒𝑖,𝑟𝑖 − 𝑟𝑂 𝑖)𝑂 + ∑ 𝐹𝑖𝑂(𝑒𝑖,𝑟𝑖 − 𝑟𝑂 𝑖)𝑂 (3) The first term, 𝐹𝑖 0(𝑣𝑖,𝑣𝑖 0𝑒𝑖), in equation (3) represents pedestrian’s individual desired direction, where 𝑣𝑖, 𝑣𝑖 0 , and 𝑒𝑖 represents, respectively, actual speed, desired speed, and desired direction of pedestrian i. The second term, 𝐹𝑖𝑗(𝑒𝑖,𝑟𝑖 − 𝑟𝑗) represents the influence of other pedestrian to pedestrian i, where 𝑟𝑖 and 𝑟𝑗 represent position of pedestrian i and j. In particular, the pedestrian keeps a certain distance from other pedestrian and to avoid collision, depends on the desired speed (𝑣𝑖 0) and pedestrian density. A repulsive effect of other pedestrian j is denoted in this term. The third and fourth term represent, respectively, a repulsive effects of an obstacle 𝐹𝑖𝑂(𝑒𝑖,𝑟𝑖 − 𝑟𝑂 𝑖) and an attractive effect of an attractive object/person, 𝐹𝑖𝐴(𝑒𝑖, 𝑟𝑖 − 𝑟𝐴, 𝑡). Pedestrians are sometimes attracted by other persons (friends, street artists, commercials, etc). Since research focus on obstacle/collision avoidance behavior and formation control, the fourth term from equation (3) was excluded. So, by using this simplification and equation in [3], the avoidance behavior part of SFM is written as: ∑ 𝐹𝑖𝑗(𝑒𝑖,𝑟𝑖𝑗)𝑗 = 𝑤𝑑𝐶𝑑𝑒 0.5√(‖𝑟𝑖𝑗‖+‖𝑟𝑖𝑗−s‖) 2 −s2 (4) and ∑ 𝐹𝑖𝑂(𝑒𝑖,𝑟𝑖 − 𝑟𝑂 𝑖)𝑂 = 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠)/𝐵 (5) where 𝐵,𝐶𝑠,𝐶𝑑 are positive scalar, 𝑟𝑖𝑗 ≜ (r𝑖 − r𝑗) , 𝑠 is step distance of the robot and 𝑟𝑜𝑏𝑠is position of an obstacle. The repulsive effects of equations (4) and (5) only hold for situation that are perceived in the Figure 3. Communication topology from the virtual center to all robots in the system Figure 4. Comparison between simulations with experimental results of the heuristic pedestrian walking model during simple avoidance maneuvers in a corridor [6] A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 70 pedestrian’s field of view (FOV, 2φ). In order to take this effect of perception into account, it need to introduce the direction dependent weights 𝑤𝑠 and 𝑤𝑑: 𝑤(𝑒,𝑓) ∶= { 1, if𝑒 ∙ 𝑓 ≥ ‖𝑓‖cosφ c, 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒. (6) By replacing (4) and (5) into (3), SFM equation can be derived as: 𝐹𝑖(𝑡) = 𝐹𝑖 0(𝑣𝑖,𝑣𝑖 0𝑒𝑖) + ∑ 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠)/𝐵 𝑂 + ∑ 𝑤𝑑𝐶𝑑𝑒 0.5√(‖(r𝑖−r𝑗)‖+‖(r𝑖−r𝑗)−s‖) 2 −s2 𝑗 (7) Comparing with the other obstacle/collision avoidance equations which were used in some algorithms [6-9], the use of FOV in this algorithm will become a distinctive factor, with the others. In the experiment, the value of φ is set to 60°. The avoidance behavior of the robots should be acted differently. The first term of equation (7) is the formation control term which will maintain robot position on a formation or still keeping them in a group. This formation control term will be described in the next section. B. Consensus Algorithm for Formation Control Consensus algorithm (CA) is a distributed control algorithm for multi-agent systems, which allow each agent in the system to achieve agreement with the other agents by sharing its information states. CA is a major method to solve many multi-agent cooperative control problems. Currently, CA has been developed and used in many applications of multi-robot systems. This is because the algorithm is distributive, so that the control equation for the robot can be simpler than the centralized control method. A necessary condition to achieve consensus is the availability of a communication topology that allow the information states are shared to all member of the group. If a communication topology in a multi-robots system is established for all robots, then consensus will be achieved if and only if the topology has a spanning tree [4]. In the case of formation establishment and control, some information states are needed to be shared. In this paper, virtual structure (VS) approach to solve the formation control problems was used. Using this approach, the entire formation is treated as a rigid body or single structure, and then, the control strategy is derived in three stages [3]: 1. Stage 1: define the desired dynamics of the virtual leader/virtual center of a virtual struc- ture. This stage is illustrated in Figure 1. 2. Stage 2: translate the motion of the virtual leader/virtual center into desired motion for each robot. This stage is also illustrated in Figure 1. 3. Stage 3: derive tracking controls for each robot. Since the research used 4 robots, a triangle formation for the group of 3 robots (R1, R2, and R3) was defined. The 4th robot will be acted as dynamic obstacle. In the group, the information states are shared to all robots by using communication topology depicted in Figure 3, while R4 received the position of the other robots and the obstacle. All information states (i.e. 𝑟𝑖,𝑟𝑗,𝑟𝑜𝑏𝑠 ) are provided by a Visual Localization Module (VLM), which consists of a web camera and a PC. VLM uses visual odometry (VO) technique to obtain all robots and obstacle positions and then share it to all robots. By taking the capability of robots used in the experiment into account, VO is regarded as the most appropriate technique to apply in the experiment. The illustration of the experiment is depicted in Figure 5. The algorithm of VO technique is depicted in Figure 6. Figure 5. Illustration of the experiment Figure 6. The algorithm of visual odometry technique A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 71 For stage 3 of the VS approach, CA equation for reference tracking was used, which can be written as in equation (8). All position states (𝑟𝑖,𝑟𝑗,𝑟𝑜𝑏𝑠) are obtained by using VO. 𝑢𝑖 = �̇�𝑖 𝑑 − 𝛼𝑖(𝑟𝑖 − 𝑟𝑖 𝑑) − ∑ 𝑎𝑖𝑗[(𝑟𝑖 − 𝑟𝑖 𝑑) −𝑛𝑗=1 (𝑟𝑗 − 𝑟𝑗 𝑑)] (8) where 𝛼𝑖 is a positive scalar, 𝑎𝑖𝑗 is the (𝑖, 𝑗) entry of adjacency matrix 𝐴𝑛 associated with communication topology 𝐺𝑛 and 𝑟𝑖 𝑑 = [𝑥𝑖 𝑑,𝑦𝑖 𝑑] 𝑇 is the i-th robot’s desired position. While, 𝑟𝑖 − 𝑟𝑖 𝑑 and 𝑟𝑗 − 𝑟𝑗 𝑑 represents respectively, distance between the i-th robot’s actual and desired position, the j-th robot’s actual and desired position. C. Obstacle/Collision Avoidance Technics To implement SFM into robot’s behavior, CA equation (8) was integrated into equation (7), so that equation (7) is expanded to equation (9). 𝑢𝑖 = �̇�𝑖 𝑑 − 𝛼𝑖(𝑟𝑖 − 𝑟𝑖 𝑑) − ∑ 𝑎𝑖𝑗[(𝑟𝑖 − 𝑟𝑖 𝑑) −𝑛𝑗=1 (𝑟𝑗 − 𝑟𝑗 𝑑)] + 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠)/𝐵 + 𝑤𝑑𝐶𝑑𝑒 0.5√(‖𝑟𝑖𝑗‖+‖(𝑟𝑖𝑗)−s‖) 2 −s2 (9) Equation (9) is the final equation to be implemented into humanoids robot. The obstacle and collision avoidance force are respectively presented by the fourth and fifth term of equation (9). The stability analysis of this algorithm is derived by using Lyapunov’s stability analysis and will be explained in the next subsection. To implement equation (9) to robot, it need 2 more processes, which are: frame coordinate transformation and limitation process of the robot’s steps and orientation. Since robot Nao has some limitation on its moves, the research try to imitate pedestrian’s walking behavior that is tend to be a non-holonomic behavior, treat and reprogram Nao as a non-holonomic robot. To make a leg movement on Nao, control input given by equation (9) must not exceed robot’s maximum foot step parameters, which are 0.08 m to step forward (X-axis) and 0.06 m to step aside (Y-axis). Control input 𝑢𝑖 is transformed into foot step 𝑢𝑖𝑥 and 𝑢𝑖𝑦 where 𝑢𝑖𝑥 ≤ 0.08 and 𝑢𝑖𝑦 ≤ 0.06. By using this foot step parameter, the robot’s maximum steps is set to 𝑠𝑗 = 0.08. As a result, robot will move in its maximum velocity if 𝑢𝑖𝑥 > 0.08 or 𝑢𝑖𝑦 > 0.06. D. Stability Analysis In this subsection, will be carried out analysis of the stability of equation (9). The purpose of this analysis is examining equation (9), to ensure the robot able to avoid obstacles and still returning to its mission toward its desired destination. The analysis was performed using Lyapunov stability analysis approach. To understand the analysis, some assumptions and definition are needed. Throughout this section, a system of nonlinear differential equations was considered. �̇� = 𝑓(𝑥), 𝑥(𝑡0) = 0 (10) where 𝑥,𝑥0 ∈ ℝ 𝑛 and 𝑓(∙):ℝ𝑛 → ℝ𝑛. 1) Definition a) Equilibrium point (𝑥∗) : 𝑥∗ is said to be an equilibrium point of equation (10) if 𝑓(𝑥∗) = 0. b) Stable Equilibrium: The equilibrium point 𝑥∗ = 0 is said to be a stable point of equation (10) if, for all 𝜖 > 0, there exists a 𝛿(𝜖) such that ‖𝑥0‖ < 𝛿(𝜖) ⇒ ‖𝑥(𝑡)‖ < 𝜖,∀𝑡 ≥ 𝑡0 where 𝑥(𝑡) is the solution of equation (10). c) Asymptotic Stability: The equilibrium point 𝑥∗ = 0 is said to be an asymptotically stable point of (10) if: (a) it is stable; (b) it is attractive, i.e. there exists a 𝛿 such that: ‖𝑥0‖ < 𝛿 ⇒ lim 𝑡→∞ ‖𝑥(𝑡)‖ = 0, where 𝑥(𝑡) is the solution of equation (10). Note that (a) above does not necessarily imply (b). d) Locally Positive Definite Function: A continuous function 𝑉(𝑥): ℝ𝑛 → ℝ+ is called a locally positive definite function if, for some ℎ > 0 and 𝛼(∙),𝑉(0) = 0 and 𝑉(𝑥) ≥ 𝛼(‖𝑥‖) ∀𝑥:‖𝑥‖ < ℎ where 𝛼(∙): ℝ𝑛 → ℝ+ is continuous, strictly increasing, and 𝛼(0) = 0. 2) Assumptions a) The robot’s radii (𝑅𝑟𝑜𝑏𝑜𝑡) is 0.2 m. b) The maximum robot’s walking step (𝑠𝑗 𝑚𝑎𝑥) is 0.08 m. Following the works of [4] and [10], analysis is started by noting that: 𝐹𝑖 = 𝐹𝑖 𝑎𝑡𝑡 + 𝐹𝑖 𝑟𝑒𝑝 (11) where 𝐹𝑖 𝑎𝑡𝑡 is the attractive potential function and 𝐹𝑖 𝑟𝑒𝑝 is the repulsive potential function of the i-th robot. Intuitively and necessarily, potential functions should have the properties that. { 𝐹𝑖 𝑎𝑡𝑡(0) = 0, ∇𝐹𝑖 𝑎𝑡𝑡(𝑟𝑖 − 𝑟𝑖 𝑑)| (𝑟𝑖−𝑟𝑖 𝑑)=0 = 0, 0 < 𝐹𝑖 𝑎𝑡𝑡(𝑟𝑖 − 𝑟𝑖 𝑑) < ∞,if ‖𝑟𝑖 − 𝑟𝑖 𝑑‖ ≠ 0 is finite, ‖∇𝐹𝑖 𝑎𝑡𝑡(𝑟𝑖 − 𝑟𝑖 𝑑)‖ < +∞ if ‖𝑟𝑖 − 𝑟𝑖 𝑑‖ is finite (12) A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 72 and { 𝐹𝑖 𝑟𝑒𝑝 (𝑟𝑖 − 𝑟𝑜𝑏𝑠) = 0, if(𝑟𝑖 − 𝑟𝑜𝑏𝑠) ∉ 𝑆𝑜𝑏𝑠, 𝐹𝑖 𝑟𝑒𝑝 (𝑟𝑖 − 𝑟𝑜𝑏𝑠) ∈ (0,∞), if (𝑟𝑖 − 𝑟𝑜𝑏𝑠) ∈ 𝑆𝑜𝑏𝑠. (13) It can be defined that: 1. 𝐹𝑖 𝑎𝑡𝑡 > 𝐹𝑖 𝑟𝑒𝑝 , if ‖𝑟𝑖 − 𝑟𝑜𝑏𝑠‖ > 𝑅𝑠𝑎𝑓𝑒, 2. 𝐹𝑖 𝑎𝑡𝑡 < 𝐹𝑖 𝑟𝑒𝑝 , if ‖𝑟𝑖 − 𝑟𝑜𝑏𝑠‖ < 𝑅𝑠𝑎𝑓𝑒, 3. 𝐹𝑖 𝑎𝑡𝑡 = −𝐹𝑖 𝑟𝑒𝑝 , if ‖𝑟𝑖 − 𝑟𝑜𝑏𝑠‖ = 𝑅𝑠𝑎𝑓𝑒 ⟹ equilibrium point (𝑟𝑖 ∗). where 𝑆𝑜𝑏𝑠 is an area around the obstacle, defined as a circle with a radius 𝑅𝑠𝑎𝑓𝑒. This condition is illustrated in Figure 7. Since this paper focuses on obstacle/collision avoidance, only 𝐹𝑖 𝑟𝑒𝑝 term was considered for analysis. Using properties in equation (11) can be rewrote to equation (10) where: 𝑢𝑖 𝑟𝑒𝑝 = 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠)/𝐵 + 𝑤𝑑𝐶𝑑𝑒 0.5√(‖(r𝑖−r𝑗)‖+‖(r𝑖−r𝑗)−𝑠𝑗‖) 2 −𝑠𝑗 2 (14) Focused on obstacle/collision avoidance term, define Lyapunov-like function candidate for (14) as 𝑉 = 𝑢𝑖 𝑟𝑒𝑝 , where: 𝑉 = 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠) 𝐵 + 𝑤𝑑𝐶𝑑𝑒 0.5√(‖𝑟𝑖𝑗‖+‖𝑟𝑖𝑗−𝑠𝑗‖) 2 −𝑠𝑗 2 (15) Assumption III.1 implies that 𝑟𝑖𝑗 > 2𝑅𝑅𝑜𝑏𝑜𝑡 > 𝑠𝑗. Because 𝑤𝑠,𝑤𝑑, 𝐶𝑠, and 𝐶𝑑 are positive scalar, it is obvious that 𝑉 is a positive definite function. To find the derivative function of 𝑉,𝑉 should be seen as 𝑉 = 𝑉𝑎 + 𝑉𝑏where : 𝑉𝑎 = 𝑤𝑠𝐶𝑠𝑒 (𝑟𝑖−𝑟𝑜𝑏𝑠)/𝐵, (16) 𝑉𝑏 = 𝑤𝑑𝐶𝑑𝑒 0.5√(‖(r𝑖−r𝑗)‖+‖(r𝑖−r𝑗)−𝑠𝑗‖) 2 −𝑠𝑗 2 (17) Then, by using derivative calculation, the derivative functions of 𝑉𝑎 and 𝑉𝑏 are given by: �̇�𝑎 = − 𝑤𝑠𝐶𝑠 𝐵 𝑒−‖𝑟𝑖−𝑟𝑜𝑏𝑠‖/𝐵 < 0 (18) and �̇�𝑏 = −0.5𝑤𝑑𝐶𝑑𝑒 −0.5√(‖𝑟𝑖𝑗‖+‖r𝑖𝑗−𝑠𝑗‖) 2 −𝑠𝑗 2 ( ‖𝑟𝑖𝑗‖−𝑠𝑗 ‖𝑠𝑗−𝑟𝑖𝑗‖ + 𝑟𝑖𝑗 ‖𝑟𝑖𝑗‖ )(‖𝑠𝑗 − 𝑟𝑖𝑗‖ + ‖𝑟𝑖𝑗‖) < 0 (19) E. System Architecture In this subsection, will be described how to implement the new algorithm into a robot. The VS approach needs a special architecture that can perform those three steps. So, the system architecture is built by using three hierarchical layers: a consensus tracking module, a consensus-based formation control module, and the physical robot control module. The elaboration of virtual structure approach into architecture for formation control with obstacle/collision avoidance system is shown in Figure 8. Figure 7. Illustration of obstacle/collision avoidance and formation control Figure 8. The elaboration of VS approach into distributed architecture for obstacle/collision avoidance and formation control [3] A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 73 Figure 9 shows the robot which was used in the experiment. The robots used for experiment are Nao robot from Aldebaran Robotic, France. IV. RESULT AND DISCUSSION In this section, some simulation and experiment results of the application of the proposed equation on a group of humanoid robots were presented. As mentioned before, robot’s dynamics is assumed as a single integrator system. For simulation and experiment, 3 robots are placed in the left side of the experiment area and 1 robot on the right. An obstacle also placed randomly in the middle of the area. The group of robots walk to their destination on the right side and the 4th robot walks to left side of the experiment area. A. Simulation Result Simulations were performed on 4 agents, representing 4 robots, using topology in Figure 3. The initial position of Robot1, Robot2, Robot3, and Robot4 are, respectively, 𝑟𝑅1 = [0.7300,1.1905] , 𝑟𝑅2 = [−0.0081,1.2198] , 𝑟𝑅3 = [−0.0149,0.4276], and 𝑟𝑅4 = [7.1790, 0.4428]; while the obstacle is placed at position 𝑟𝑂𝑏𝑠𝑡 = [2, 0.6]. All these positions are obtained by using VO, which the algorithm is depicted in Figure 6. As seen in Figure 9, all robots wear a marker on their head. Figure 10 shows the simulation result of the proposed algorithm. The result shows that when Robot1 met the safety barrier ( 𝑅𝑠𝑎𝑓𝑒 ), it started to avoid the obstacle. During Robot1 was avoiding the obstacle (point [a]), Robot2, and Robot3, also perform an avoidance maneuvers, although there is no obstacle in front of them. This is because the consensus term has worked while the avoidance term has not active yet. This can be seen in the behavior of Robot3, as shown in point [b]. At point [c], it is shown that Robot2 is following the formation of Robot1, but must meet the 𝑅𝑠𝑎𝑓𝑒 of the obstacle. Robot2 reacts to turn left, but at point [d] he met with dynamic obstacle (Robot4). The reaction of Robot3 is keeping its maneuver by continuing to turn left, while Robot4 being avoiding static obstacle turn to the right [e]. As result, the two robots constantly avoiding each other, until at some point one of them sense the absence of the obstacle. It’s shown in point [f], where Robot4 and Robot3 sensed the absence of the obstacle. Robot4 was continuing its mission towards point [0, 0.6] and Robot3 was returning back to its formation [g]. B. Experiment Result Experiments were performed on 4 Nao robot, using topology in Figure 3. An ASUS RT-N10 router, a Genius F-120 web camera and a computer were used to perform the experiment. The video of this experiment can be watched on Youtube channel [11]. The experiment result is depicted in Figure 11 showing that the experiment result also get a similar result to the simulation. Figure 9. Aldebaran’s Nao robots for the experiment Figure 10. Simulation result Figure 11. Experiment result A. Sadiyoko et al. / J. Mechatron. Electr. Power Veh. Technol. 06 (2015) 67-74 74 V. CONCLUSION In this research, a new algorithm for obstacle and collision avoidance on a group of humanoid robots inspired by SFM is successfully developed. Stability analysis on the new algorithm has proved that algorithm can make a group of humanoid robots avoid obstacle. Comparing to our previous results, this algorithm has a smoother avoidance maneuver and faster to return to its formation. It also found in this study that the CA part on the algorithm is succeeded to maintain the position of the robot back to its formation. In the case of robot is trapped in a crowded situation (singularity condition), robot will still trying to look a new position, until it find a condition that allow him to move forward. ACKNOWLEDGEMENT The authors would like to thank to Advanced Robotic Laboratory at School of Electronic Engineering and Informatics, Institut Teknologi Bandung for providing all robots and facilities for the research. REFERENCES [1] Weidlich, W. "Sociodynamics – A systematic approach to mathematical modelling in the social sciences," An Interdisciplinary Journal on Nonlinear Phenomena in Complex Systems, vol. 5, no. 4, 2002, pp. 479-487. [2] D. Helbing and P. Molnár. "Social force model for pedestrian dynamics," Physical Review E, vol 51, May, 1995, pp. 4282– 4286. [3] A. Sadiyoko et al., “Consensus algorithm for obstacle avoidance and formation control,” Proceedings of 10 th Asian Control Conference, 2015. [4] Cao et al., “An overview of recent progress in the study of distributed multi-agent coordination,” IEEE Trans. On Industrial Informatics, Vol. 9, No. 1, 2013, pp. 427- 438. [5] Moussaïd et al., “How simple rules determine pedestrian behavior and crowd disasters,” Proceedings of the National Academy Science of the USA, vol. 108, no. 17, 2011, pp. 6884-6888. [6] M. Casini et al., “A LEGO mindstorms multi-robot setup in the automatic control telelab,” Proc. of the 18 th World Congress of the International Federation of Automatic Control (IFAC2011), 2011, pp. 9812-9817. [7] M. Ho et al., “Collision free cooperative navigation of multiple wheeled robots in unknown cluttered environments,” Robot. Auton. Syst., Vol. 60, No. 10, October 2012, pp. 1253-1266. [8] D. Panagou et al., “Multi-objective control for multi-agent systems using Lyapunov- like barrier functions,” Proceedings of the 52 nd Annual Conference on Decision and Control (CDC), 2013, pp. 1478-1483. [9] Z. Chao et al., “Collision-free UAV formation flight control based on nonlinear MPC,” Proceedings of the IEEE International Conference on Electronics, Communication & Control, 2011, pp. 1951- 1956. [10] J. Chunyu et al., “A new reactive target- tracking control with obstacle avoidance in a dynamic environment,” Proceedings of the 2009 American Control Conference, 2009, pp. 3872-3877. [11] A. Sadiyoko, “Nao: robots formation control, obstacle & collision avoidance,” https://youtu.be/2793RwcAmGM, 2015.