Formation Control of Nonholonomic Mobile Robots

Transcription

1 Proceedings of the 6 American Control Conference Minneapolis, Minnesota, USA, June -6, 6 FrC Formation Control of Nonholonomic Mobile Robots WJ Dong, Yi Guo, and JA Farrell Abstract In this paper, formation control of a group of nonholonomic wheeled robots are considered By introducing a unified error of the formation and trajectory tracking, state feedback control laws are proposed for formation control with a desired trajectory Graph theory and Lyapunov theory are used in the control design After that, by introducing observers, output feedback control laws are also proposed for the formation control Simulation study shows the proposed controllers are effective I INTRODUCTION Cooperative control of multiple systems has received considerable attention recently due to its challenging features and many applications in rescue mission, moving a large object, troop hunting, formation control, and cluster of satellites Different control strategies have been proposed, which include the behavior based, virtual structure, leader following, and graph theoretical approaches While most existing results use linear vehicle dynamics to simplify control design, we study formation control of nonholonomic mobile robots, and design nonlinear state and output feedback control for a group of robots to achieve formation on given trajectories While a complete review of existing work on cooperative control is beyond of the scope of this paper, we mention a few methods that motivated our research Arkin studied cooperation without communication for multiple robots foraging and retrieving objects in a hostile environment [] In [9], Lewis and Tan proposed the virtual structure concept in formation control of mobile robots In [], the authors discussed the problem of coordinating multiple spacecraft to fly in tightly controlled formations using the virtualstructure method The leader-following approach was used in [], [], [8], [9] Some mobile robots are designated as leaders while others as followers The leaders track desired trajectories, and the followers track desired trajectories with respect to the leaders The advantage of this approach is its simplicity in that the reference trajectories of the leaders are pre-defined and the internal stability of the formation are guaranteed by the individual robot s control laws The graph theory approach was proposed for cooperative control of multiple linear systems by Fax and Murray [] Then, different control laws were designed with the aid of graph theory [8], [] Communication links among systems are described by Laplacian matrices Each vehicle is treated as a vertex and the communication links between vehicles are WJ Dong and JA Farrell are with Department of Electrical Engineering, University of California, Riverside, CA 9 s: wdong@eeucredu and farrell@eeucredu Yi Guo is with Department of Electrical and Computer Engineering, Stevens Institute of Technology, Hoboken, NJ 7 yguo@stevensedu treated as edges Stability of the whole system is guaranteed by stability of each modified individual linear system However, the methods are limited to linear systems The consensus problem is closely related to cooperative control and has been widely discussed recently In [6], cooperative laws were proposed using nearest neighbor rules In [], it was shown how to make a group of mobile robots converge to a line or general geometric form by solving the consensus problem In [], the consensus problem for networks of dynamic agents with fixed and switching topologies was discussed Two consensus protocols for networks with and without time-delays were proposed for convergence analysis in different communication cases In [], [7], the authors considered the problem of information consensus among multiple agents in the presence of limited and unreliable information exchange with dynamically changing interaction topologies Updated algorithms were proposed for information consensus in both discrete and continuous cases In addition, in [], a distributed smooth time-varying feedback control law is proposed for coordinating motions of multiple nonholonomic mobile robots of the Hilare-type to capture/enclose a target by making troop formations with the aid of averaging theory In this paper, we discuss the cooperative control of a group of mobile robots with a given formation and a desired trajectory as a group In order to solve the formation control problem, we first introduce a unified error which consists of the formation error and the tracking error It is shown that the mobile robots come into formation and move along the desired trajectory if the unified error converges to zero Based on the dynamics of the unified error, a state feedback control law is proposed for each robot, which renders the team into formation and asymptotically moves the team along the desired trajectory After that, we extend the design to the output feedback for situations full states are not available for control By introducing an observer for each robot, we design an output feedback controller for each robot to achieve the same control objective In contrast to existing results on linear vehicle systems, we focus on nonholonomic vehicle dynamics and design nonlinear control laws to achieve formation using Lyapunov techniques Simulations show satisfactory performances II PROBLEM STATEMENT Consider m mobile robots which are moving on a plane Without lose of generality, the mobile robots are indexed with,,, m For simplicity, we assume that each member of the group of mobile robots has the same mechanical structure, ie, they have the same kinematic model except ---7/6/$ 6 IEEE 6

2 that the geometric parameters may be different By the existing results on the mobile robots in [], the kinematics of the group of mobile robots are equivalent to the following canonical form by global or local state diffeomorphisms and input transformations q j = u j q ij = u j q i+,j, i n, j m () q nj = u j q ij ( i n) are the states of robot j and u j and u j are the control inputs of the robot j Specifically, (q j,q j ) are the position (x j,y j ) of robot j in the fixed world coordinate system States q ij ( i n) are the remaining states of robot j Represent the m mobile robots as m vertices in V of a graph G = {V, E} The communication between the robots can be described by the edge E of the graph G An edge (i, j) E means that the state of robot i is available to robot j Let N j be a collection of neighbors of robot j, ie, a set of indexes of robots whose states are available to robot j The information available for robot j in the control are only states of robot j and robot i for i N j Due to sensor range limitations and bounded bandwidth of communications between robots, the topology of the graph G may vary from time to time, which means N j is time-varying In the plane, the geometric form of the formation can be described either by relative positions between the robots or by position vectors of robots [] In the later approach, a desired formation F is described by the vector offset denoted by (h jx,h jy ) for vehicle j The offset after rotation and translation is relative to a time varying centroid trajectory T the centroid trajectory T is generated by the virtual mobile robot q = u q id = u q i+,d, i n () q nd = u d q id ( i n), u, and u d are known time-varying functions Our control problem is defined as follows Formation Control Problem: Design a controller for each robot based on its and its neighbor s states such that the group of robots comes into formation F and the center of the group of robots moves along the desired trajectory T That is, design control laws (u j,u j ) which are functions of q ij and q ik for k N j and i n such that the group of robots converges to the desired geometric formation F, ie, lim (q j(t) q i (t)) = h jx h ix, ( i j m) () t lim (q j(t) q i (t)) = h jy h iy, ( i j m), () t and the formation centroid moves along the desired trajectory T, ie, lim m q j (t) q (t) = t m j= () lim m q j (t) q d (t) = t m (6) j= In order to solve the formation control problem, we make the following assumption on u Assumption : lim t inf u (t) = ɛ> This assumption is not unusual and easily satisfied in practical control In (), we assume without loss of generality that q j and q j are the X and Y coordinates of robot j This assumption is purely for the convenience of presentation Without this assumption, similar results can still be developed III FORMATION CONTROLLER DESIGN In this section, we design a formation controller for each robot based on its own state and that of its neighbors To this end, define the variable transformation z j = q j q h jx + m l= h lx z j = q j q d h jy + m l= h ly z ij = q ij q id + α ij ( i n, j m) α j = k u ρ α ij = k i u ρ i + α i,j l= u [l] i z j z i,j z i,j + z i+,j u [l+] u + α i,j α i,j +z j )+ z lj z l,j + z l+,j ), ( i n) ( k l u ρ z lj ( k u ρ z j k i ( i ) are positive constants and ρ = n, we have ż j = u j u ż j = k u ρ z j + u z j +(u j u )β j ż j = k u ρ z j + u (z j z j )+(u j u )β j ( j m) ż,j = k u ρ z,j u z n,j + u z nj +(u j u )β,j ż nj = u j u d + α nj ( k u ρ z j + z j ) n α nj + ( k l u ρ z lj z l,j + z l+,j ) z lj +(u j u )β nj (9) β j = z j α j + q d, β ij = z i+ α i+ + q i+,d + i α ij l= z lj β lj, ( i n) (7) (8) 6

3 Lemma : For the state variable transformation (7), if lim t z j = and lim t z j = for j m, then ()-(6) are satisfied Proof: Simple calculation can derive the results Variables z j and z j ( j m) are unified errors of formation and trajectory tracking between each robot and the desired trajectory By Lemma, the formation control problem can be solved by designing a control law for system (9) such that z j and z j converge to zero, respectively By the structure of (9), the controller can be designed in two steps In the first step, we design u j such that z j ( j m) converge to zero In the second step, we design u j such that z j ( j m) converge to zero Based on the first equation of (9), we have the following lemma Lemma : For system (9), control laws u j = k z j a ji (z j z i )+u, ( j m) () make lim t z j = constants k > and a ij > for j m Proof: The closed-loop z dynamic system can be written as ż = k z L G z () L G is the weighted Laplacian matrix [], [], z = [z,,z m ] Since the sum of each row of L G is zero, k I L G is a diagonal dominant matrix with negative elements in the diagonal So, the eigenvalues of k I L G are negative Therefore, () is asymptotically stable, ie, lim t z j = for j m Next, we design control law u j With the aid of the structure of (9), we have the following lemma Lemma : For system (9), under Assumption, if u j ( j m) are chosen as (), control laws u j = k n z nj a ji (z nj z ni ) α nj (z j k u ρ z j ) k l u ρ n α nj z lj (z l+,j z l,j z lj )+u d (u j u )β nj () ( j m) make lim t z kj =, ( k n, j m) constant k n > Proof: With control law (), we have ż nj = k n z nj a ji (z nj z ni ), j m () By the same method as for Lemma, we can prove that lim t z nj (t) = Next, we prove that lim t z ij (t) = for i n Let the positive definite function V = i= z ij Differentiating V along (9), we have V = i= k i u ρ z ij + i= (u j u )z ij β ij +u z,j z nj With the control laws () and (), (u j u ) and z nj converge to zero by Lemmas and Noting the expressions of β ij, V kmin u ρ V + ξ (t)v + ξ (t) V k min = min{k i, i n }, ξ (t) and ξ (t) are non-negative and converge to zero Let χ = V,wehave χ ( k min u ρ +ξ (t)/)χ+ξ (t)/ Noting Assumption, χ tends to zero which implies that V and z ij ( i ) tend to zero Therefore, lim t z ij =for i n By Lemmas -, we have the following theorem Theorem : For system (), under Assumption, control laws () and () make ()-(6) satisfied, the control parameters are chosen as in Lemmas - Proof: With control laws () and (), z j and z j converge to zero By Lemma, ()-(6) are satisfied In Theorem, the formation control problem is solved Because the z ij unified error variables combine trajectory and formation relative information, by driving the z ij variables to zero the control law simultaneously maintains the formation and drives the formation along the trajectory using self-state and local communication only between neighbors In control laws () and (), the control parameters are k i (> ) and a ij (> ) The convergence rate of the formation errors is dependent on k i ( i n), the a ij s, and the communication topology among the group of robots Generally, larger values of k i make the tracking and formation errors converge to zero faster Delays in the communication affect the stability of the closed-loop system However, that analysis is beyond the space limitations of this paper IV OUTPUT FEEDBACK FORMATION CONTROL In this section, we discuss the formation control using output feedback We assume the measured outputs are (q,j,q,j ) for each robot j and the information used in the control are only (q,j,q,j ) for j m To this end, we first design an observer Let ξ ij = q i+,j L ij q j, i n, j m () L ij are constant parameters to be designed later For each j, wehave ξ j =(ξ j + L j q j )u j L j (ξ j + L j q j )u j j m ξ n,j =(ξ n,j + L n,j q j )u j L n,j (ξ j + L j q j )u j ξ n,j = u j L n,j (ξ j + L j q j )u j () Motivated by the Luenberger observer design, we introduce the following observer for () ([7]) ξ j = ( ξ j + L j q j )u j L j ( ξ j + L j q j )u j j m ξ n,j = ( ξ n,j + L n,j q j )u j L n,j ( ξ j +L j q j )u j ξ n,j = u j L n,j ( ξ j + L j q j )u j (6) 6

4 Let ξ j =[ ξ j,, ξ n,j ]=[ξ j ξ j,,ξ n,j ξ n,j ] for j m, wehave A j = ξ j = u j A j ξj, j m (7) L j L j L n,j L n,j Lemma ([7]): If u j u ( j m) converges to zero and u ( ) satisfies Assumption, then ξ j ( j m) converge to zero, L ij are chosen such that A j is asymptotically stable By Lemma, (6) is an observer of () if u j satisfy assumptions in Lemma Noting (), we can easily obtain the estimate of q ij ( i n, j m) Introduce the following variables e j = q j q h jx + m l= h lx e j = q j q d h jy + m l= h ly e j = ξ j (q d L j (q d + h jy m l= h ly)) j m e nj = ξ n,j (q nd L n,j (q d + h jy m l= h ly)) (8) we have ė j = u j u ė j =(e j + L j e j )u + ξ j u j +(e j + q d + L j e j )(u j u ) ė j =(e j + L j e j L j (e j + L j e j ))u +(e j + L j e j L j (e j + L j e j ) + q d L j q d )(u j u ) j m ė,j =(e nj + L n,j e j L n,j (e j + L j e j ))u +(e nj + L n,j e j L n,j (e j + L j e j )+q nd L n,j q d )(u j u ) ė nj = u j L n,j ( ξ j + L j q j )u j u d + L n,j q d u (9) By the transformation { zj = e j,z j = e j () z ij = e ij α ij, i n, j m α j = k u ρ z j L j z j z i,j z i,j L i,j e j α ij = k i u ρ +L i,j (e j + L j e j )+ α i,j + i α i,j +L j e j )) + i α i,j l= u [l] e j (e j + L j e j ) (e l+,j + L l,j e j L l,j (e j u [l+] u, ( i n) () k i ( i n ) are constants and ρ n, wehave ż j = u j u ż j = k u ρ z j + u z j + u j ξj + β j (u j u ) ż j = k u ρ z j u z j + u z j α j u ξj + β j (u j u ) j m ż,j = k u ρ z,j u z n,j + u z nj α,j u ξj + β n,j (u j u ) ż nj = u j L n,j ( ξ j + L j q j )u j u d α nj + L n,j q d u u [l+] l= u [l] α nj ((e j + L j e j )u + e ξ j u j ) j α nj (e l+,j + L l,j e j L l,j (e j + L j e j ))u + β,j () β j = e j + q d + L j e j β i,j = e i+,j + L i,j e j L i,j (e j + L j e j ) + q i+,d L i,j q d α ij (e j + L j e j + q d ) e j i α ij (e l+,j + L l,j e j L l,j (e j + L j e j )+q l+,d L l,j q d )( i n ) β,j = α nj (e j + L j e j + q d ) e j α nj (e l+,j + L l,j e j L l,j (e j + L j e j )+q l+,d L l,j q d ) () Based on the proposed observer (6), using the ideas in the state feedback, we have the following results Theorem : For system (), under Assumption, control laws u j = k z j a ji (z j z i )+u () u j = k n z nj a ji (z nj z ni )+L n,j ( ξ j +L j q j )u j + u d L n,j q d u α n,j + u [l+] l= u [l] + α nj ((e j + L j e j )u e j + ξ j u j )+ α n,j (e l+,j + L l,j e j +L l,j (e j + L j e j ))u β,j, () ( j m) make ()-(6) satisfied, the control parameters are chosen as in Lemmas - 6

5 Control laws ()-() solve the output feedback formation control problem In the control laws, we only use the output states The convergence rate of the observer errors can be adjusted by the control parameters L ij The formation errors and the tracking errors are dependent on the control parameters [k,,k n ] and a ji, the observer parameter L ij, and the communication topology V SIMULATION This section presents a simulation validation of performance Assume there are five car-like mobile robots which are indexed by,,,, and The kinematic model of robot j is: { ẋj = R j v j cos θ j, ẏ j = R j v j sin θ j, θ j = R j v j tan φ j /l j, φ (6) j = v j (x j,y j ) represents the Cartesian coordinates of the middle point of the rear wheel axle, θ j is the orientation of the robot body with respect to the X-axis, φ j is the steering angle, l j is the distance between the front and rear wheelaxle centers, R j is the radius of rear driving wheel, v j is the angular velocity of the driving wheel, and v j is the steering velocity of the front wheels With the state transformation q j = x j,q j = y j,q j = tan θ j,q j = tan φ j l j cos (7) θ j and the input transformation u j = v j R j cos θ j u j = v jl j cos θ j + sin θ j sin φ j u j (8) lj cos θ j cos, φ j system (6) is transformed into q j = u j, q j = q j u j, q j = q j u j, q j = u j (9) which is a special case of () Obviously, states (q j,q j ) is the position of robot j in the X-Y plane States q j and q j are generalized angles related to the j-th robot It should be noted that the transformation is local, ie θ j and φ j Assume the desired formation is defined by (h x,h y )= (, ), (h x,h y ) = (, ), (h x,h y ) = (, ), (h x,h y )=(, ), (h x,h y )=(, ) (See Fig ) The desired trajectory of the center of the group of mobile robots is a line and is generated by a virtual robot with states: x d = t, y d = t, θ d =, φ d =Soq = x d = t, q d = y d = t, q d = tan θ d =,q d = Assume the digraph G is fixed and denoted as G The neighbors of each robot are as follows: N = {, }, N = {, }, N = {, }, N = {, }, N = {, } Using controllers ()-(), Fig shows path of each robot Fig shows logarithms of the norm of the formation error and the norm of the tracking error of the center of the group of mobile robots From the figures, it can be seen that ()-(6) are satisfied If the communication digraph G does not strongly connected, control laws ()-() can still make ()-(6) satisfied For example, if the communication graph is: N =, Fig Desired formation Fig Path of each robot (strong communication interconnection) time (sec) formation error tracking error Fig Logarithms of the norm of the formation error and the norm of the tracking error (strong communication interconnection) N = {, }, N = {, }, N = {, }, N = {, } Fig shows path of each robot Fig shows logarithms of the norm of the formation error and the norm of the tracking error of the center of the group of mobile robots The simulation results demonstrate that ()-(6) are still satisfied For output feedback formation control with communication graph G, we apply the control laws ()-() with the same control parameter values as before and L j =, L j = Fig 6 shows path of each robot Fig 7 shows logarithms of the norm of the formation error and the norm of the tracking error The results in Fig 6 and Fig 7 show 66

6 that the output control law ()-() also make the group of robots form the desired formation 6 VI CONCLUSION We consider the formation control of nonholonomic mobile robots State feedback and output feedback controllers are proposed, which render the robot team to a given formation moving along a desired trajectory Simulation results demonstrate satisfactory performances REFERENCES [] R C Arkin, Cooperation without coomunication: multiagent schemabased robot navigation, J of Robotic Systems, vol9, pp - 6, 99 [] T Balch and R C Arkin, Behavior-based formation control for multirobot teams, IEEE Trans on Robotics and Automation, vol, no 6, pp 96-99, 998 [] R W Beard, J Lawton, and F Y Hadaegh, A Coordination Architecture for Spacecraft Formation Control, IEEE Trans on Control Systems Technology, vol 9, no 6, pp , [] J P Desai, J Ostrowski, and V Kumar, Controlling formations of multiple mobile robots, Proc of the IEEE Int Conf on Robotics and Automation, (Leuven, Belgium), pp , 998 [] J A Fax, and R M Murray, Information Flow and Cooperative Control of Vehicle Formations, IEEE Trans on Automatic Control, vol 9, pp 6-76, [6] A Jadbabaie, J Lin, and A S Morse, Coordination of Groups of Mobile Autonomous Agents Using Nearest Neighbor Rules, IEEE Trans on Automatic Control, vol 8, pp 988-, [7] Z-P Jiang, Lyapunov Design of Global State and Output Feedback Trackers for Non-holonomic Control systems, Int J of Control, vol 7, no 9, pp 7-76, [8] G Lafferriere, A Williams, J Caughman, and J J P Veermany, Decentralized Control of Vehicle Formations, systems and Control Letters, vol, no 9, pp 899-9, [9] M A Lewis and K-H Tan, High precision formation control of mobile robots using virtual structures, Autonomous Robots, vol, pp 87-, 997 [] Z Lin, B Francis, and M Maggiore, Necessary and sufficient graphical conditions for formation control of unicycles, IEEE Trans on Automatic Control, vol, no, pp -7, [] M Mesbahi and F Y Hadaegh, Formation flying control of multiple spacecraft via graphs, matrix inequalities, and switching, AIAA J of Guidance, Control, and Dynamics, vol, pp 69-77, [] R M Murray and S S Sastry, Nonholonomic motion planning: Steering using sinusoids, IEEE Trans on Automatic Control, vol 8, no, pp 7-76, 99 [] R Olfati-Saber and R M Murray, Distributed structural stabilization and tracking for formations of dynamic multiagents, Proc IEEE Conf Decision and Control, Las Vegas, NV, Dec, pp 9- [] R Olfati-Saber and R M Murray, Consensus Problems in Networks of Agents with Switching Topology and Time-Delays, IEEE Trans on Automatic Control, vol 9, pp -, [] W Ren and R W Beard, Consensus of information under dynamically changing interaction topologies, Proc of American Control Conf, pp 99-9, [6] W Ren and R W Beard, Formation Feedback Control for Multiple Spacecraft Via Virtual Structures, submitted to IEE Proceedings - Control Theory and Applications, [7] W Ren and R W Beard, Consensus Seeking in Multi-agent Systems Under Dynamically Changing Interaction Topologies, IEEE Trans on Automatic Control, [8] H G Tanner, G J Pappas, and V Kumar, Leader-to-Formation Stability, IEEE Trans on Robotics and Automation, vol, pp -, [9] P K C Wang and F Y Hadaegh, Coordination and control of multiple microspacecraft moving in formation, The J of the Astronautical Sciences, vol, no, pp -, 996 [] H Yamaguchi, A distributed motion coordination strategy for multiple nonholonomic mobile robots in cooperative hunting operations, Robotics and Autonomous Systems, vol, pp 7-8, 67 Fig Path of each robot (weak communication interconnection) 7 6 time (sec) formation error tracking error Fig Logarithms of the norm of the formation error and the norm of the tracking error (weak communication interconnection) Fig 6 Path of each robot (output feedback) time (sec) formation error tracking error Fig 7 Logarithms of the norm of the formation error and the norm of the tracking error (output feedback)