Formation Tracking Control of Unicycle-Type Mobile Robots With ...
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
527
Formation Tracking Control of Unicycle-Type Mobile Robots With Limited Sensing Ranges K. D. Do Abstract—A constructive method is presented to design cooperative controllers that force a group of unicycle-type mobile robots with limited sensing ranges to perform desired formation tracking and guarantee no collisions between the robots. Physical dimensions and dynamics of the robots are also considered in the control design. Smooth and times differential bump functions are introduced and incorporated into novel potential functions to design a formation tracking control system. Despite the robot limited sensing ranges, no switchings are needed to solve the collision avoidance problem. Simulations illustrate the results. Index Terms—Bump function, formation tracking, mobile robot, potential function.
I. INTRODUCTION
F
ORMATION control of multiple agents has received a lot of attention from the control community over the last few years due to its potential applications to search, rescue, coverage, surveillance, reconnaissance and cooperative transportation. Formation control can be roughly understood as controlling positions of a group of the agents such that they stabilize/ track desired locations relative to reference point(s), which can be another agent(s) within the team, and can either be stationary or moving. Research works on formation control often use one or more of leader-following (e.g., [1], [2]), behavioral (e.g., [3], [4]), and use of virtual structures (e.g., [5], [6]) approaches in either a centralized or decentralized manner. Centralized control schemes (e.g., [2] and [7]) use a single controller that generates collision-free trajectories in the workspace. Although these guarantee a complete solution, centralized schemes require high computational power and are not robust due to the heavy dependence on a single controller. A nice application of formation control based on the potential field method [2] and Lyapunov’s direct method [8] to gradient climbing was recently addressed in [9]. However, the final configuration of formation cannot be foretold. On the other hand, decentralized schemes, see, e.g., [10] and [11], require less computational effort and are relatively more scalable to the team size. The decentralized approach usually involves a combination of agent-based local potential fields (e.g., [2], [12], and [13]). The main problem with the decentralized approach, when collision avoidance is taken into account, is that it is extremely difficult to predict and control the critical points of the controlled systems. An interesting work addressing
Manuscript received December 27, 2006; revised April 3, 2007. Manuscript received in final form June 17, 2007. Recommended by Associate Editor K. Kozlowski. The author is with the School of Mechanical Engineering, The University of Western Australia, Crawley, WA 6009, Australia (e-mail: [email protected]. au). Color versions of one or more of the figures in this brief are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TCST.2007.908214
geometric formation based on Voronoi partition optimization is given in [14], but the final arrangement of the agents cannot be foretold due to locality of Lloy’s algorithm. Recently, a method based on a different navigation function from [15] provided a centralized formation stabilization control design strategy proposed in [7]. This work is extended to a decentralized version in [11]. A similar result but based on different navigation function motivated by the work in [15] is given in [16]. However, the formation is stabilized to any point in workspace instead of being “tied” to a fixed coordinate frame. Moreover, the potential function, which possesses all properties of a navigation function (see [15]), is finite when a collision occurs. This complicates the analysis of collision avoidance in the sense that one cannot directly use the first time derivative of the potential function to prove no collisions between agents. In [7], [15], [16], and [11], the tuning constants, which are crucial to guarantee that the only desired equilibrium points are asymptotically stable and that the other critical points are unstable, are extremely difficult to obtain explicitly. In most of the above papers, point-robots with simple (single or double integrator) dynamics (e.g., [2], [7], [11], [13], [14], and [17]) or fully actuated vehicles [6] (which can be converted to double-integrator dynamics via a feedback linearization) were investigated. It should be mentioned that decentralized navigation of nonpoint agents with single-integrator dynamics was also investigated in [18], but each agent requires global knowledge of position of other agents. Vehicles with nonholonomic constraints were also considered (e.g., [3]). However, the nonholonomic kinematics are transformed to double-integrator dynamics by controlling the hand position instead of the inertial position of the vehicles. Consequently, the vehicle heading is not controlled. In addition, switching control theory [19] is often used to design a decentralized formation control system (e.g., [1], where a case-by-case basis is proposed), especially when the vehicles have limited sensing ranges and collision avoidance between vehicles must be considered. Clearly, it is more desirable if we are able to design a nonswitching formation control system that can handle the above decentralized and collision avoidance requirements. Moreover, in the tracking control of single nonholonomic mobile robots (e.g., [20]–[22]), where it seems that the backstepping technique was first used in [20] to take the robot kinetic into account in the control design, the tracking errors are often interpreted in a frame attached to the reference trajectory using the coordinate transformation in [23] to overcome difficulties due to nonholonomic constraints. If these techniques are migrated to formation control of a group of mobile robots, it is extremely difficult to incorporate collision avoidance between the robots. The above problems motivate the contribution of this brief. In this brief, cooperative controllers are designed to force a unicycle-type mobile robots with limited sensing group of ranges to perform desired formation tracking and to guarantee
1063-6536/$25.00 © 2008 IEEE Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
528
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
with
(3) Fig. 1. Robot parameters.
no collisions between the robots. The physical dimensions and dynamics of the robots are also considered. The control development is based on novel potential functions, which attain the minimum value when the desired formation is achieved and are equal to infinity when a collision occurs. Moreover, smooth and times differential bump functions are introduced and incorporated into the potential functions to avoid the use of switchings, which are often required in literature to deal with the robot’s limited sensing ranges. II. PROBLEM STATEMENT
and are the masses of the body and wheel with where , and are the moments of inertia of the body a motor, , (center of mass), the wheel about the vertical axis through with the rotor of a motor about the wheel axis, and the wheel with the rotor of a motor about the wheel diameter, respectively, , , and are defined in Fig. 1, and the nonnegative constants and are the damping coefficients. For convenience, we convert the wheel velocities of the robot to its linear and angular velocities by (4) where
A. Robot Dynamics
, and is invertible since . With (4), we can write the robot dynamics (1) as
follows:
We consider a group of mobile robots, of which each has the following dynamics [21]:
(5) (1) where , denotes the position , the coordinates of the middle point, , between the left and right driving wheels, and heading of the robot coordinated in the earth-fixed frame (see Fig. 1), , where and are the angular velocities of the wheels of the robot , , where and are the control torques applied to the wheels of the robot . The rotation matrix , mass matrix , Coriolis matrix , and damping matrix in (1) are given by where
with
(2) Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(6)
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
529
, where a strictly positive constant. Moreover, bounded. The constant vectors ,
, and is and are also , satisfy (10)
Fig. 2. Formation setup.
B. Formation Control Objective In this brief, we will study a formation control problem under the following assumption. Assumption 1: 1) The robot has a physical safety circular area, which is , has a radius , and has a circular centered at the point communication area, which is centered at the point and has a radius (see Fig. 2). The radius is strictly larger than , , . 2) The robot broadcasts its state and its reference trajectory in its communication area. Moreover, the robot can receive the states and reference trajectories broadcast by other robots , , , in the group if the points of these robots are in the communication area of the robot . 3) The dimensional terms of the robot are known to the robot . The terms involved with mass, inertia, and damping, , of the robots are unknown but constant. 4) At the initial time , each robot starts at a location that is outside of the safety areas of other robots in the group, i.e., there exists a strictly positive constant such that (7) , , where for all 5) The reference trajectory for the robot is which is generated by
for all , , where is a strictly positive constant. Remark 1: Items 1) and 2) in Assumption 1 specify the way each robot communicates with other robots in the group within its communication range. In Fig. 2, the robots and are communicating with each other since the points and are in the communication areas of the robots and , respecare not communicating with each tively. The robots and other since the points and are not in the communicaand , respectively. Similarly, the tion areas of the robots and are not communicating with each other. robots Item 3) makes sense in practice because the dimensional terms can be easily predetermined while the terms involving mass, inertia, and damping are often difficult to predetermine. Moreover, is known, the assumptions in this item mean that the matrix while the matrices and and coefficients of the entries of the matrix are unknown but constant. In item 5), the , specifies the desired formaconstant vectors , tion configuration with respect to the earth-fixed frame . The condition (9) implies that the common reference is regular and its velocity , which specifies how the desired formation, whose configuration is determined by , moves along , is bounded and satisfies a persistent excitation condition, i.e., the desired formation always moves along the common reference trajectory . The condition (10) specifies feasibility of the reference trajectories , (recall from (8) that , ) due to physical safety circular areas of the robots. Finally, all of the robots in the group require knowledge of the common reference trajectory since this trajectory specifies how the whole formation should move with respect to the earth-fixed frame . Formation Control Objective: Under Assumption 1, design the control input and update laws for all terms involving mass, inertia, and damping ( , , , , and ) for each robot such that each robot asymptotically tracks its desired reference trajectory while avoiding collisions with all other robots in the group, i.e., for all , , :
. , (8)
where is referred to as the common reference trajectory with being the common trajectory parameter, and is a constant vector. The trajectory satisfies the following conditions:
(11) where
, and
is a positive constant.
III. PRELIMINARIES We here present one definition and one lemma, which will be used in the control design in the next section. is called a times Definition 1: A scalar function differential bump function if it enjoys the following properties if if
(9)
is
if times differentiable with respect to
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(12)
530
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
A. Stage I Since the robot is underactuated, we divide this stage into two steps using the backstepping technique. In the first step, and the robot linear velocity are used the robot heading as immediate controls to fulfill the task of position tracking and collision avoidance. In the second step, the robot angular veis used as an immediate control to stabilize the error locity between the actual robot heading and its immediate value at the origin. We do not use the transformation in [23] to interpret the tracking errors in a frame attached to the reference trajectories as is often done in literature (e.g., [20]–[22]) to avoid difficulties when dealing with collision avoidance. 1) Step I.1: Define
Fig. 3. Twice-differentiable bump function.
where is a positive integer, , and and are constants such that . Moreover, if the function is infinite times differentiable with respect to , then it is called a smooth bump function. It is noted that bump functions are not new to the control combump function was used in [24] for munity. A nontrivial obstacle avoidance in flocking cooperation control of multiple agents with limited communication. Here, we give a constructive method to construct the times differentiable and smooth bump functions. These functions are useful when dealing with cooperative control of multiple agents with limited communication and high-order dynamics. Construction of these bump functions is given in the following lemma. Lemma 1: Let the scalar function be defined as
(15) and are virtual controls of and , respectively. where With (15), the first two equations of (5) are read as (16) where
and
(13) where the function if
is defined as follows: and
if
(14)
where is a positive integer. Then, the function is a times differentiable bump function. Moreover, if in , then property 4) is replaced (14) is replaced by is a smooth bump function. by: 4 ) Proof: See Appendix A. Fig. 3 illustrates a twice-differen, ). tiable bump function ( Remark 2: As specified in Lemma 1, the times differentiable bump functions can be obtained explicitly, i.e., the integral (13) can be solved analytically. The smooth bump functions cannot be obtained explicitly. Thus, a numerical procedure is needed to solve the integral (13). However, in many applications, including the formation control of mobile robots in this brief, the times differentiable bump functions are sufficient. IV. CONTROL DESIGN Since the robot dynamics (5) are of a strict feedback form [25] with respect to the robot linear velocity and angular velocity , we will use the backstepping technique [25] to design the control input . The control design is divided into two main stages. In the first stage, we consider the first three equations of being considered as immediate controls. In (5) with and the second stage, the actual control will be designed.
(17) To fulfill the task of position tracking and collision avoidance, we consider the following potential function: (18) and are the goal and related collision avoidance where functions for the robot specified as follows. • The goal function is designed such that it puts penalty on the tracking error for the robot and is equal to zero when the robot is at its desired position. A simple choice of this function is (19) • The related collision function should be chosen such that it is equal to infinity whenever any robots come in contact with the robot , i.e., a collision occurs, and attains the minimum value when the robot is at its desired location with respect to other group members belonging to the set robots, where is the set that contains all of the of robots in the group except for the robot . This function is chosen as follows:
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(20)
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
where the function is a function of and enjoys the following properties: and if 1) 2) if 3) and if 4) 5) 6)
with
if and . is at least three times differentiable with respect to if
(21)
, is a strictly positive constant where , , such that , and are positive constants, and , , and are defined as follows: , , if , , , and if . Remark 3: Properties 1)–4) imply that the function is positive definite, is equal to zero when all of the robots are at their desired locations, and is equal to infinity when a collision between any robots in the group occurs. Moreover, Property 1) and attains the function given in (19) ensure that the function the (unique) minimum value of zero when all of the robots are at their desired positions. Also, Property 3) ensures that the collision avoidance between the robots and is only taken into account when they are in their communication areas. Property 5) is used to prove stability of the closed-loop system. Property 6) ensures that we can use techniques such as the backstepping and direct Lyapunov design methods [25], [26] for control design and stability analysis for continuous systems instead of techniques for switched, nonsmooth and discontinuous systems [19], [27] to handle the collision avoidance problem under the robot’s limited sensing ranges. There are many functions that satisfy all properties of given in (21). An example is (22) where is a times differentiable and bump function defined in Definition 1 with , and . The time along the solutions of (16) satisfies derivative of (23) where we have used ,
, , and
,
(24) From (23), we choose a bounded control
531
denotes a vector of bounded functions of elements where with and of in the sense that the first and second rows of , i.e., . The function is a scalar, at least three-times differentiable and bounded function with respect to and satisfies ; 1) if if ; 2) 3) ; 4) for all , , where , , are strictly positive constants. Some functions that satisfy the above properties and . The strictly positive constant is are chosen such that (26) The above condition ensures that and are solvable from . We now need to solve for and from the expression of in (25) and (17). From (25) and (17), we have
(27) where
we
have
used and
since and (see Assumption 1). The left-hand sides of (27) are actually the in the and directions. Now, multiplying coordinates of and both both sides of the first equation of (27) with and then sides of the second equation of (27) with adding the resulting equations together yields
(28) On the other hand, multiplying both sides of the first equation and both sides of the second equation of of (27) with and then subtracting the resulting equations (27) with gives
(29) From (28) and (29), we solve for and as shown in (30), shown at the bottom of the next page. It is noted that (30) is well defined since , where the condition (26) has been used. Moreover, it is of interest to note and are at least twice-differentiable functions of that , , , , and with , . defined in (24) is substituted into (25), Remark 4: When the control can be written as
designed as follows: (25)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(31)
532
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
It is seen from (31) that the argument of consists of two or , referred to parts. The first part, as the attractive force, plays the role of forcing the robot to its desired location. The second part, or , referred to as the repulsive force, takes care of collision avoidance for the robot with the other robots. Moreover, the immediate control of the robot given in (25) depends on only its own state and reference trajectory and the of these robots states of other neighbor robots if the points are in the circular communication area of the robot , since out[see Property 3) of ]. side this area Now, substituting (25) into (23) results in
whose derivative along the solutions of (32) and (35) satisfies
(32) Substituting (25) into (16) results in
(37) It is noted that
(33) as an immediate control 2) Step I.2: In this step, we view at the origin. As such, we define to stabilize
and smooth functions for all as virtual control
is well defined since are . From (37), we choose the
(34) is a virtual control of . To prepare for designing where , let us calculate . Differentiating both sides of the first equation of (15) along the solutions of (34), the third equation of (5) and the second equation of (30) results in
(35) To design the virtual control
, we consider the function (36)
(38)
where is a positive constant. It is of interest to note that is an at least once-differentiable function of , , , , , , , , , and , with , . Moreover, it contains only the should be noted that the virtual control state and reference trajectory of the robot , and the states of other neighbor robots if the points of these robots are in the communication area of the robot , because outside this area thanks to Property 3) of . Substituting (38)
(30)
(39)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
into (37) results in (39), shown at the bottom of the previous page, after some manipulation. Substituting (38) into (35) gives
533
B. Stage II In this stage, we design the actual control input vector and for each update laws for unknown system parameter vector robot . To do so, we consider the following function:
(44) (40)
To prepare for the next section, let us compute the term , where . From the second equation of (15), (34), and the last equation of (5), we have
where and is an estimate of , and is a symmetric positive definite matrix. Differentiating both sides of (44) along the solutions of (41) and (39) yields
(41) where
(45)
which suggests that we choose (46), shown at the bottom of the , and is a symmetric page, where positive definite matrix. Substituting the first equation of (46) into (41) gives (42)
(43)
where , . Again, and contain only the state and reference trajectory of the robot , and the states of other neighbor robots if the points of these robots are in the communication area of the robot , , , and because outside this area , thanks to Property 3) of .
(47) By construction, the control and the update given in (46) of the robot contain only the state and reference trajectory of the robot , and the states of other neighbor robots if these robots of the robot are in a circular area, which is centered at point
(46)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
534
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
Fig. 4. Simulation results with ten robots.
and has a radius no greater than (45) results in
. Now, substituting (46) into
(48) For convenience, we rewrite the closed-loop system consisting of (33), (40), (47), and the second equation of (46) as follows:
(49) We now state the main result of our brief in the following theorem. Theorem 1: Under Assumption 1, the control and the update law given in (46) for the robot solve the formation control objective. In particular, no collisions between any robots
, the closed-loop system (49) is forcan occur for all ward complete, and the position and orientation of the robots track their reference trajectories asymptotically in the sense of (11). Proof: See Appendix B. V. SIMULATIONS Here, we simulate formation tracking control of a group of identical mobile robots to illustrate the effectiveness of the proposed controller. The physical parameters of the robot , , , are taken from [21]: , , , , , , , , and , . The robots are initialized as follows: , , , , where for , and for . The initial values of , , are taken as random numbers between 0 and . The initial values of are taken as half of their true values. , The reference trajectories are taken as and . This choice of the reference trajectories means that the common refforms a sinusoidal trajectory and that the erence trajectory desired formation configuration is a polygon whose vertices uniformly distribute on a circle centered on the common reference , , trajectory and with a radius 10. The functions , are taken as in (22) with , , and . is taken as . The control gains and upThe function , , , and date gains are chosen as , where is a -dimensional identity matrix. Indeed, the above choice of satisfies condition (26). Simulation results are plotted in Fig. 4. It is seen that all robots nicely track their reference trajectories. During the first four seconds, the robots quickly move away from each other to avoid collisions then
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
track their desired reference trajectory in Fig. 4(a), where the trajectory of the robot is plotted in the thick line. For clarity, of the robot are plotted in only the control inputs Fig. 4(b). Fig. 4(c) plots the tracking errors , , of the robot . Indeed, these errors tend to zero and and other asymptotically. The distances between the robot robots are plotted in Fig. 4(d). Clearly, these distances are al, , i.e., there ways larger than are no collisions between the robot and all other robots in the group. Moreover, in Fig. 4(e), we plot the product of all gaps . It between robots: is seen that is always larger than zero. This means that , , , i.e., no collisions between any agents occurred. For clarity, we only plot the results for the first 20 s in Fig. 4(b)–(e). VI. CONCLUSION We have presented a method to design cooperative formaunicycle-type motion tracking controllers for a group of bile robots with limited sensing ranges. The control design was constructed in such a way that the robots asymptotically track their reference trajectories and avoid collisions among them. The novel potential functions with embedded smooth or/and times differential bump functions are attractive parts of the brief since they do not require the use of switching control theory despite the robot limited sensing ranges. These functions can certainly be modified to solve other cooperative control problems such as flocking and consensus of mobile agents. In addition, and is the method of constructing the virtual controls also attractive, since they are derived directly from the position tracking errors instead of transforming the robot kinematics to a body frame as often done in the literature. Future work is an extension of the proposed techniques in this brief and those for single underactuated ocean vessels in [28] and [29] to achieve a desired formation for a group of underactuated vessels.
535
any positive integer. We will prove Property 4 ) by induc. It is clear that tion. First of all, we check that . On the other hand, where and we have used l’Hopital’s rule. Since both left- and right-hand . Now assume that limits are equal to 0, we have . We now compute . The left-hand limit is equal to 0 as above. The right hand limit is
(by l’Hopital’s rule), where and is another polynomial of . Since both left- and right-hand limits are equal to 0, we have , which completes the proof of Property 4 ). APPENDIX B PROOF OF THEOREM 1 We first prove that no collisions between the robots can occur, the closed-loop system (49) is forward complete and that the robots asymptotically approach their target points or some critical points. We then investigate stability of the closed-loop system (49) at these points and show that the position and orientation of the robots asymptotically track their reference trajectories. Proof of No Collisions and Complete Forwardness of Closed Loop System: From (48) and properties of the function [see , which implies that , (26)], we have . With definition of the function in (44) and its components in (36), (18), (19), and (20), we have
APPENDIX A PROOF OF LEMMA 1 given in We need to verify that the function (13) satisfies all properties defined in (12). Property 1) , we have holds because, by (14), for all . Property 2) holds since, by (14), for we have . Property 3) holds because it is not hard to show that all
(50)
for all . To prove Property 4), we just need to show that is times differentiable. We first note is smooth except at . Hence, we only need that for any to verify that . Clearly, since positive integer , . On the other hand, since , we have . Since both left- and right-hand limits are equal to 0, we have . Hence, Property 4) holds. Now we turn to the case where in (14) is replaced by . Properties 1)–3) can be proven without difficulty. We focus on proof of Property 4 ). , We first note that is a polynomial function of , and is where
for all . From the condition specified in item 4) , and the definition of , of Assumption 1, Property 5) of , the right-hand side of (50) is bounded by a positive constant depending on the initial conditions. The boundedness of the right-hand side of (50) implies that the left-hand side of (50) must be smaller than must be also bounded. As a result, some positive constant depending on the initial conditions for . From properties of [see (21)], all must be larger than some positive constant depending on the initial conditions denoted by , i.e., there are no colli. Boundedness of the left-hand side sions for all , , , and of (50) also implies that of
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
536
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
for all . This in turn implies by construc, , , , and do not escape to tion that infinity in finite time. Therefore, the closed-loop system (49) is forward-complete. Equilibrium Points: Since we have already proved that there are no collisions between any robots, an application of [26, Theorem 8.4] to (48) yields
Properties of Equilibrium Points: The system (54) can be written in a vector form as (55) where
point
, , and . Therefore, near an equilibrium , which can be either or , we have (56)
(51) as specified in item 5) of By noting that Assumption 1, the limit equation (51) implies that
By construction, imply that definition of in (24),
and . Moreover, from the means (52)
The
limit
equation
element of the matrix , , where set of all agents. A simple calculation shows that
where the
(52) implies that can tend to
since (Property 1) of ), or some vector denoted by as the time goes to infinity, i.e., the equilibrium points can be or . It is noted that some elements of can be equal to that of . However, for simplicity, we abuse the notation, i.e., we still denote that vector as . Indeed, the vector is such that
is is the
(57) for all , , , where denotes the identity matrix of size . Let be the set of the agents such that, if the agents and belong to the set , then . Next, we will show that is asymptotically stable and that is unstable. Proof of Being Asymptotic Stable: Using properties of and listed in (21) and (26), we have from (57) that for all ,
(58) (53) To investigate properties of the equilibrium points, and , we consider the first equation of the closed-loop system (49), i.e., (54) Since we have already proved that the closed-loop system and (49) is forward complete, and imply from the expressions of and [see (17)] that , we treat as an input to (54) instead of a state. Moreover, we have already proved that the trajectory can approach either the set of desired equilibrium points denoted by or the set of undesired equilibrium points denoted by “almost globally.” The term “almost globally” refers to the fact that the agents start from a set that includes both condition (7) and that does not coincide at any point with the set of the undesired saddle point . Therefore, we now need to prove that is locally asymptotically stable and that is locally unstable. Once this is proved, we can conclude that the trajectory approaches from almost everywhere except for from the set denoted by the condition (7) and the set denoted by , which is unstable.
where
and . We consider the function
, with (59)
whose derivative along the solutions of (56) with replaced by , using (58), and noting that satisfies
(60) [see Property 1) in (21)]. The last inequality since of (60) implies that is asymptotically stable because
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
, and we have already proved that . Proof of Being Unstable: Again, using properties of and in (21) and (26), we have from (57) that
537
and repulsive forces are nonzero). Therefore, the point, say , , , , must locate between the where , , i.e., there exists a points and for all strictly positive constant such that , which is substituted into (64) to yield (65)
(61)
, , , there exists a nonempty Since such that, for all , , set is strictly negative, i.e., there exists a strictly positive constant such that , , . We now write (63) as
for all
, , where , , and . Since the related collision avoidance functions [see (20)] are specified in terms of relative distances between agents and it is extremely difficult to explicitly by solving (53), it is very difficult to use obtain the Lyapunov function candidate to investigate stability of (56) at . Therefore, we consider the Lyapunov function candidate (62) and . Differentiating both sides of (62) along the solution of (56) with replaced by gives where
(66) where
. We now define a subspace such that and , In this subspace, we have
,
, .
(63) and (61) has been used. To investigate stability where properties of based on (63), we will use (53). Define , , where [see (53)]. Therefore, . Hence, , , which, by using (53), is expanded to
(67)
Using (67) as
,
,
, we can write
(68)
(64) where . The sum is strictly negative , , , since, at the point, say , where all attractive and repulsive forces are equal to zero, while at the , , the sum of point, say , where attractive and repulsive forces are equal to zero (but attractive
Now assume that
is a stable equilibrium point, i.e., , , where is a , we have nonnegative constant. Noting that , , which im, plies that
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
538
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
, stant, since consider two cases:
, where and
is a nonnegative con. We now and
. : Since (Assumption 1) and we have already , , in (68) is shown that divergent. Therefore, cannot tend to a constant but must be divergent. This contradicts , i.e., cannot be a set of stable equilibrium points but must be a set of an unstable ones in this case. : There would Case I: be no contradiction. However, this case is never observed in practice since the ever-present physical noise would cause to be different from zero at the . We now need to show that, once the sum time is different from zero, this sum will , i.e., the set of undesired not come back zero again for all equilibrium points is not attractive. To do so, consider (68) with the initial time instead of , i.e., Case I:
(69)
and , where is for a positive constant. Since (Assumption 1) , , and we have already shown that in (69) is divergent for . Therefore, , for , cannot tend to a constant but must be divergent. , i.e., This contradicts must also be a set of unstable ones point in this case. The proof of Theorem 1 is completed. ACKNOWLEDGMENT The author would like to thank the reviewers for their helpful comments which helped him improve the brief significantly. REFERENCES [1] A. K. Das, R. Fierro, V. Kumar, J. P. Ostrowski, J. Spletzer, and C. J. Taylor, “A vision based formation control framework,” IEEE Trans. Robot. Autom., vol. 18, no. 5, pp. 813–825, Oct. 2002. [2] N. E. Leonard and E. Fiorelli, “Virtual leaders, artificial potentials and coordinated control of groups,” in Proc. 40th IEEE Conf. Decision Control, Orlando, FL, 2001, pp. 2968–2973. [3] R. T. Jonathan, R. W. Beard, and B. J. Young, “A decentralized approach to formation maneuvers,” IEEE Trans. Robot. Autom., vol. 19, no. 6, pp. 933–941, Dec. 2003. [4] T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE Trans. Robot. Autom., vol. 14, no. 6, pp. 926–939, Dec. 1998. [5] M. A. Lewis and K.-H. Tan, “High precision formation control of mobile robots using virtual structures,” Auton. Robots, vol. 4, no. 4, pp. 387–403, 1997.
[6] R. Skjetne, S. Moi, and T. I. Fossen, “Nonlinear formation control of marine craft,” in Proc. 41st IEEE Conf. Decision Control, Las Vegas, NV, 2002, pp. 1699–1704. [7] H. G. Tanner and A. Kumar, “Towards decentralization of multi-robot navigation functions,” in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain, 2005, pp. 4132–4137. [8] P. Ogren, M. Egerstedt, and X. Hu, “A control Lyapunov function approach to multi-agent coordination,” IEEE Trans. Robot. Autom., vol. 18, no. 5, pp. 847–851, Oct. 2002. [9] P. Ogren, E. Fiorelli, and N. E. Leonard, “Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment,” IEEE Trans. Autom. Control, vol. 49, no. 8, pp. 1292–1302, 2004. [10] D. M. Stipanovic, G. Inalhan, R. Teo, and C. J. Tomlin, “Decentralized overlapping control of a formation of unmanned aerial vehicles,” Automatica, vol. 40, no. 8, pp. 1285–1296, 2004. [11] H. G. Tanner and A. Kumar, “Formation stabilization of multiple agents using decentralized navigation functions,” Robot.: Sci. Syst. I, pp. 49–56, 2005. [12] H. G. Tanner, A. Jadbabaie, and G. J. Pappas, “Stable flocking of mobile agents. Part II: Dynamics topology,” in Proc. 42nd IEEE Conf. Decision Control, Maui, HI, 2003, vol. 2, pp. 2016–2021. [13] S. S. Ge and Y. J. Cui, “New potential functions for mobile robot path planning,” IEEE Trans. Robot. Autom., vol. 16, no. 5, pp. 615–620, Oct. 2000. [14] J. Cortes, S. Martinez, and T. K. F. Bullo, “Coverage control for mobile sensing networks,” IEEE Trans. Robot. Autom., vol. 20, no. 2, pp. 243–255, Apr. 2004. [15] E. Rimon and D. E. Koditschek, “Robot navigation functions on manifolds with boundary,” Adv. Appl. Math., vol. 11, no. 4, pp. 412–442, 1990. [16] D. V. Dimarogonas and K. J. Kyriakopoulos, “Formation control and collision avoidance for multi-agent systems and a connection between formation infeasibility and flocking behavior,” in Proc. 44th Conf. Decision Control/Eur. Control Conf., Seville, Spain, 2005, pp. 84–89. [17] R. Olfati-Saber and R. M. Murray, “Distributed cooperative control of multiple vehicle formations using structural potential functions,” presented at the 15th IFAC World Congr., Barcelona, Spain, 2002. [18] D. V. Dimarogonas, S. G. Loizou, K. J. Kyriakopoulos, and M. M. Zavlanos, “A feedback stabilization and collision avoidance scheme for multiple independent non-point agents,” Automatica, vol. 42, no. 2, pp. 229–243, 2006. [19] D. Liberzon, Switching in Systems and Control. Cambridge, MA: Birkauser, 2003. [20] R. Fierro and F. L. Lewis, “Control of a nonholonomic mobile robot: Backstepping kinematics into dynamics,” in Proc. 34th IEEE Conf. Decision Control, New Orleans, LA, 1995, vol. 4, pp. 3805–3810. [21] T. Fukao, H. Nakagawa, and N. Adachi, “Adaptive tracking control of a nonholonomic mobile robot,” IEEE Trans. Robot. Autom., vol. 16, no. 5, pp. 609–615, Oct. 2000. [22] K. D. Do, Z. P. Jiang, and J. Pan, “A global output-feedback controller for simultaneous tracking and stabilization of unicycle-type mobile robots,” IEEE Trans. Robot. Autom., vol. 20, no. 3, pp. 589–584, Jun. 2004. [23] C. Samson, “Velocity and torque feedback control of a nonholonomic cart,” in Advanced Robot Control, ser. Lecture Notes in Control and Information Sciences, C. Canudas de Wit, Ed. Berlin, Germany: Heidelberg, 1991, pp. 125–151. [24] R. O. Saber and R. M. Murray, “Flocking with obstacle avoidance: Cooperation with limited communication in mobile networks,” in Proc. 42nd IEEE Conf. Decision Control, Maui, HI, 2003, vol. 2, pp. 2022–2028. [25] M. Krstic, I. Kanellakopoulos, and P. V. Kokotovic, Nonlinear and Adaptive Control Design. New York: Wiley, 1995. [26] H. Khalil, Nonlinear Systems. Upper Saddle River, NJ: Prentice-Hall, 2002. [27] H. G. Tanner and K. J. Kyriakopoulos, “Backstepping for nonsmooth systems,” Automatica, vol. 39, no. 7, pp. 1259–1265, 2003. [28] K. D. Do, Z. P. Jiang, and J. Pan, “Universal controllers for stabilization and tracking of underactuated ships,” Syst. Control Lett., vol. 47, no. 4, pp. 299–317, 2002. [29] K. D. Do and J. Pan, “Underactuated ships follow smooth paths with integral actions and without velocity measurements for feedback: Theory and experiments,” IEEE Trans. Control Syst. Technol., vol. 14, no. 2, pp. 308–322, Mar. 2006.
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
527
Formation Tracking Control of Unicycle-Type Mobile Robots With Limited Sensing Ranges K. D. Do Abstract—A constructive method is presented to design cooperative controllers that force a group of unicycle-type mobile robots with limited sensing ranges to perform desired formation tracking and guarantee no collisions between the robots. Physical dimensions and dynamics of the robots are also considered in the control design. Smooth and times differential bump functions are introduced and incorporated into novel potential functions to design a formation tracking control system. Despite the robot limited sensing ranges, no switchings are needed to solve the collision avoidance problem. Simulations illustrate the results. Index Terms—Bump function, formation tracking, mobile robot, potential function.
I. INTRODUCTION
F
ORMATION control of multiple agents has received a lot of attention from the control community over the last few years due to its potential applications to search, rescue, coverage, surveillance, reconnaissance and cooperative transportation. Formation control can be roughly understood as controlling positions of a group of the agents such that they stabilize/ track desired locations relative to reference point(s), which can be another agent(s) within the team, and can either be stationary or moving. Research works on formation control often use one or more of leader-following (e.g., [1], [2]), behavioral (e.g., [3], [4]), and use of virtual structures (e.g., [5], [6]) approaches in either a centralized or decentralized manner. Centralized control schemes (e.g., [2] and [7]) use a single controller that generates collision-free trajectories in the workspace. Although these guarantee a complete solution, centralized schemes require high computational power and are not robust due to the heavy dependence on a single controller. A nice application of formation control based on the potential field method [2] and Lyapunov’s direct method [8] to gradient climbing was recently addressed in [9]. However, the final configuration of formation cannot be foretold. On the other hand, decentralized schemes, see, e.g., [10] and [11], require less computational effort and are relatively more scalable to the team size. The decentralized approach usually involves a combination of agent-based local potential fields (e.g., [2], [12], and [13]). The main problem with the decentralized approach, when collision avoidance is taken into account, is that it is extremely difficult to predict and control the critical points of the controlled systems. An interesting work addressing
Manuscript received December 27, 2006; revised April 3, 2007. Manuscript received in final form June 17, 2007. Recommended by Associate Editor K. Kozlowski. The author is with the School of Mechanical Engineering, The University of Western Australia, Crawley, WA 6009, Australia (e-mail: [email protected]. au). Color versions of one or more of the figures in this brief are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TCST.2007.908214
geometric formation based on Voronoi partition optimization is given in [14], but the final arrangement of the agents cannot be foretold due to locality of Lloy’s algorithm. Recently, a method based on a different navigation function from [15] provided a centralized formation stabilization control design strategy proposed in [7]. This work is extended to a decentralized version in [11]. A similar result but based on different navigation function motivated by the work in [15] is given in [16]. However, the formation is stabilized to any point in workspace instead of being “tied” to a fixed coordinate frame. Moreover, the potential function, which possesses all properties of a navigation function (see [15]), is finite when a collision occurs. This complicates the analysis of collision avoidance in the sense that one cannot directly use the first time derivative of the potential function to prove no collisions between agents. In [7], [15], [16], and [11], the tuning constants, which are crucial to guarantee that the only desired equilibrium points are asymptotically stable and that the other critical points are unstable, are extremely difficult to obtain explicitly. In most of the above papers, point-robots with simple (single or double integrator) dynamics (e.g., [2], [7], [11], [13], [14], and [17]) or fully actuated vehicles [6] (which can be converted to double-integrator dynamics via a feedback linearization) were investigated. It should be mentioned that decentralized navigation of nonpoint agents with single-integrator dynamics was also investigated in [18], but each agent requires global knowledge of position of other agents. Vehicles with nonholonomic constraints were also considered (e.g., [3]). However, the nonholonomic kinematics are transformed to double-integrator dynamics by controlling the hand position instead of the inertial position of the vehicles. Consequently, the vehicle heading is not controlled. In addition, switching control theory [19] is often used to design a decentralized formation control system (e.g., [1], where a case-by-case basis is proposed), especially when the vehicles have limited sensing ranges and collision avoidance between vehicles must be considered. Clearly, it is more desirable if we are able to design a nonswitching formation control system that can handle the above decentralized and collision avoidance requirements. Moreover, in the tracking control of single nonholonomic mobile robots (e.g., [20]–[22]), where it seems that the backstepping technique was first used in [20] to take the robot kinetic into account in the control design, the tracking errors are often interpreted in a frame attached to the reference trajectory using the coordinate transformation in [23] to overcome difficulties due to nonholonomic constraints. If these techniques are migrated to formation control of a group of mobile robots, it is extremely difficult to incorporate collision avoidance between the robots. The above problems motivate the contribution of this brief. In this brief, cooperative controllers are designed to force a unicycle-type mobile robots with limited sensing group of ranges to perform desired formation tracking and to guarantee
1063-6536/$25.00 © 2008 IEEE Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
528
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
with
(3) Fig. 1. Robot parameters.
no collisions between the robots. The physical dimensions and dynamics of the robots are also considered. The control development is based on novel potential functions, which attain the minimum value when the desired formation is achieved and are equal to infinity when a collision occurs. Moreover, smooth and times differential bump functions are introduced and incorporated into the potential functions to avoid the use of switchings, which are often required in literature to deal with the robot’s limited sensing ranges. II. PROBLEM STATEMENT
and are the masses of the body and wheel with where , and are the moments of inertia of the body a motor, , (center of mass), the wheel about the vertical axis through with the rotor of a motor about the wheel axis, and the wheel with the rotor of a motor about the wheel diameter, respectively, , , and are defined in Fig. 1, and the nonnegative constants and are the damping coefficients. For convenience, we convert the wheel velocities of the robot to its linear and angular velocities by (4) where
A. Robot Dynamics
, and is invertible since . With (4), we can write the robot dynamics (1) as
follows:
We consider a group of mobile robots, of which each has the following dynamics [21]:
(5) (1) where , denotes the position , the coordinates of the middle point, , between the left and right driving wheels, and heading of the robot coordinated in the earth-fixed frame (see Fig. 1), , where and are the angular velocities of the wheels of the robot , , where and are the control torques applied to the wheels of the robot . The rotation matrix , mass matrix , Coriolis matrix , and damping matrix in (1) are given by where
with
(2) Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(6)
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
529
, where a strictly positive constant. Moreover, bounded. The constant vectors ,
, and is and are also , satisfy (10)
Fig. 2. Formation setup.
B. Formation Control Objective In this brief, we will study a formation control problem under the following assumption. Assumption 1: 1) The robot has a physical safety circular area, which is , has a radius , and has a circular centered at the point communication area, which is centered at the point and has a radius (see Fig. 2). The radius is strictly larger than , , . 2) The robot broadcasts its state and its reference trajectory in its communication area. Moreover, the robot can receive the states and reference trajectories broadcast by other robots , , , in the group if the points of these robots are in the communication area of the robot . 3) The dimensional terms of the robot are known to the robot . The terms involved with mass, inertia, and damping, , of the robots are unknown but constant. 4) At the initial time , each robot starts at a location that is outside of the safety areas of other robots in the group, i.e., there exists a strictly positive constant such that (7) , , where for all 5) The reference trajectory for the robot is which is generated by
for all , , where is a strictly positive constant. Remark 1: Items 1) and 2) in Assumption 1 specify the way each robot communicates with other robots in the group within its communication range. In Fig. 2, the robots and are communicating with each other since the points and are in the communication areas of the robots and , respecare not communicating with each tively. The robots and other since the points and are not in the communicaand , respectively. Similarly, the tion areas of the robots and are not communicating with each other. robots Item 3) makes sense in practice because the dimensional terms can be easily predetermined while the terms involving mass, inertia, and damping are often difficult to predetermine. Moreover, is known, the assumptions in this item mean that the matrix while the matrices and and coefficients of the entries of the matrix are unknown but constant. In item 5), the , specifies the desired formaconstant vectors , tion configuration with respect to the earth-fixed frame . The condition (9) implies that the common reference is regular and its velocity , which specifies how the desired formation, whose configuration is determined by , moves along , is bounded and satisfies a persistent excitation condition, i.e., the desired formation always moves along the common reference trajectory . The condition (10) specifies feasibility of the reference trajectories , (recall from (8) that , ) due to physical safety circular areas of the robots. Finally, all of the robots in the group require knowledge of the common reference trajectory since this trajectory specifies how the whole formation should move with respect to the earth-fixed frame . Formation Control Objective: Under Assumption 1, design the control input and update laws for all terms involving mass, inertia, and damping ( , , , , and ) for each robot such that each robot asymptotically tracks its desired reference trajectory while avoiding collisions with all other robots in the group, i.e., for all , , :
. , (8)
where is referred to as the common reference trajectory with being the common trajectory parameter, and is a constant vector. The trajectory satisfies the following conditions:
(11) where
, and
is a positive constant.
III. PRELIMINARIES We here present one definition and one lemma, which will be used in the control design in the next section. is called a times Definition 1: A scalar function differential bump function if it enjoys the following properties if if
(9)
is
if times differentiable with respect to
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(12)
530
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
A. Stage I Since the robot is underactuated, we divide this stage into two steps using the backstepping technique. In the first step, and the robot linear velocity are used the robot heading as immediate controls to fulfill the task of position tracking and collision avoidance. In the second step, the robot angular veis used as an immediate control to stabilize the error locity between the actual robot heading and its immediate value at the origin. We do not use the transformation in [23] to interpret the tracking errors in a frame attached to the reference trajectories as is often done in literature (e.g., [20]–[22]) to avoid difficulties when dealing with collision avoidance. 1) Step I.1: Define
Fig. 3. Twice-differentiable bump function.
where is a positive integer, , and and are constants such that . Moreover, if the function is infinite times differentiable with respect to , then it is called a smooth bump function. It is noted that bump functions are not new to the control combump function was used in [24] for munity. A nontrivial obstacle avoidance in flocking cooperation control of multiple agents with limited communication. Here, we give a constructive method to construct the times differentiable and smooth bump functions. These functions are useful when dealing with cooperative control of multiple agents with limited communication and high-order dynamics. Construction of these bump functions is given in the following lemma. Lemma 1: Let the scalar function be defined as
(15) and are virtual controls of and , respectively. where With (15), the first two equations of (5) are read as (16) where
and
(13) where the function if
is defined as follows: and
if
(14)
where is a positive integer. Then, the function is a times differentiable bump function. Moreover, if in , then property 4) is replaced (14) is replaced by is a smooth bump function. by: 4 ) Proof: See Appendix A. Fig. 3 illustrates a twice-differen, ). tiable bump function ( Remark 2: As specified in Lemma 1, the times differentiable bump functions can be obtained explicitly, i.e., the integral (13) can be solved analytically. The smooth bump functions cannot be obtained explicitly. Thus, a numerical procedure is needed to solve the integral (13). However, in many applications, including the formation control of mobile robots in this brief, the times differentiable bump functions are sufficient. IV. CONTROL DESIGN Since the robot dynamics (5) are of a strict feedback form [25] with respect to the robot linear velocity and angular velocity , we will use the backstepping technique [25] to design the control input . The control design is divided into two main stages. In the first stage, we consider the first three equations of being considered as immediate controls. In (5) with and the second stage, the actual control will be designed.
(17) To fulfill the task of position tracking and collision avoidance, we consider the following potential function: (18) and are the goal and related collision avoidance where functions for the robot specified as follows. • The goal function is designed such that it puts penalty on the tracking error for the robot and is equal to zero when the robot is at its desired position. A simple choice of this function is (19) • The related collision function should be chosen such that it is equal to infinity whenever any robots come in contact with the robot , i.e., a collision occurs, and attains the minimum value when the robot is at its desired location with respect to other group members belonging to the set robots, where is the set that contains all of the of robots in the group except for the robot . This function is chosen as follows:
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(20)
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
where the function is a function of and enjoys the following properties: and if 1) 2) if 3) and if 4) 5) 6)
with
if and . is at least three times differentiable with respect to if
(21)
, is a strictly positive constant where , , such that , and are positive constants, and , , and are defined as follows: , , if , , , and if . Remark 3: Properties 1)–4) imply that the function is positive definite, is equal to zero when all of the robots are at their desired locations, and is equal to infinity when a collision between any robots in the group occurs. Moreover, Property 1) and attains the function given in (19) ensure that the function the (unique) minimum value of zero when all of the robots are at their desired positions. Also, Property 3) ensures that the collision avoidance between the robots and is only taken into account when they are in their communication areas. Property 5) is used to prove stability of the closed-loop system. Property 6) ensures that we can use techniques such as the backstepping and direct Lyapunov design methods [25], [26] for control design and stability analysis for continuous systems instead of techniques for switched, nonsmooth and discontinuous systems [19], [27] to handle the collision avoidance problem under the robot’s limited sensing ranges. There are many functions that satisfy all properties of given in (21). An example is (22) where is a times differentiable and bump function defined in Definition 1 with , and . The time along the solutions of (16) satisfies derivative of (23) where we have used ,
, , and
,
(24) From (23), we choose a bounded control
531
denotes a vector of bounded functions of elements where with and of in the sense that the first and second rows of , i.e., . The function is a scalar, at least three-times differentiable and bounded function with respect to and satisfies ; 1) if if ; 2) 3) ; 4) for all , , where , , are strictly positive constants. Some functions that satisfy the above properties and . The strictly positive constant is are chosen such that (26) The above condition ensures that and are solvable from . We now need to solve for and from the expression of in (25) and (17). From (25) and (17), we have
(27) where
we
have
used and
since and (see Assumption 1). The left-hand sides of (27) are actually the in the and directions. Now, multiplying coordinates of and both both sides of the first equation of (27) with and then sides of the second equation of (27) with adding the resulting equations together yields
(28) On the other hand, multiplying both sides of the first equation and both sides of the second equation of of (27) with and then subtracting the resulting equations (27) with gives
(29) From (28) and (29), we solve for and as shown in (30), shown at the bottom of the next page. It is noted that (30) is well defined since , where the condition (26) has been used. Moreover, it is of interest to note and are at least twice-differentiable functions of that , , , , and with , . defined in (24) is substituted into (25), Remark 4: When the control can be written as
designed as follows: (25)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
(31)
532
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
It is seen from (31) that the argument of consists of two or , referred to parts. The first part, as the attractive force, plays the role of forcing the robot to its desired location. The second part, or , referred to as the repulsive force, takes care of collision avoidance for the robot with the other robots. Moreover, the immediate control of the robot given in (25) depends on only its own state and reference trajectory and the of these robots states of other neighbor robots if the points are in the circular communication area of the robot , since out[see Property 3) of ]. side this area Now, substituting (25) into (23) results in
whose derivative along the solutions of (32) and (35) satisfies
(32) Substituting (25) into (16) results in
(37) It is noted that
(33) as an immediate control 2) Step I.2: In this step, we view at the origin. As such, we define to stabilize
and smooth functions for all as virtual control
is well defined since are . From (37), we choose the
(34) is a virtual control of . To prepare for designing where , let us calculate . Differentiating both sides of the first equation of (15) along the solutions of (34), the third equation of (5) and the second equation of (30) results in
(35) To design the virtual control
, we consider the function (36)
(38)
where is a positive constant. It is of interest to note that is an at least once-differentiable function of , , , , , , , , , and , with , . Moreover, it contains only the should be noted that the virtual control state and reference trajectory of the robot , and the states of other neighbor robots if the points of these robots are in the communication area of the robot , because outside this area thanks to Property 3) of . Substituting (38)
(30)
(39)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
into (37) results in (39), shown at the bottom of the previous page, after some manipulation. Substituting (38) into (35) gives
533
B. Stage II In this stage, we design the actual control input vector and for each update laws for unknown system parameter vector robot . To do so, we consider the following function:
(44) (40)
To prepare for the next section, let us compute the term , where . From the second equation of (15), (34), and the last equation of (5), we have
where and is an estimate of , and is a symmetric positive definite matrix. Differentiating both sides of (44) along the solutions of (41) and (39) yields
(41) where
(45)
which suggests that we choose (46), shown at the bottom of the , and is a symmetric page, where positive definite matrix. Substituting the first equation of (46) into (41) gives (42)
(43)
where , . Again, and contain only the state and reference trajectory of the robot , and the states of other neighbor robots if the points of these robots are in the communication area of the robot , , , and because outside this area , thanks to Property 3) of .
(47) By construction, the control and the update given in (46) of the robot contain only the state and reference trajectory of the robot , and the states of other neighbor robots if these robots of the robot are in a circular area, which is centered at point
(46)
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
534
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
Fig. 4. Simulation results with ten robots.
and has a radius no greater than (45) results in
. Now, substituting (46) into
(48) For convenience, we rewrite the closed-loop system consisting of (33), (40), (47), and the second equation of (46) as follows:
(49) We now state the main result of our brief in the following theorem. Theorem 1: Under Assumption 1, the control and the update law given in (46) for the robot solve the formation control objective. In particular, no collisions between any robots
, the closed-loop system (49) is forcan occur for all ward complete, and the position and orientation of the robots track their reference trajectories asymptotically in the sense of (11). Proof: See Appendix B. V. SIMULATIONS Here, we simulate formation tracking control of a group of identical mobile robots to illustrate the effectiveness of the proposed controller. The physical parameters of the robot , , , are taken from [21]: , , , , , , , , and , . The robots are initialized as follows: , , , , where for , and for . The initial values of , , are taken as random numbers between 0 and . The initial values of are taken as half of their true values. , The reference trajectories are taken as and . This choice of the reference trajectories means that the common refforms a sinusoidal trajectory and that the erence trajectory desired formation configuration is a polygon whose vertices uniformly distribute on a circle centered on the common reference , , trajectory and with a radius 10. The functions , are taken as in (22) with , , and . is taken as . The control gains and upThe function , , , and date gains are chosen as , where is a -dimensional identity matrix. Indeed, the above choice of satisfies condition (26). Simulation results are plotted in Fig. 4. It is seen that all robots nicely track their reference trajectories. During the first four seconds, the robots quickly move away from each other to avoid collisions then
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
track their desired reference trajectory in Fig. 4(a), where the trajectory of the robot is plotted in the thick line. For clarity, of the robot are plotted in only the control inputs Fig. 4(b). Fig. 4(c) plots the tracking errors , , of the robot . Indeed, these errors tend to zero and and other asymptotically. The distances between the robot robots are plotted in Fig. 4(d). Clearly, these distances are al, , i.e., there ways larger than are no collisions between the robot and all other robots in the group. Moreover, in Fig. 4(e), we plot the product of all gaps . It between robots: is seen that is always larger than zero. This means that , , , i.e., no collisions between any agents occurred. For clarity, we only plot the results for the first 20 s in Fig. 4(b)–(e). VI. CONCLUSION We have presented a method to design cooperative formaunicycle-type motion tracking controllers for a group of bile robots with limited sensing ranges. The control design was constructed in such a way that the robots asymptotically track their reference trajectories and avoid collisions among them. The novel potential functions with embedded smooth or/and times differential bump functions are attractive parts of the brief since they do not require the use of switching control theory despite the robot limited sensing ranges. These functions can certainly be modified to solve other cooperative control problems such as flocking and consensus of mobile agents. In addition, and is the method of constructing the virtual controls also attractive, since they are derived directly from the position tracking errors instead of transforming the robot kinematics to a body frame as often done in the literature. Future work is an extension of the proposed techniques in this brief and those for single underactuated ocean vessels in [28] and [29] to achieve a desired formation for a group of underactuated vessels.
535
any positive integer. We will prove Property 4 ) by induc. It is clear that tion. First of all, we check that . On the other hand, where and we have used l’Hopital’s rule. Since both left- and right-hand . Now assume that limits are equal to 0, we have . We now compute . The left-hand limit is equal to 0 as above. The right hand limit is
(by l’Hopital’s rule), where and is another polynomial of . Since both left- and right-hand limits are equal to 0, we have , which completes the proof of Property 4 ). APPENDIX B PROOF OF THEOREM 1 We first prove that no collisions between the robots can occur, the closed-loop system (49) is forward complete and that the robots asymptotically approach their target points or some critical points. We then investigate stability of the closed-loop system (49) at these points and show that the position and orientation of the robots asymptotically track their reference trajectories. Proof of No Collisions and Complete Forwardness of Closed Loop System: From (48) and properties of the function [see , which implies that , (26)], we have . With definition of the function in (44) and its components in (36), (18), (19), and (20), we have
APPENDIX A PROOF OF LEMMA 1 given in We need to verify that the function (13) satisfies all properties defined in (12). Property 1) , we have holds because, by (14), for all . Property 2) holds since, by (14), for we have . Property 3) holds because it is not hard to show that all
(50)
for all . To prove Property 4), we just need to show that is times differentiable. We first note is smooth except at . Hence, we only need that for any to verify that . Clearly, since positive integer , . On the other hand, since , we have . Since both left- and right-hand limits are equal to 0, we have . Hence, Property 4) holds. Now we turn to the case where in (14) is replaced by . Properties 1)–3) can be proven without difficulty. We focus on proof of Property 4 ). , We first note that is a polynomial function of , and is where
for all . From the condition specified in item 4) , and the definition of , of Assumption 1, Property 5) of , the right-hand side of (50) is bounded by a positive constant depending on the initial conditions. The boundedness of the right-hand side of (50) implies that the left-hand side of (50) must be smaller than must be also bounded. As a result, some positive constant depending on the initial conditions for . From properties of [see (21)], all must be larger than some positive constant depending on the initial conditions denoted by , i.e., there are no colli. Boundedness of the left-hand side sions for all , , , and of (50) also implies that of
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
536
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
for all . This in turn implies by construc, , , , and do not escape to tion that infinity in finite time. Therefore, the closed-loop system (49) is forward-complete. Equilibrium Points: Since we have already proved that there are no collisions between any robots, an application of [26, Theorem 8.4] to (48) yields
Properties of Equilibrium Points: The system (54) can be written in a vector form as (55) where
point
, , and . Therefore, near an equilibrium , which can be either or , we have (56)
(51) as specified in item 5) of By noting that Assumption 1, the limit equation (51) implies that
By construction, imply that definition of in (24),
and . Moreover, from the means (52)
The
limit
equation
element of the matrix , , where set of all agents. A simple calculation shows that
where the
(52) implies that can tend to
since (Property 1) of ), or some vector denoted by as the time goes to infinity, i.e., the equilibrium points can be or . It is noted that some elements of can be equal to that of . However, for simplicity, we abuse the notation, i.e., we still denote that vector as . Indeed, the vector is such that
is is the
(57) for all , , , where denotes the identity matrix of size . Let be the set of the agents such that, if the agents and belong to the set , then . Next, we will show that is asymptotically stable and that is unstable. Proof of Being Asymptotic Stable: Using properties of and listed in (21) and (26), we have from (57) that for all ,
(58) (53) To investigate properties of the equilibrium points, and , we consider the first equation of the closed-loop system (49), i.e., (54) Since we have already proved that the closed-loop system and (49) is forward complete, and imply from the expressions of and [see (17)] that , we treat as an input to (54) instead of a state. Moreover, we have already proved that the trajectory can approach either the set of desired equilibrium points denoted by or the set of undesired equilibrium points denoted by “almost globally.” The term “almost globally” refers to the fact that the agents start from a set that includes both condition (7) and that does not coincide at any point with the set of the undesired saddle point . Therefore, we now need to prove that is locally asymptotically stable and that is locally unstable. Once this is proved, we can conclude that the trajectory approaches from almost everywhere except for from the set denoted by the condition (7) and the set denoted by , which is unstable.
where
and . We consider the function
, with (59)
whose derivative along the solutions of (56) with replaced by , using (58), and noting that satisfies
(60) [see Property 1) in (21)]. The last inequality since of (60) implies that is asymptotically stable because
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
, and we have already proved that . Proof of Being Unstable: Again, using properties of and in (21) and (26), we have from (57) that
537
and repulsive forces are nonzero). Therefore, the point, say , , , , must locate between the where , , i.e., there exists a points and for all strictly positive constant such that , which is substituted into (64) to yield (65)
(61)
, , , there exists a nonempty Since such that, for all , , set is strictly negative, i.e., there exists a strictly positive constant such that , , . We now write (63) as
for all
, , where , , and . Since the related collision avoidance functions [see (20)] are specified in terms of relative distances between agents and it is extremely difficult to explicitly by solving (53), it is very difficult to use obtain the Lyapunov function candidate to investigate stability of (56) at . Therefore, we consider the Lyapunov function candidate (62) and . Differentiating both sides of (62) along the solution of (56) with replaced by gives where
(66) where
. We now define a subspace such that and , In this subspace, we have
,
, .
(63) and (61) has been used. To investigate stability where properties of based on (63), we will use (53). Define , , where [see (53)]. Therefore, . Hence, , , which, by using (53), is expanded to
(67)
Using (67) as
,
,
, we can write
(68)
(64) where . The sum is strictly negative , , , since, at the point, say , where all attractive and repulsive forces are equal to zero, while at the , , the sum of point, say , where attractive and repulsive forces are equal to zero (but attractive
Now assume that
is a stable equilibrium point, i.e., , , where is a , we have nonnegative constant. Noting that , , which im, plies that
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
538
IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 16, NO. 3, MAY 2008
, stant, since consider two cases:
, where and
is a nonnegative con. We now and
. : Since (Assumption 1) and we have already , , in (68) is shown that divergent. Therefore, cannot tend to a constant but must be divergent. This contradicts , i.e., cannot be a set of stable equilibrium points but must be a set of an unstable ones in this case. : There would Case I: be no contradiction. However, this case is never observed in practice since the ever-present physical noise would cause to be different from zero at the . We now need to show that, once the sum time is different from zero, this sum will , i.e., the set of undesired not come back zero again for all equilibrium points is not attractive. To do so, consider (68) with the initial time instead of , i.e., Case I:
(69)
and , where is for a positive constant. Since (Assumption 1) , , and we have already shown that in (69) is divergent for . Therefore, , for , cannot tend to a constant but must be divergent. , i.e., This contradicts must also be a set of unstable ones point in this case. The proof of Theorem 1 is completed. ACKNOWLEDGMENT The author would like to thank the reviewers for their helpful comments which helped him improve the brief significantly. REFERENCES [1] A. K. Das, R. Fierro, V. Kumar, J. P. Ostrowski, J. Spletzer, and C. J. Taylor, “A vision based formation control framework,” IEEE Trans. Robot. Autom., vol. 18, no. 5, pp. 813–825, Oct. 2002. [2] N. E. Leonard and E. Fiorelli, “Virtual leaders, artificial potentials and coordinated control of groups,” in Proc. 40th IEEE Conf. Decision Control, Orlando, FL, 2001, pp. 2968–2973. [3] R. T. Jonathan, R. W. Beard, and B. J. Young, “A decentralized approach to formation maneuvers,” IEEE Trans. Robot. Autom., vol. 19, no. 6, pp. 933–941, Dec. 2003. [4] T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE Trans. Robot. Autom., vol. 14, no. 6, pp. 926–939, Dec. 1998. [5] M. A. Lewis and K.-H. Tan, “High precision formation control of mobile robots using virtual structures,” Auton. Robots, vol. 4, no. 4, pp. 387–403, 1997.
[6] R. Skjetne, S. Moi, and T. I. Fossen, “Nonlinear formation control of marine craft,” in Proc. 41st IEEE Conf. Decision Control, Las Vegas, NV, 2002, pp. 1699–1704. [7] H. G. Tanner and A. Kumar, “Towards decentralization of multi-robot navigation functions,” in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain, 2005, pp. 4132–4137. [8] P. Ogren, M. Egerstedt, and X. Hu, “A control Lyapunov function approach to multi-agent coordination,” IEEE Trans. Robot. Autom., vol. 18, no. 5, pp. 847–851, Oct. 2002. [9] P. Ogren, E. Fiorelli, and N. E. Leonard, “Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment,” IEEE Trans. Autom. Control, vol. 49, no. 8, pp. 1292–1302, 2004. [10] D. M. Stipanovic, G. Inalhan, R. Teo, and C. J. Tomlin, “Decentralized overlapping control of a formation of unmanned aerial vehicles,” Automatica, vol. 40, no. 8, pp. 1285–1296, 2004. [11] H. G. Tanner and A. Kumar, “Formation stabilization of multiple agents using decentralized navigation functions,” Robot.: Sci. Syst. I, pp. 49–56, 2005. [12] H. G. Tanner, A. Jadbabaie, and G. J. Pappas, “Stable flocking of mobile agents. Part II: Dynamics topology,” in Proc. 42nd IEEE Conf. Decision Control, Maui, HI, 2003, vol. 2, pp. 2016–2021. [13] S. S. Ge and Y. J. Cui, “New potential functions for mobile robot path planning,” IEEE Trans. Robot. Autom., vol. 16, no. 5, pp. 615–620, Oct. 2000. [14] J. Cortes, S. Martinez, and T. K. F. Bullo, “Coverage control for mobile sensing networks,” IEEE Trans. Robot. Autom., vol. 20, no. 2, pp. 243–255, Apr. 2004. [15] E. Rimon and D. E. Koditschek, “Robot navigation functions on manifolds with boundary,” Adv. Appl. Math., vol. 11, no. 4, pp. 412–442, 1990. [16] D. V. Dimarogonas and K. J. Kyriakopoulos, “Formation control and collision avoidance for multi-agent systems and a connection between formation infeasibility and flocking behavior,” in Proc. 44th Conf. Decision Control/Eur. Control Conf., Seville, Spain, 2005, pp. 84–89. [17] R. Olfati-Saber and R. M. Murray, “Distributed cooperative control of multiple vehicle formations using structural potential functions,” presented at the 15th IFAC World Congr., Barcelona, Spain, 2002. [18] D. V. Dimarogonas, S. G. Loizou, K. J. Kyriakopoulos, and M. M. Zavlanos, “A feedback stabilization and collision avoidance scheme for multiple independent non-point agents,” Automatica, vol. 42, no. 2, pp. 229–243, 2006. [19] D. Liberzon, Switching in Systems and Control. Cambridge, MA: Birkauser, 2003. [20] R. Fierro and F. L. Lewis, “Control of a nonholonomic mobile robot: Backstepping kinematics into dynamics,” in Proc. 34th IEEE Conf. Decision Control, New Orleans, LA, 1995, vol. 4, pp. 3805–3810. [21] T. Fukao, H. Nakagawa, and N. Adachi, “Adaptive tracking control of a nonholonomic mobile robot,” IEEE Trans. Robot. Autom., vol. 16, no. 5, pp. 609–615, Oct. 2000. [22] K. D. Do, Z. P. Jiang, and J. Pan, “A global output-feedback controller for simultaneous tracking and stabilization of unicycle-type mobile robots,” IEEE Trans. Robot. Autom., vol. 20, no. 3, pp. 589–584, Jun. 2004. [23] C. Samson, “Velocity and torque feedback control of a nonholonomic cart,” in Advanced Robot Control, ser. Lecture Notes in Control and Information Sciences, C. Canudas de Wit, Ed. Berlin, Germany: Heidelberg, 1991, pp. 125–151. [24] R. O. Saber and R. M. Murray, “Flocking with obstacle avoidance: Cooperation with limited communication in mobile networks,” in Proc. 42nd IEEE Conf. Decision Control, Maui, HI, 2003, vol. 2, pp. 2022–2028. [25] M. Krstic, I. Kanellakopoulos, and P. V. Kokotovic, Nonlinear and Adaptive Control Design. New York: Wiley, 1995. [26] H. Khalil, Nonlinear Systems. Upper Saddle River, NJ: Prentice-Hall, 2002. [27] H. G. Tanner and K. J. Kyriakopoulos, “Backstepping for nonsmooth systems,” Automatica, vol. 39, no. 7, pp. 1259–1265, 2003. [28] K. D. Do, Z. P. Jiang, and J. Pan, “Universal controllers for stabilization and tracking of underactuated ships,” Syst. Control Lett., vol. 47, no. 4, pp. 299–317, 2002. [29] K. D. Do and J. Pan, “Underactuated ships follow smooth paths with integral actions and without velocity measurements for feedback: Theory and experiments,” IEEE Trans. Control Syst. Technol., vol. 14, no. 2, pp. 308–322, Mar. 2006.
Authorized licensed use limited to: University of Western Australia. Downloaded on July 21,2010 at 00:08:00 UTC from IEEE Xplore. Restrictions apply.
