Formation Control of Mobile Robots with Obstacle Avoidance using Fuzzy Artificial Potential Field

Transcription

1 Formation Control of Mobile Robots with Obstacle Avoidance using Fuzzy Artificial Potential Field Abbas Chatraei Department of Electrical Engineering, Najafabad Branch, Islamic Azad University, Najafabad, Iran. abbas.chatraei@gmail.com Hossein Javidian Department of Electrical Engineering, Najafabad Branch, Islamic Azad University, Najafabad, Iran. hosseinjavidian@gmail.com Abstract In this paper, a combined formation control of nonholonomic wheeled mobile robots (WMR) is presented. In this method, the sliding mode control (SMC) strategy is used to solve the tracking problem of any robot agent. In our scenario, we assume the static and dynamic obstacles in the working environment. For guaranteeing obstacle avoidance, we use the artificial potential field method in which the coefficient of attractive and repulsive functions is determined by a Mamdani fuzzy system. Moreover, in order to maintain the desired formation considered between WMRs a fuzzy system is used to control the position and orientation of any agent with respect to the desired formation. The efficiency and simplicity of proposed control scheme has been proved by simulation on different situations. I. INTRODUCTION In the few recent years according to the application of multi-agent systems in the various tasks such as collaborating in exploring an environment [1, and etc., the attention of researchers have been focused on controlling of such systems. In this area several control techniques including various Lyapunov based methods have been developed. In [2 a geometric framework has been used to solve the control problem of leader-follower formation. In order to control the separation and bearing of agents, the fuzzy control can be utilized to keep constant relative distance and constant angle to the leader robot [3. Some researchers focused their study on developing backstepping controller for leaderfollower based formation control of nonholonomic WMRs [4 [6. In [7 an artificial potential field based navigation and leader-follower formation control scheme has been proposed in which the formation control of a group of differentially driven wheeled mobile robots has been studied. The researchers of [8 suggested a new framework based on the leader-follower and cascade system by which one can simplify the communication difficulties to design controllers of a group of WMRs. The real-time navigation of WMRs was worked in [9 in which the fuzzy logic technique, wireless communication and MATLAB were used to control a WMR in an obstacle ridden environment. The proposed tracking controller represented by researchers can be categorized into four classes : linear, nonlinear, geometric and intelligent approaches [10. In the aforementioned literature, a perfect case in which some followers follow a leader WRM with a desired formation, in an environment with static and dynamic obstacles has not been studied. In this paper we considered all of these case in our research. Hence, the proposed approach presented in this paper deals with the formation control of WMRs in the presence of static and dynamic obstacles. Accordingly, each agent tracks a prespecified trajectory while it has to maintain its pose in considered formation. The trajectory tracking problem is solved by utilizing sliding mode control methodology. In addition, avoiding collisions with static and dynamic objects as well as other mobile robots by each agent must be guaranteed. In fact, obstacle avoidance has higher priority than trajectory tracking. It means that each WMR may temporarily leave the formation after passing obstacles and then back to the formation. Obstacle avoidance task is carried on using artificial potential field strategy and formation control is performed by a local fuzzy controller on each agent. The rest of this paper is organized as follows. In section II, the leader-follower formation control using artificial potential field is presented. Then, the applied SMC approach for trajectory tracking of leader and follower WMRs are addressed in section III. Fuzzy logic leader-follower control is discussed in section IV. After that, section V addresses the application of the proposed scheme for a group of non-holonomic robots while maintaining a desired formation. Eventually, we draw some conclusions in VI. II. LEADER FOLLOWER FORMATION CONTROL USING ARTIFICIAL POTENTIAL FIELD A. Problem Description Let us consider a nonholonomic mobile robot for which the aim is that it tracks a desired prespecified trajectory while guaranteeing to avoid with static and dynamic obstacles as well as other agents in the environment. Accordingly, We are given a group of WMRs modeled by the following equations ẋ = v cos (θ) ẏ = v sin (θ) θ = u where x, y R are the Cartesian coordinates, θ [0, 2π) is the orientation of the robot with respect to the world frame and v, u are the linear and angular velocity inputs, respectively. One member of this group is shown in Fig.1. In addition (1) /15/$31.00 c 2015 IEEE

2 Fig. 1: The electrically driven nonholonomic WMR., for tracking problem we consider the reference trajectory parametrized by x r, y r with bounded derivative. Assuming the reference trajectory given by, x r, y r, with bounded derivatives, one can define { E x = e x + V x E y = e y + V y where e x = x x r and e y = y y r are position errors in directions x and y, respectively. Then, the desired orientation can be obtained as θ r = Atan2( E y, E x ) (6) In addition, the orientation error is given as e θ = θ θ r in which θ r is the desired direction of motion that depends on the reference trajectory, the robot s position and the obstacle to be avoided by the robot. Note that in order to avoid singular cases, the following conditions must be met B. Avoidance Function In order to avoid any collision with static or moving obstacles, we define the distance d a as d a = (x x a ) 2 + (y y a ) 2 (2) where x a and y a are the coordinates of the object to be avoided. We address the obstacle avoidance problem using the following avoidance function V = (min{0, d2 a R 2 ) 2 d 2 a r 2 }, r > 0, R > 0, R > r (3) where r, R are radii of the avoidance and detection regions, respectively, as shown in Fig. 2. This function is infinite at the boundary of the avoidance region and is zero outside the detection region. The partial derivatives of V with respect to x and y are as follows [11 : Fig. 2: Avoidance (radius r ) and detection (radius R) regions around the robot. V 0 if d a < r x = 4(R 2 r 2 )(d 2 a R2 ) (d 2 a r2 ) (x x 3 a ) if R > d a > r 0 if d a R and V 0 if d a < r y = 4(R 2 r 2 )(d 2 a R2 ) (d 2 a r2 ) (y y 3 a ) if R > d a > r 0 if d a R (4) (5) The reference trajectory must be smooth and e θ π 2 For r d a < R it requires that ẋ r = ẏ r = 0 causing that as the robot detects an obstacle in its path, temporarily does not consider its tracking performance, It means that collision avoidance has a higher priority than tracking. In order to guarantee ˆ θr to be a sufficient smooth estimate of III. θ r = E xėy ĖxE y D 2, (7) the following inequality must be satisfied for small positive values T, ɛ where and D = E x (Ėy ˆĖ y ) E y (Ėx ˆĖ x ) D 2 ɛ (8) ˆĖ x = E x(t + T ) E x (t) T ˆĖ y = E y(t + T ) E y (t) T Ex 2 + Ey 2 SLIDING MODE CONTROL OF LEADER AND FOLLOWER WMRS In order to track the desired trajectory by leader and follower WMRs in a formation, the SMC approach can be employed. In this paper for ith follower robot, we use the following control law [14, [15 [ ei2 u i + v r cos(e i3 ) + tanh(s 1 /ɛ) + k 2 s 1 [ vi = u i u r+ α α vr+ vr e (v r sin(e i2 i3)+tanh(s 2/ɛ)+k 3s 2 1+ α e i2 where ɛ, k 2, k 3 are small positive numbers and 0 < a < 1, as well as α = arctan(v r e i2 ). In (9), the dynamics of error between actual and reference velocities (v r, u r ) T is given by e i = [ėi1 ė i2 ė i3 = [ ui e i2 v i + v r cos(e i3 ) u i e i1 + v r sin(e i3 ) u r u i (9) (10) 2

3 In addition, in (9) the sliding surfaces s 1 and s 2 can be considered as [ [ s1 e s = = i1 (11) s 2 e i3 + arctan(v r e i2 ) For proving the exponentially stability of the control system, we can consider the following Lyapunov function whose derivative is V i = 1 2 st s (12) V i = k 2 e 2 i1 k 2 e 2 i1e 2 i2 (e i3 + arctan(v r e i2 )) 2 0. (13) Therefore, the switching function can converge to zero in a finite time. When s 0, e i3 α and then the tracking posture error converges to zero. On the other hand, the following control law can be utilized for the jth follower WMR [14, [15 [ [ vj v = i cos(e j3 ) + k 4 e j1 + u j u i + (v i + k v )k 5 e j2 + (v i + k v )k 6 sin(e j3 ) [ γvj (14) γ uj where k 4, k 5, k 6, k v are positive real number. Note that the parameter k v ensures the asymptotic stability of the system. Other part of (14) is specified as γ vj = u i l ijd sin(ψ ijd + e j3 ) (15) γ uj = e j2 (u i (d j + l ijd ) + k 6 (v i + k v )d j + k v ) (1/k 5 )+ e j2 d j (16) In the equations (14) to (16), the distances d j, l ijd and angle ψ ijd are specified according to Fig.3. Fig. 3: Basic Leader-Follower setup. In order to prove that the control system of jth follower WMR is stable under the input we chose, the following Lyapunov function can be adopted V j = 1 2 (e2 j1 + e 2 j2) + 1 k 5 (1 cos(e j3 )) (17) where it is obvious that V j 0 and V j = 0 if and only if e j = 0. The time derivative V j of (17) will be as follows by using control law (14) V j = e j1 ( v j + v i cos(e j3 ) + ω j e j2 ω i l ijd sin(ψ ijd + e j3 )) + e j2 ( ω j e j1 + v i sin(e j3 ) d j ω j + ω i l ijd cos(ψ ijd + e j3 )) + 1 k 2 (ω i ωj) sin(e j3 ) k 4 e 2 j1 k 5 d j (v i + k v )e 2 j2 k 6 (v i + k v ) sin 2 (e j3 )+ e j2 (ω i (d j + l ijd ) + k v + k 5 1 k 6 (v i + k v )d j + (d j + k 5 e j2 )γ ωj) k 4 e 2 j1 k 5 d j (v i + k v )e 2 j2 k 6 k 5 (v i + k v ) sin 2 (e j3 ) 0 Hence, employing the velocity control input (14) is caused the asymptotical stability of the follower WMR s control system. IV. FUZZY LOGIC LEADER-FOLLOWER FORMATION CONTROL In order to keep the considered formation between agents, we utilize a fuzzy controller. First let us consider Fig. 4 in which θ L, θ F are headings of leader and follower WMRs, respectively. R (x, y, θ) is pose of a robot, in terms of coordinates (x, y) and robot s heading θ. L (x, y, θ) and F (x, y, θ) are poses of leader and follower WMRs, respectively. Distance d and angles α, θ L are specified as shown in this figure. For a robot to be in its position in a desired formation, the following conditions must be satisfied : The angle between the robots, α, must be equal to the heading of the leader, θ L. The heading of the follower, θ F, must be equal to the heading of the leader, θ L. The distance between the robots must be equal to a predefined value which is measurable. Let us define angle φ as φ = (θ L α) (θ L θ F ) = θ F α (18) When angle φ equates to zero, the robot is in the correct pose for a desired formation shape. Two inputs to the formation Fuzzy Logic Controller (FLC) are angle φ and distance d. Two outputs of formation FLC are linear and angular velocities v, u. In this FLC, we use trapezoidal membership function for inputs as shown in Figs 5 and 6. Moreover, the rules used in the FLC are as follows For angular velocity output : 3

4 V. SIMULATION RESULTS A. Wheeled Mobile Robot and Moving Obstacles With Fuzzy Artificial Potential Field: In order to verify the efficiency of proposed controller, simulation results are derived by developing algorithms in MATLAB environment. Fig. 4: The position leader-follower robot Fig. 5: Angle input(degrees). B. Leader-Follower Nonholonomic Robot without Obstacle We used five identical nonholonomic mobile robots is considered where leader s trajectory is the desired formation trajectory, and simulations are carried out in MATLAB under following two scenarios. In the first scenario, we impose the following control input to leader : v L = 1.6 m/s, u L = sin(1.2t) rad/s. Moreover, the initial conditions taken into account are q L = (5, 2, π/2) T, q F 1 = (3.5, 2.2, π/6) T, q F 2 = (5.5, 1.5, 2π/3) T, q F 3 = (3, 3, π/4) T and q F 4 = (6.5, 1, π/6) T. We consider desired value d 1 = 0.9 cm, d 2 = 0.9 cm, φ 1 = π/3 rad/s, and φ 2 = π/3 rad/s. For SMC, used parameters are k 1 = k 2 = 5,k v = 1, v F 1 = v F 2 = 4 m/s and sample time is dt = 0.01 s. Followers keep a certain distance and velocity track the leader vehicle, so we can regard it as four vehicles formation as shown in Fig. 7. Fig. 6: Distance input(meters). if φ = 0, Robot is in line with leader, then no action required. if φ > 0, Robot is too far to the right of the leader, then turn left. if φ < 0, Robot is too far to the left of the leader, then turn right. for translational velocity output : if d = 0, Robot is at correct distance, then no action required. if d > 0, Robot is too close to the leader, then slow down. if d < 0, Robot is too far away from the leader, then speed up. Note also that for outputs, we employ the singleton membership function. Fig. 7: Followers curvature Leader. For second scenario, we use circular trajectory tracking for leader and three followers wheeled mobile robots with same kinematics and dynamics equations. Thus initial positions and orientations for reference,leader and follower robots given by : v r = 0.4 (5 cos(π t/4)) m/s, u r = 0.1 (5 cos(pi t/4)) rad/s, q r = (3, 3, π/3) T, q l = (2, 2, π/4) T, q f1 = (1, 2, π/3) T, q f2 = (2, 0.7, π/6) T, d 1 = 2 cm, d 2 = 2 cm, k 1 = 20, k 2 = 10. Fig. 8 shows the formation control between leader and two followers. For this scenario, Fig. 10 shows linear and angular velocity of two follower WMRs. Moreover, fig. 9 shows error dynamics for linear and angular velocities followers goes to zero. In this scenario, we use a FLC whose rules are determined by three laser sensors mounted on the left, front and right sides of the robot as shown in Fig. 11. These rules have been summarized in TABLE I. 4

5 Fig. 11: Laser Input Range TABLE I: Rules required for collision avoidance Fig. 8: Trajectory tracking of two follower WMRs in a desired formation. Fig. 9: Error dynamics for linear and angular velocities of follower WMRS. Fig. 10: Linear and Angular Velocities of Leader and Followers. Rules Left Right F ront Rotation V elocity 1 near near near extremely right reverse 2 near near medium straight ahead normal 3 near far near straight ahead fast 4 near medium near very right reverse 5 near medium medium straight ahead very slow 6 near medium far very right normal 7 near far near very right reverse 8 near far medium very right very slow 9 near far far very right normal 10 medium near near very left reverse 11 medium near medium very left slow 12 medium near far very left slow 13 medium medium near very left reverse 14 medium medium medium right normal 15 medium medium far very right normal 16 medium far near very right reverse 17 medium far medium very right very slow 18 medium far far very right normal 19 far near near extremely left reverse 20 far near medium very left slow 21 far near far very left normal 22 far medium near very left reverse 23 far medium medium left normal 24 far medium far left normal 25 far far near very right reverse 26 far far medium right normal 27 far far far straight ahead normal In the first scenario, two followers follow a leader in a environment with static and dynamic obstacles as shown in Fig. 12. In this scenario, initial values for parameters are as follows. The amount of radius of the avoidance and detection regions are r = 0.3 cm, R = 1 cm, respectively. Moreover the rest parameters are : d 1 = 2.5 cm, d 2 = 2.5 cm, v l = 1 m/s, u l = 0.5 rad/s, φ d1 = π/3 rad/s, φ d2 = π/3 rad/s, q l = (2, 1, π/3) T, q f1 = (3, 0.5, π/4) T and q f2 = (0.3, 2.3, π/4) T. In the second scenario, two followers seek the leader in the presence moving obstacles in a circular refrence trajectory as shown in Fig. 13. Follower robots automatically make decision to turn the obstacle from left side or right side. Initial values for this scenario are : r d = 0.5 cm, R D = 1 cm, d 1 = 1.5 cm, d 2 = 1.5 cm, φ d1 = 2π/3 rad/s, φ d2 = 2π/3 rad/s, q r = (3, 3, π/3) T, q l = (2, 2, π/4) T, q f1 = (0.5, 2, π/3) T and q f2 = (2, 0.7, π/6) T. C. Simulation Leader-Follower Nonholonomic Robot with Static and Moving Obstacle: For simulation leader follower with static and dynamic obstacle, two scenarios are setup. VI. CONCLUSION In this paper, we have studied the problem of trajectory tracking and collision avoidance for non-holonomic systems. Accordingly, we developed a novel leader-follower formation 5

6 Fig. 12: Leader-Followers and static and dynamic Obstacles. Fig. 13: Trajectory Tracking of leader and follower WMRs with Dynamic Obstacles. control in which a group of follower WMRs seek a leader in a desired formation shape so that the leader tracks a reference trajectory and follower WMRs follow their leader in an environment with static and dynamic obstacles. The sliding mode control was used to trajectory tracking of agents and artificial potential field was responsible for avoiding the agents with obstacles as well as other WMRs. For keeping the formation shape while moving the robots, a fuzzy logic controller was employed. The simulation was conducted based on Matlab/Simulink. Simulation results in different situations indicate the proposed framework can work well. [2 L. Consolini, F. Morbidi, D. Prattichizzo and M. Tosques, On the Control of a Leader-Follower Formation of Nonholonomic Mobile Robots, 45th IEEE Conference on Decision and Control, San Diego, CA, pp , Dec [3 A. Bazoula, and H. Maaref, Fuzzy Separation Bearing Control for Mobile Robots Formation, International Journal of Mechanical, Aerospace, Industrial and Mechatronics Engineering, vol. 1, no. 5, pp , [4 T. Dierks, S. Jagannathan, Control of Nonholonomic Mobile Robot Formations : Backstepping Kinematics into Dynamics, Proceedings of the IEEE International Conference on Control Applications, Singapore, pp , [5 R. Fierro, F.L. Lewis, Control of a Nonholonomic Mobile Robot : Backstepping Kinematics Into Dynamics, Proceedings IEEE Conference Decision Control, Kobe, Japan, pp , [6 X. Li, J. Xiao, and Z. Cai, Backstepping Based Multiple Mobile Robots Formation Control, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp , [7 K. H, Kowdiki, R. K. Barai and S. Bhattacharya, Leader-Follower Formation Control Using Artificial Potential Functions : A Kinematic Approach, IEEE-International Conference On Advances In Engineering, Science And Management, pp , March [8 C. Guo, J. Qin, Y. Ge and Y. Chen, Leader-follower and cascade system based formation control of nonholonomic intelligent vehicles, The 26th Chinese Control and Decision Conference (2014 CCDC), Changsha, pp , March [9 H. Ramdane, M. Faisa, M. Algabri, K. Al-Mutib, Mobile Robot Navigation with Obstacle Avoidance in Unknown Indoor Environment using MATLAB, International Journal of Computer Science and Network, vol. 2, no. 6, pp , [10 I. M.H. Sanhoury, S. H.M. Amin and A. R. Husain3, Tracking Control of a Nonholonomic Wheeled Mobile Robot, Precision Instrument and Mechanology, vol. 1, no. 1, pp. 7-11, [11 S. Mastellone, D.M. Stipanovic, C.R. Graunke, K.A. Intlekofer and M.W. Spong, Formation Control and Collision Avoidance for Multiagent Nonholonomic Systems : Theory and Experiments, Int. J. Rob. Res., vol. 27, no. 1, pp , [12 K. D. Aveek, R. Fierro, R. K. Vijay, J. P. Ostrowski, J. Spletzer, and C. J. Taylor, A Vision-Based Formation Control Framework, IEEE Transactions on Robotics and Automation, vol. 18, no.5, [13 A. Saffiotti, The Uses of Fuzzy Logic in Autonomous Robot Navigation, Soft Computing, vol. 1, no. 4, pp , [14 Z. P. Wang, W. R. Yang and G. X. Ding, Sliding mode control for trajectory tracking of nonholonomic wheeled mobile robots based on neural dynamic model, Second WRI Global Congress on Intelligent Systems, vol. 2, pp , [15 J. Sanchez and R. Fierro, Sliding mode control for robot formations, IEEE International Symposium on Intelligent Control, Houston, TX, USA, pp , REFERENCES [1 D. Fox, W. Burgard, H. Kruppa, and S. Thrun, A probabilistic approach to collaborative multi-robot localization, Autonomous Robots, vol. 8, no. 3, pp ,