Formation Control with Configuration Space Constraints
Formation Control with Configuration Space Constraints Guilherme A. S. Pereira§ §
Aveek K. Das†
Vijay Kumar†
Mario F. M. Campos§
VERLab, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil. † GRASP Lab., University of Pennsylvania, Philadelphia, PA, USA. {gpereira, mario}@dcc.ufmg.br, {aveek, kumar}@grasp.cis.upenn.edu
Abstract— We address the problem of controlling a team of robots subject to constraints on relative positions. We adopt the general framework of leader-follower control in [1], [2] in which a network of controllers is used to control the position and orientation of the team and its shape. We propose two improvements to this scheme. First, we introduce cooperative leader-following where the motion of a robot is determined not only by its leader, but also by other robots including their followers. Second, we allow constraints that are induced by limitations on ranges of sensors and wireless network cards. Our approach is based on potential field controllers for each robot and the on-line modification of these controllers to accommodate motion constraints induced by other robots in the group. We present experimental results with a team of three car-like robots equipped with omnidirectional cameras and 802.11b network cards.
I. I NTRODUCTION Many approaches for motion coordination of large scale multi-robot systems use the leader-following framework [1], [3], [4]. In this framework, each robot has at least one designated leader. Leaders can be other robots in the group or virtual robots that represent pre-computed trajectory supplied by a higher level planner. Thus, each robot is a follower that tries to maintain a specified relative configuration (a fixed separation and bearing for example) to its leader(s). One disadvantage with this framework is that there is a explicit dependence of the motion of followers on their leaders, but the leaders’ motion is independent of their followers. If, for example, a robot fails or slows down, its followers’ motion will be directly affected by this behavior, while its leaders will continue their task without modifying their plans. In situations where it is important to maintain a sensing or communication network, a single failure could result in the failure of the task. In this paper we modify the notion of leader-following and present a framework where robots change their motion plans in real time in order to satisfy constraints related to other robots. These constraints may have to do with a task of maintaining a pre-specified formation. Alternatively, robots may have constraints because of limited ranges or fields of view of sensors, or of communication radios and antenna. Thus we introduce cooperative leader-following, a modification of the standard leader-following approach, where the motion of the robots can be dependent not only on their leaders but also on other robots including their followers.
In our previous work [5], [6], we solved a similar problem assuming that all robots had prior knowledge of the motion plan for the group in the form of a navigation function. Further, all the robot velocities derived from the gradients of the navigation function were assumed to be equal or close to each other. Each robot could deviate from this motion plan in order to satisfy formation constraints, inequality constraints on individual robots induced by the other robots in the team. This approach was used for cooperative manipulation [5], and to maintain communication and sensing constraints [6]. In this paper we use a similar approach for formation control. We still use the same definition of formation constraint, but now we consider formation setpoints as in [1], [2], leading to equality constraints. Thus, our constraints are divided into two types: (i) equality constraints that specify the relative position between a robot and its leader, and (ii) inequalities constraints that characterize a configuration space between a robot and other robots in the group. The equality constraints (i) are standard constraints in formation control [1], [2], [3], [4]. Constraints of type (ii) are used to maintain communication and sensor constraints allowing the group to deviate from the prescribed formation in (i). This allows, for example, a leader to wait for a possibly slow follower, allowing the team to adapt to failures. This kind of behavior can be found in centralized formation control approaches such as the one presented in [7], but are not explored in any decentralized control policies [1], [3]. Our goal in this paper, is to address both types of specifications (i) and (ii) with decentralized control policies. Our framework uses potential field controllers [8], [9], [10]. For a single robot navigating an obstacle field, a potential function with a single minimum in the goal position provide a Lyapunov function that guarantees the robot’s convergence to the goal [9]. Potential functions can be locally modified to accommodate unmodeled obstacles or dynamic constraints [11]. When the domain is convex, these modifications lead to guarantees on global performance. In order to leverage these results, we design potential functions for a robot to enable it to follow a leader. Each potential function is a function of the leader’s and follower’s position. We change the potential functions in real time to accommodate dynamic constraints while providing results on convergence for a team of holonomic, fully-actuated robots. We also point to extensions to
(a)
(b)
R3
R5
R3
R1
R4
R5
R2
R1
R4
R2
Fig. 1. Graph modeling for a group of 5 robots: (a) – formation control graph; (b) – constraint graph.
non-holonomic robots and present experiments with our team of car-like robots with omnidirectional cameras and wireless network cards. II. P ROBLEM DEFINITION Consider a planar world, W = R2 , occupied by a group R = {R1 , R2 , . . . Rn } of n robots. The ith robot Ri is represented by the configuration qi in the configuration space C. A formation of n robots is represented by a directed graph called formation control graph1, Gf = (R, Ef ), and a second directed graph called constraint graph, Gc = (R, Ec ), where R is the set of nodes and Ef and Ec are edge sets. For the formation control graph, Gf , each edge eij = (Ri , Rj ) ∈ Ef is associated with a specification for Rj following Ri . For each edge, Ri is the leader and Rj is the follower. The robot that does not have any leaders and is responsible for guiding the others through the environment is called the lead robot [1]. Only one lead robot is allowed in our approach. Also, the robots that do not have any followers are called terminal followers. Figure 1(a) shows an example of a formation control graph where R3 is the lead robot and R2 , R4 and R5 are terminal followers. Robot R1 follows R3 and is followed by R2 and R4 . The edges eij = (Ri , Rj ) ∈ Ec of Gc are associated with constraints on relative position and orientation. While Ef describes leader-following relationships and set-points for the shape of the formation, Ec describes inequalities that reflect constraints such as communication and sensing constraints. Figure 1(b) shows an example of a constraint graph. In this figure R3 , for example, needs to maintain constraints with respect to R1 and R5 . The bidirectional edge between R2 and R4 indicates that these robots need to maintain constraints with each other. With the previous model, the control problem can be divided in two parts namely graph assignment and controller design. The first problem involves designing Gf and Gc and is not the main focus of this paper. Measures of performance that depend on Gf are discussed in [12] and heuristics for selecting edges are described in [13]. This paper is concerned with the problem of maintaining the formation described by Gf and the constraints described 1 The term control graph is used in [1], [2] to describe what we are calling a formation control graph.
by Gc . We assume that graphs themselves are preassigned and focus our attention on controlling the robots to satisfy the edge specifications. For Gf , the specification for each edge is a configuration for robot Rj with respect to its leader Ri . On the other hand, the specification for each edge in Gc is a convex function g(qi , qj ) that represents the allowable configuration space for Rj parameterized by the configuration of Ri . While Gf specifies, for each robot (except the lead), a unique point in configuration space, Gc specifies the allowable subset of configuration space. Although Gf and Gc are apparently independent, in order to allow robot Ri to reach its set-point qid (qj ) specified by Gf and still satisfy the constraints specified by Gc , we need to guarantee that, except for the lead robot, qid is inside the allowable configuration space Cid (q1 , . . . , qi−1 , qi+1 , . . . , qn ) defined by all constraints in Gc . Thus, the edge definition for the two graphs must satisfy the following condition: (q1d , q2d , . . . , qnd ) ∈ C1d × C2d × · · · × Cnd .
(1)
Moreover, since each is an intersection of convex sets, the right hand side of (1) is also a convex set. Therefore, if the robots are initially inside this set, they can always reach their goal configurations without going out of the set. Our goal in this paper is to design control laws that take in account the formation set-points and the allowable configuration spaces. Before continuing any further we will make three assumptions: Cid
Assumption 1 All robots are identical in terms of geometry, and in terms of capabilities and constraints related to sensing, communication, control, and mobility. Assumption 2 The robots are fully-actuated, holonomic, point robots. For the ith robot, the dynamical model is then given by: q˙i = ui , where qi = (xi , yi ). Assumption 3 Gf is acyclic and the in-degree at each node is 1. In other words, every follower has only one leader2 . III. P OTENTIAL F UNCTIONS In this paper, we use artificial potential fields to plan and control the robots’ motion. Potential field methods yield closed loop controllers that allow convergence to the goal in the presence of actuator and sensor noise and other disturbances [8]. Thus, for a potential function, φi , robot Ri ’s input is given by ui = −k∇φi (qi ) where ∇φi (qi ) is the gradient of φi computed in the configuration qi . The integral curves of the vector field formed by −∇φi (qi ), define implicity paths from every start configuration in C to the goal configuration qid . As pointed out in [11], a 2 This is somewhat restrictive since the in-degree for systems with two inputs can be up to two [1], [2].
potential function with a single minimum in qid can be thought of as a Lyapunov function for the system q˙i = ui (qi ), ui (qi ) = −∇φi (qi ), because φi (qi ) is positive definite and its value is, by definition, always decreasing along system trajectories. In the leader-following problem, we chose a navigation function [9] as a potential function for the lead robot. Navigation functions are constructed as the functions that solve the non-cooperative problem of steering a individual robot towards the goal while avoiding the static obstacles in the environment. The methodology presented in [9] can be directly applied for designing the function. While these functions can be very complicated, we limit the class of functions to quadratic functions in this paper. For the robots that have at least one leader, the potential function is constructed as a function of the leaders’ position. In the case that Ri follows Rj , we can describe the follower’s relative configuration in local coordinates as q¯ = (qj − qi ). We consider a quadratic Lyapunov function candidate of the form: 1 d φi (¯ q ) = k¯ q − q¯k2 . 2 If φi is a Lyapunov function we can use it as a leaderfollowing potential function. The input for the follower robot Ri is then given by the negative of the gradient of φi (qi , qj ) as: ui = −k∇φi = −k(¯ q d − q¯) , where k is a positive constant and ∇φi = ∂φi /∂qi . The derivative of the potential function for this input is given by: φ˙ i = −∇φi · q¯˙ = −∇φi · (q˙j − q˙i ) = −∇φi · (q˙j − ui ) = −∇φi · (q˙j + k∇φi ) = −kk∇φi k2 − ∇φi · q˙j . Observe that φ˙ i decreases along the system trajectory if: kk∇φi k2 > −∇φi · q˙j . In the worst case, q˙j and −∇φi are parallel and the previous condition can be written as the following sufficient condition: kk∇φi k > kq˙j k . (2) Because a real robot is subject to dynamics, there is a practical limit on its velocities. We assume each robot (i.e., all leaders) have a maximum velocity of q˙max . From Equation (2), it is clear that if we exclude the region given by the ball: q˙max k¯ q d − q¯k < γ = , k φ˙ i decreases along the trajectories of the system. Thus we can show that trajectories that start outside the ball (i.e., when k¯ q d − q¯k > γ), will converge to the ball. In other words, φ˙ i < 0 for k¯ q d − q¯k ≥ γ. The constant γ is the maximum allowable steady state error in q¯.
We note that it is possible to make γ arbitrarily small by allowing for feedforward control. If the follower input is given by: ui = −k∇φi + q˙j = −k(¯ q d − q¯) + q˙j , where q˙j is feedforward information, the controller exponentially converges to q¯ = 0. The feedforward velocity requires estimation of the leader’s velocity by the follower robot and is discussed elsewhere [2]. IV. C ONSTRAINTS As mentioned before, with each edge (Ri , Rj ) ∈ Ec , we associate a configuration constraint for Rj induced by Ri as a inequality of the form g(qi , qj ) ≤ 0. For example, if Rj must keep Ri in sight using a omnidirectional camera, g(qi , qj ) = (xi − xj )2 + (yi − yj )2 − r2 [6]. In this paper each constraint g(qi , qj ) defines three regions in the configuration spaces of Ri and Rj (see Figure 2). In the safe region, g(qi , qk ) < δ, where the small negative number δ can be thought of as a threshold. The region defined by δ ≤ g(qi , qk ) < 0 is the critical region for the robot. The constant δ is designed in order to guarantee that the constraint is still satisfied in the critical region and also to ensure that the robot does not leave this region. We say that a constraint is active when g(qi , qk ) ≥ δ. If g(qi , qk ) ≥ 0 the robot is in the unsafe region. Depending on the nature of the constraints, the robots may not be able to return to the safe region of the configuration space. Our decentralized controllers are designed with the objective of keeping the robots in the safe configuration space. V. C ONTROLLERS Our control system is decentralized and implemented using a set of three reactive controllers. Based on the two graphs, Gf and Gc , described in Section II we define a third graph that will govern the switching between the controllers. We call this time dependent graph that changes with the state of the robots, the formation graph H = (R, Eh ), where Eh is defined as the union of two subsets of Ef and Ec : Eh = E¯f ∪ E¯c , E¯f = {eij | [eij ∈ Ef ] ∧ [g(k, i) < 0 ∀eki ∈ Ec , Rk ∈ R]} , E¯c = {eij | [eij ∈ Ec ] ∧ [g(i, j) ≥ δ]} . Thus based on H, each robot Ri has three basic behaviors or modes depending on the number and type of incoming edges at Ri . If there is only one incoming control edge (eji ∈ Ef ), the robot is in the S AFE mode, corresponding to the safe region in Section IV. The control law in this mode is given by: ui = −k∇φi ,
(3)
where ∇φi is the gradient vector of the potential function φi , and k is a positive gain. For the lead robot it is
Critical δ
Safe
R3
Unsafe g(qi, qj) < 0
g(qi, qj)
0
R5
g(qi, qj) < δ
R1
R4
Fig. 2. The activation of the constraints define three regions in the robots’ configuration space.
a deliberative controller with a pre-planned navigation function that guides the robot toward the goal. For the followers it is a reactive controller designed to maintain the edge of the graph as shown in Section III. When all incoming edges are constraint edges (eji ∈ Ec ), Ri is in the U NSAFE mode. The robot tries to move in order to satisfy the constraints without using the potential function. In other words, the constraints themselves act as potential fields attracting the robots to each other and forcing them into a feasible configuration that satisfies all the constraints. The control input in this mode is: ui = −k ∇g j ,
(4)
where ∇g j is the gradient of the constraint g j (qi , qj ) defined by ∂g j /∂qi and g j is the constraint induced on Ri by Rj . If Ri has more than one active constraint to be satisfied, ∇g j represent the sum of their gradients. The third mode is a linear combination of the other two. A robot switches to this mode if it has one incoming control edge (eji ∈ Ef ) and at least one constraint edge (eji ∈ Ec ). The robot must navigate toward its goal while maintaining the constraint whose boundary it is closest to. This mode is called the C RITICAL mode. The input in the mode is: ui = −k(αi ∇g j + ∇φi ) , (5) where 0 < αi ≤ 1. The constant αi determines how much each robot will deviate to its main objective in order to preserve a constraint. It must be chosen so that: αi
Aveek K. Das†
Vijay Kumar†
Mario F. M. Campos§
VERLab, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil. † GRASP Lab., University of Pennsylvania, Philadelphia, PA, USA. {gpereira, mario}@dcc.ufmg.br, {aveek, kumar}@grasp.cis.upenn.edu
Abstract— We address the problem of controlling a team of robots subject to constraints on relative positions. We adopt the general framework of leader-follower control in [1], [2] in which a network of controllers is used to control the position and orientation of the team and its shape. We propose two improvements to this scheme. First, we introduce cooperative leader-following where the motion of a robot is determined not only by its leader, but also by other robots including their followers. Second, we allow constraints that are induced by limitations on ranges of sensors and wireless network cards. Our approach is based on potential field controllers for each robot and the on-line modification of these controllers to accommodate motion constraints induced by other robots in the group. We present experimental results with a team of three car-like robots equipped with omnidirectional cameras and 802.11b network cards.
I. I NTRODUCTION Many approaches for motion coordination of large scale multi-robot systems use the leader-following framework [1], [3], [4]. In this framework, each robot has at least one designated leader. Leaders can be other robots in the group or virtual robots that represent pre-computed trajectory supplied by a higher level planner. Thus, each robot is a follower that tries to maintain a specified relative configuration (a fixed separation and bearing for example) to its leader(s). One disadvantage with this framework is that there is a explicit dependence of the motion of followers on their leaders, but the leaders’ motion is independent of their followers. If, for example, a robot fails or slows down, its followers’ motion will be directly affected by this behavior, while its leaders will continue their task without modifying their plans. In situations where it is important to maintain a sensing or communication network, a single failure could result in the failure of the task. In this paper we modify the notion of leader-following and present a framework where robots change their motion plans in real time in order to satisfy constraints related to other robots. These constraints may have to do with a task of maintaining a pre-specified formation. Alternatively, robots may have constraints because of limited ranges or fields of view of sensors, or of communication radios and antenna. Thus we introduce cooperative leader-following, a modification of the standard leader-following approach, where the motion of the robots can be dependent not only on their leaders but also on other robots including their followers.
In our previous work [5], [6], we solved a similar problem assuming that all robots had prior knowledge of the motion plan for the group in the form of a navigation function. Further, all the robot velocities derived from the gradients of the navigation function were assumed to be equal or close to each other. Each robot could deviate from this motion plan in order to satisfy formation constraints, inequality constraints on individual robots induced by the other robots in the team. This approach was used for cooperative manipulation [5], and to maintain communication and sensing constraints [6]. In this paper we use a similar approach for formation control. We still use the same definition of formation constraint, but now we consider formation setpoints as in [1], [2], leading to equality constraints. Thus, our constraints are divided into two types: (i) equality constraints that specify the relative position between a robot and its leader, and (ii) inequalities constraints that characterize a configuration space between a robot and other robots in the group. The equality constraints (i) are standard constraints in formation control [1], [2], [3], [4]. Constraints of type (ii) are used to maintain communication and sensor constraints allowing the group to deviate from the prescribed formation in (i). This allows, for example, a leader to wait for a possibly slow follower, allowing the team to adapt to failures. This kind of behavior can be found in centralized formation control approaches such as the one presented in [7], but are not explored in any decentralized control policies [1], [3]. Our goal in this paper, is to address both types of specifications (i) and (ii) with decentralized control policies. Our framework uses potential field controllers [8], [9], [10]. For a single robot navigating an obstacle field, a potential function with a single minimum in the goal position provide a Lyapunov function that guarantees the robot’s convergence to the goal [9]. Potential functions can be locally modified to accommodate unmodeled obstacles or dynamic constraints [11]. When the domain is convex, these modifications lead to guarantees on global performance. In order to leverage these results, we design potential functions for a robot to enable it to follow a leader. Each potential function is a function of the leader’s and follower’s position. We change the potential functions in real time to accommodate dynamic constraints while providing results on convergence for a team of holonomic, fully-actuated robots. We also point to extensions to
(a)
(b)
R3
R5
R3
R1
R4
R5
R2
R1
R4
R2
Fig. 1. Graph modeling for a group of 5 robots: (a) – formation control graph; (b) – constraint graph.
non-holonomic robots and present experiments with our team of car-like robots with omnidirectional cameras and wireless network cards. II. P ROBLEM DEFINITION Consider a planar world, W = R2 , occupied by a group R = {R1 , R2 , . . . Rn } of n robots. The ith robot Ri is represented by the configuration qi in the configuration space C. A formation of n robots is represented by a directed graph called formation control graph1, Gf = (R, Ef ), and a second directed graph called constraint graph, Gc = (R, Ec ), where R is the set of nodes and Ef and Ec are edge sets. For the formation control graph, Gf , each edge eij = (Ri , Rj ) ∈ Ef is associated with a specification for Rj following Ri . For each edge, Ri is the leader and Rj is the follower. The robot that does not have any leaders and is responsible for guiding the others through the environment is called the lead robot [1]. Only one lead robot is allowed in our approach. Also, the robots that do not have any followers are called terminal followers. Figure 1(a) shows an example of a formation control graph where R3 is the lead robot and R2 , R4 and R5 are terminal followers. Robot R1 follows R3 and is followed by R2 and R4 . The edges eij = (Ri , Rj ) ∈ Ec of Gc are associated with constraints on relative position and orientation. While Ef describes leader-following relationships and set-points for the shape of the formation, Ec describes inequalities that reflect constraints such as communication and sensing constraints. Figure 1(b) shows an example of a constraint graph. In this figure R3 , for example, needs to maintain constraints with respect to R1 and R5 . The bidirectional edge between R2 and R4 indicates that these robots need to maintain constraints with each other. With the previous model, the control problem can be divided in two parts namely graph assignment and controller design. The first problem involves designing Gf and Gc and is not the main focus of this paper. Measures of performance that depend on Gf are discussed in [12] and heuristics for selecting edges are described in [13]. This paper is concerned with the problem of maintaining the formation described by Gf and the constraints described 1 The term control graph is used in [1], [2] to describe what we are calling a formation control graph.
by Gc . We assume that graphs themselves are preassigned and focus our attention on controlling the robots to satisfy the edge specifications. For Gf , the specification for each edge is a configuration for robot Rj with respect to its leader Ri . On the other hand, the specification for each edge in Gc is a convex function g(qi , qj ) that represents the allowable configuration space for Rj parameterized by the configuration of Ri . While Gf specifies, for each robot (except the lead), a unique point in configuration space, Gc specifies the allowable subset of configuration space. Although Gf and Gc are apparently independent, in order to allow robot Ri to reach its set-point qid (qj ) specified by Gf and still satisfy the constraints specified by Gc , we need to guarantee that, except for the lead robot, qid is inside the allowable configuration space Cid (q1 , . . . , qi−1 , qi+1 , . . . , qn ) defined by all constraints in Gc . Thus, the edge definition for the two graphs must satisfy the following condition: (q1d , q2d , . . . , qnd ) ∈ C1d × C2d × · · · × Cnd .
(1)
Moreover, since each is an intersection of convex sets, the right hand side of (1) is also a convex set. Therefore, if the robots are initially inside this set, they can always reach their goal configurations without going out of the set. Our goal in this paper is to design control laws that take in account the formation set-points and the allowable configuration spaces. Before continuing any further we will make three assumptions: Cid
Assumption 1 All robots are identical in terms of geometry, and in terms of capabilities and constraints related to sensing, communication, control, and mobility. Assumption 2 The robots are fully-actuated, holonomic, point robots. For the ith robot, the dynamical model is then given by: q˙i = ui , where qi = (xi , yi ). Assumption 3 Gf is acyclic and the in-degree at each node is 1. In other words, every follower has only one leader2 . III. P OTENTIAL F UNCTIONS In this paper, we use artificial potential fields to plan and control the robots’ motion. Potential field methods yield closed loop controllers that allow convergence to the goal in the presence of actuator and sensor noise and other disturbances [8]. Thus, for a potential function, φi , robot Ri ’s input is given by ui = −k∇φi (qi ) where ∇φi (qi ) is the gradient of φi computed in the configuration qi . The integral curves of the vector field formed by −∇φi (qi ), define implicity paths from every start configuration in C to the goal configuration qid . As pointed out in [11], a 2 This is somewhat restrictive since the in-degree for systems with two inputs can be up to two [1], [2].
potential function with a single minimum in qid can be thought of as a Lyapunov function for the system q˙i = ui (qi ), ui (qi ) = −∇φi (qi ), because φi (qi ) is positive definite and its value is, by definition, always decreasing along system trajectories. In the leader-following problem, we chose a navigation function [9] as a potential function for the lead robot. Navigation functions are constructed as the functions that solve the non-cooperative problem of steering a individual robot towards the goal while avoiding the static obstacles in the environment. The methodology presented in [9] can be directly applied for designing the function. While these functions can be very complicated, we limit the class of functions to quadratic functions in this paper. For the robots that have at least one leader, the potential function is constructed as a function of the leaders’ position. In the case that Ri follows Rj , we can describe the follower’s relative configuration in local coordinates as q¯ = (qj − qi ). We consider a quadratic Lyapunov function candidate of the form: 1 d φi (¯ q ) = k¯ q − q¯k2 . 2 If φi is a Lyapunov function we can use it as a leaderfollowing potential function. The input for the follower robot Ri is then given by the negative of the gradient of φi (qi , qj ) as: ui = −k∇φi = −k(¯ q d − q¯) , where k is a positive constant and ∇φi = ∂φi /∂qi . The derivative of the potential function for this input is given by: φ˙ i = −∇φi · q¯˙ = −∇φi · (q˙j − q˙i ) = −∇φi · (q˙j − ui ) = −∇φi · (q˙j + k∇φi ) = −kk∇φi k2 − ∇φi · q˙j . Observe that φ˙ i decreases along the system trajectory if: kk∇φi k2 > −∇φi · q˙j . In the worst case, q˙j and −∇φi are parallel and the previous condition can be written as the following sufficient condition: kk∇φi k > kq˙j k . (2) Because a real robot is subject to dynamics, there is a practical limit on its velocities. We assume each robot (i.e., all leaders) have a maximum velocity of q˙max . From Equation (2), it is clear that if we exclude the region given by the ball: q˙max k¯ q d − q¯k < γ = , k φ˙ i decreases along the trajectories of the system. Thus we can show that trajectories that start outside the ball (i.e., when k¯ q d − q¯k > γ), will converge to the ball. In other words, φ˙ i < 0 for k¯ q d − q¯k ≥ γ. The constant γ is the maximum allowable steady state error in q¯.
We note that it is possible to make γ arbitrarily small by allowing for feedforward control. If the follower input is given by: ui = −k∇φi + q˙j = −k(¯ q d − q¯) + q˙j , where q˙j is feedforward information, the controller exponentially converges to q¯ = 0. The feedforward velocity requires estimation of the leader’s velocity by the follower robot and is discussed elsewhere [2]. IV. C ONSTRAINTS As mentioned before, with each edge (Ri , Rj ) ∈ Ec , we associate a configuration constraint for Rj induced by Ri as a inequality of the form g(qi , qj ) ≤ 0. For example, if Rj must keep Ri in sight using a omnidirectional camera, g(qi , qj ) = (xi − xj )2 + (yi − yj )2 − r2 [6]. In this paper each constraint g(qi , qj ) defines three regions in the configuration spaces of Ri and Rj (see Figure 2). In the safe region, g(qi , qk ) < δ, where the small negative number δ can be thought of as a threshold. The region defined by δ ≤ g(qi , qk ) < 0 is the critical region for the robot. The constant δ is designed in order to guarantee that the constraint is still satisfied in the critical region and also to ensure that the robot does not leave this region. We say that a constraint is active when g(qi , qk ) ≥ δ. If g(qi , qk ) ≥ 0 the robot is in the unsafe region. Depending on the nature of the constraints, the robots may not be able to return to the safe region of the configuration space. Our decentralized controllers are designed with the objective of keeping the robots in the safe configuration space. V. C ONTROLLERS Our control system is decentralized and implemented using a set of three reactive controllers. Based on the two graphs, Gf and Gc , described in Section II we define a third graph that will govern the switching between the controllers. We call this time dependent graph that changes with the state of the robots, the formation graph H = (R, Eh ), where Eh is defined as the union of two subsets of Ef and Ec : Eh = E¯f ∪ E¯c , E¯f = {eij | [eij ∈ Ef ] ∧ [g(k, i) < 0 ∀eki ∈ Ec , Rk ∈ R]} , E¯c = {eij | [eij ∈ Ec ] ∧ [g(i, j) ≥ δ]} . Thus based on H, each robot Ri has three basic behaviors or modes depending on the number and type of incoming edges at Ri . If there is only one incoming control edge (eji ∈ Ef ), the robot is in the S AFE mode, corresponding to the safe region in Section IV. The control law in this mode is given by: ui = −k∇φi ,
(3)
where ∇φi is the gradient vector of the potential function φi , and k is a positive gain. For the lead robot it is
Critical δ
Safe
R3
Unsafe g(qi, qj) < 0
g(qi, qj)
0
R5
g(qi, qj) < δ
R1
R4
Fig. 2. The activation of the constraints define three regions in the robots’ configuration space.
a deliberative controller with a pre-planned navigation function that guides the robot toward the goal. For the followers it is a reactive controller designed to maintain the edge of the graph as shown in Section III. When all incoming edges are constraint edges (eji ∈ Ec ), Ri is in the U NSAFE mode. The robot tries to move in order to satisfy the constraints without using the potential function. In other words, the constraints themselves act as potential fields attracting the robots to each other and forcing them into a feasible configuration that satisfies all the constraints. The control input in this mode is: ui = −k ∇g j ,
(4)
where ∇g j is the gradient of the constraint g j (qi , qj ) defined by ∂g j /∂qi and g j is the constraint induced on Ri by Rj . If Ri has more than one active constraint to be satisfied, ∇g j represent the sum of their gradients. The third mode is a linear combination of the other two. A robot switches to this mode if it has one incoming control edge (eji ∈ Ef ) and at least one constraint edge (eji ∈ Ec ). The robot must navigate toward its goal while maintaining the constraint whose boundary it is closest to. This mode is called the C RITICAL mode. The input in the mode is: ui = −k(αi ∇g j + ∇φi ) , (5) where 0 < αi ≤ 1. The constant αi determines how much each robot will deviate to its main objective in order to preserve a constraint. It must be chosen so that: αi
