- Email: [email protected]
Modeling and Control of Dual Arm Robotic Manipulators using Decentralized Navigation Functions
10th IFAC Symposium on Robot Control International Federation of Automatic Control September 5-7, 2012. Dubrovnik, Croatia
Modeling and Control of Dual Arm Robotic Manipulators using Decentralized Navigation Functions Kim Vizins ∗ Dimos V. Dimarogonas ∗ Bo Wahlberg ∗ ∗ ACCESS Linnaeus Center, School of Electrical Engineering, KTH Royal Institute of Technology, SE-100 44,Stockholm, Sweden (e-mail: vizins,dimos,[email protected]).
Abstract: We apply multi-agent systems’ techniques to the control of dual arm robotic manipulators. The paper is divided into two separate subproblems: the first one considers decentralized navigation of a non-holonomic two-agent system with collision avoidance, corresponding to the end effectors of the robotic arms. For this problem, a controller that guarantees convergence to an arbitrarily small region around the desired destination is proposed. The second subproblem considers decentralized connectivity preserving control of a holonomic multi-agent system of fingers associated with the end effectors. Here, the goal is to account for the angle constraints that are necessary to describe the relationship between fingers connected to the same hand. For this case, both connectivity and convergence are shown with the proposed controller. 1. INTRODUCTION In this paper multi-agent systems’ tools are developed for the modeling and control of dual arm robotic manipulators. We consider a dual arm system where each arm has fingers associated with the end effectors, and associate one agent to each end effector and one agent to each fingertip, resulting in a multi-agent system. This system can then be divided into a two-agent system considering only the end effectors, and a multi-agent system considering the fingers. We consider two subproblems for the different systems described above. The first problem is design of a decentralized formation controller for two agents with collision avoidance. These agents correspond to the end effectors of the robotic arms. The movement of the end effectors is mechanically constrained and therefore they are considered as non-holonomic agents. Decentralized navigation of nonholonomic agents has been considered in Mastellone et al. [2008],Do and Lau [2011]. The controller is using a leader-follower approach and one of the tasks that it is able to perform is enclosing of an object. The decentralization is done by starting from an existing centralized controller and investigating the convergence and stability properties when it is modified for the decentralized case. Once convergence and stability has been shown in theory the controller is tested in simulations for confirmation. This controller is then generalized to include multiple agents. The second problem is to investigate vector connectivity constraints on multiple agents. These agents correspond to the fingers of the robotic arms. The finger tips are considered as holonomic agents since it is assumed that their movement can be described by a single integrator. To make sure that the agents fulfill the natural constraints on fingers, the connectivity preserving tools from Dimarog The second author is also affiliated with the Center of Autonomous Systems and is supported by the VR contract 2009-3948.
978-3-902823-11-3/12/$20.00 © 2012 IFAC
241
onas and Johansson [2010] are used. Since Dimarogonas and Johansson [2010] only handles distance constraints this approach is extended to include vector constraints to account for the angle relationship between fingers. Even though a general solution is preferable, the main consideration is to construct a controller that is adapted to the special problem of a hand with fingers associated with it. This controller is tested in simulations once convergence has been shown theoretically. The rest of the paper is organized as follows: section II includes some necessary background and described the problem in hand. Section III tackles the two agent nonholonomic system that models the hands, while section IV presents the connectivity maintenance control design for the fingers. Both sections are supported by relevant computer simulations. Finally, section V summarizes the contributions of this paper. 2. PROBLEM STATEMENT 2.1 Multi-agent non-holonomic system In a multi-agent non-holonomic system, the state vector for agent i in a plane is pi = [qiT θi ]T where qi = [xi yi ]T ∈ R2 denotes the agent’s spatial coordinates and θi denotes the agent’s orientation. The state space model is given by ⎡ ⎤ x˙ i ui cos θi q˙i (1) p˙ i = ˙ = ⎣ y˙ i ⎦ = ui sin θi θi ωi θ˙i Agent i is thus driven by the translational and angular velocities ui and ωi . The criterion for a decentralized controller is that ui and ωi cannot depend on the desired position for agent j, pdj for i = j. To make the implementation easier we also want the controller for agent i to depend only on the position of agent j, not its velocity. 10.3182/20120905-3-HR-2030.00082
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
The controllers are designed using navigation functions. They are based on the dipolar potential function ϕ in Loizou and Kyriakopoulos [2003],Tanner et al. [2003] where it is being used in centralized control. A decentralized version is found in Loizou et al. [2004] and is modified to be used here. The resulting navigation function ϕi for agent i is shown below. γi ϕi =
(2) 1/k k γi + Hnhi Gi where k > 1 is a positive tuning parameter. As stated in Loizou and Kyriakopoulos [2003], Hnhi has the form of a pseudo-obstacle Hnhi = nhi + hi (xi − xdi ) cos θdi + (yi − ydi ) sin θdi 2 where nhi and hi are positive tuning parameters and xdi , ydi and θdi denote the desired destination position and orientation. G i is the obstacle function 2 2 and is defined as Gi = − q − (r + r ) q i j i j j=i where ri is the radius of agent i. In this case, ri = 0 for agents, but if j represents an obstacle, rj > 0. Every agent j = i is treated as an obstacle by agent i, but obstacles can also be treated in this function by multiplying over j ∈ Ni where Ni is the set of all agents and obstacles that affect agent i. We assume that agents can sense the position of other agents and obstacles. Considering the two-agent system, it is a special case of the multi-agent system, with N = 2. That means that the state equations are given by (1) and the navigation function used is the one defined in (2), with i = 1, 2. In the case of the robotic arms, the controllers should drive the agents to a desired position and formation. To do this a leader-follower approach is used. This means that only one agent, the leader, needs information about the desired destination of the formation. Here, the leader is denoted agent 1 and its controller is based on desired formation but also on desired position for the formation. The follower, agent 2, is only controlled to keep formation, and the formation constraints are designed such that agent 2 converges to its desired position as agent 1 converges. The goal functions are given by γ1 = p1 − pd1 2λ + Kc p1 − p2 − c12 2λ and γ2 = p2 − p1 − c21 2λ where Kc > 0 is a tuning parameter and c12 (q) = −c21 (q) defines the formation constraints. For agent 2, the destination is given by pd2 = p1 + c21 . Note that pd2 will, in contrast to pd1 , change during the control scenario, as the leader moves. Note also that if there is a desired path for the agents to take, c12 can vary with the configuration q of the agents to adapt to that path. Thus c12 changes over time if the agents’ positions or directions change. The norm that depends on both position and angle is here denoted as p λ and, like in Tanner et al. [2003], is defined as pλ = x2 + y 2 + λθ2 where λ is a positive tuning parameter. 2.2 Multi-agent holonomic system In this case, the dynamics of the system are modeled as single integrators. The state space model is thus defined as q˙i = ui (3) where ui is the translational velocity vector. 242
The controller for agent i is thus given by the velocity ui . As stated above, ui cannot depend on the desired position for agent j, qdj for i = j. The type of navigation function used in the case of the holonomic fingers is based on the one in Dimarogonas and Johansson [2010]. This navigation function is given by γi ϕi =
(4) 1/k k γi + Gi where k > 1 is a positive tuning parameter. The goal2 function γi is defined as γi = j∈Ni (qi − qj − cij ) Dimarogonas and Johansson [2010]. Ni denotes the neighboring set of i, defined as all agents j within distance di from i. The function Gi used in this paper is based on Gi in Dimarogonas and Johansson [2010] and takes the connectivity constraints into account. The modification is shown in section 4.1. The agents considered here are fingers connected to a hand. The design of the connectivity constraints is specific to this case. The hand is denoted as agent 0 and has position and orientation, in two dimensions p0 = [q0T θ0 ]T . The fingers are denoted as agent i, i = 1, . . . N where N is the number of fingers. Each finger has a position and an angle associated with them. This angle is derived from the finger’s relative position to the center of the hand and is used to define the allowed area for that finger. The state vector for agent i is thus pi = [qiT θi ]T where θi = atan2 (yi − y0 , xi − x0 ) − θni
(5)
and θni defines the center of the allowed circle sector in relation to the hand. It is also possible to define the allowed area based on other fingers, but a central agent is still needed to define the angles. The definition of the angle for agent i is then θi = atan2 (yi − y0 , xi − x0 )
(6)
The allowed area is different in this case, and also depend on if the constraints on the angle consider only the absolute value (as in the previous case) or the sign of the angle. 3. DECENTRALIZED CONTROL FOR TWO-AGENT SYSTEMS From here on, we will use the abbreviation ϕixj =
∂ϕi ∂xj .
3.1 Control strategy Considering system (1) and decentralizing the centralized controller in Loizou and Kyriakopoulos [2003], the following law is proposed.
ui = −sgn ϕixi cos θi + ϕiyi sin θi Zi (7a) Kω (θnhi − θi ) , Δi < 0 (7b) ωi = −Kω ϕiθi , Δi ≥ 0 where Δi = Kω ϕiθi (θnhi − θi ) − ϕixi cos θi + ϕiyi sin θi Zi , Zi = Ku ((ϕixi )2 + (ϕiyi )2 ) + Kz ((xi − xdi )2 + (yi − ydi )2 ),
θnhi = atan2 sgn ((xi − xdi ) cos θdi + (yi − ydi ) sin θdi ) ϕiyi , sgn ((xi − xdi ) cos θdi + (yi − ydi ) sin θdi ) ϕixi , and sgn (x) = 1 if x ≥ 0 and sgn (x) = −1 if x < 0.
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
2
2
i 2 i 2 ϕxi + ϕyi ≤ τ1 for ϕixi ≤ μ1 ∧ ϕiyi ≤ μ2 ∧ μ1 + μ2 ≤ τ1 Let V = √
i 2 i ϕi = ϕ1 + ϕ2 with the ϕi defined in (2). ϕxi ≤ μ1 if ϕixi ≤ μ1 Its time derivative is V˙ = u1 (Vx1 cos θ1 + Vy1 sin θ1 ) + d u2 (Vx2 cos θ2 + Vy2 sin θ2 ) + +ω1 Vθ1 + ω2 Vθ2 . γ di H G − γdi H i G +H Gi γxii Hi Gi i ( ) i i i i xi x x k i i Theorem 3.1. The control (7) drives (1) to an arbitrarily
k 1/k+1 ϕxi = + ≤ 2 γmin γd +Hi Gi small region around the destination configuration. i γdi (|Hxi |Gi +Hi |Gix |) √ i i ≤ μ1 for 2 Proof: Denoting Qi = (ϕixi )2 + (ϕiyi )2 , Sii = ϕixi cos θi + kγmin i i √ 2 μ1 γmin ϕiyi sin θi , Sji = ϕjxi cos θi +ϕjyi sin θi , Ai = |θnhi − (θi − θdi )|, k ≥ 2 max{γdi (|√Hxi |Gi +Hi |Gxi |)} ∧ H ≤ di . 2 i μ γ 1 min 2 max γxi Gi Tii = ϕiθi , Tji = ϕjθi , it can be shown after some calculations that The bounded workspace defined above ensures that these bounds are finite and not equal to zero. Equivalent bounds
2 V˙ ≤ − [ (1 + Zk sgn (Skk ) Sjk − are found on k and Hi for ϕiyi ≤ μ2 . Since the i:Δi ≤0 k:|Δk |>1 j=k workspace is bounded, there exists a τ2 for which (xk − xdk )2 + (yk − ydk )2 ≤ τ2 . −Kω (θnhk − θk ) Tjk ) + j=k | = ϕjxi cos θi + ϕjyi sin θi ≤ ϕjxi + ϕjyi ≤ σ3 for |S jk ϕj ≤ τ3 ∧ ϕj ≤ τ4 ∧ τ3 + τ4 ≤ σ3 + (Zk sgn (Skk ) Sjk − xi yi k:Δk ≤1 j=k dj j γxdij Hj Gj − γdkj Hj Gjx γxi Hj Gj γdj Hj |Gjx | i ϕ = Tjk )] − −Kω (θnhk − θk ) 1/k+1 ≤ γ 2 + kγ 2 i ≤ τ3 xi min min k γdj +Hj Gj j=k 2 2 maxj=i {γdj Hj |Gjxi |} τ3 γmin − [ (2 + Zk sgn (Skk ) Sjk ) + . for k ≥ ∧ Hj ≤ 2 τ γ d 3 min i:Δi >0 k:Zk |Skk |>2 j=k 2 maxj=i γxij Gj + Zk sgn (Skk ) Sjk + The bounded workspace defined above ensures that these j=k k:Zk |Skk |≤2 bounds are finite and not equal j to zero. Equivalent bounds ϕy ≤ τ4 . are found on k and H for j (3 + Tkk Tjk ) + +Kω ( i 3.2 Analysis
k:|Tkk |2 >3
+
2
|Tkk | Tkk
k:|Tkk | ≤3
j=k
Tjk )]
j=k
(8) Given a bounded workspace with −π < θ ≤ π, and assuming that γdi > γmin > 0 and k > 1, these terms can be bounded as follows. 1) For the first term in the first sum of the right hand side of (8) 1 + Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≥ ρ1 , 0 < ρ1 < 1 for |Zk sgn (Skk )
j=k
Sjk − Kω (θnhk − θk )
Tjk | ≤ 1 − ρ(9) 1
j=k
We have Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≤ Zk (N − 1) maxj=k (|Sjk |) + Kω (N − 1)π maxj=k (|Tjk |) which makes (9) equivalent to Zk maxj=k |Sjk | + 1 −ρ1 Kω π maxj=k |Tjk | ≤ N ≡ σ1 . This is true for −1 Zk maxj=k |Sjk | ≤ σ2 σ3 ∧ Kω π maxj=k |Tjk | ≤ σ4 ∧ σ2 σ3 + σ4 ≤ σ1 . a) Zk maxj=k |Sjk | ≤ σ2 σ3 for Zk ≤ σ2 ∧ |Sjk | ≤ σ3 ∀j = k. Zk = Ku ((ϕkxk )2 + (ϕkyk )2 ) + Kz ((xk − xdk )2 + (yk − ydk )2 ) ≤ σ2 for (ϕkxk )2 +(ϕkyk )2 ≤ τ1 ∧ (xk −xdk )2 +(yk −ydk )2 ≤ τ2 Ku τ1 + Kz τ2 ≤ σ2
∧
243
b) Kω π maxj=k |Tjk | ≤ σ4 for |Tjk | ≤ Kσω4π ≡ τ5 ∀j = k. γ dj H G dj max θi j j γθ Hj Gj j i 1/k+1 ≤ |Tjk | = ϕθi = ≤ 2 γmin γdkj +Hj Gj τ5 for Hj ≤
2
τ5 γmin . d maxj=i γθ j Hj Gj i
2) For the second term in thefirst sum of the right hand side of (8) Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≥ −ρ2 < 0 for |Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk | ≤ ρ2 which is equivalent to (1). 3) For the first term in the second sum of the right hand side of (8) 2 + Zk sgn (Skk ) j=k Sjk ≥ ρ3 , 0 < ρ3 < 2 2 −ρ3 for Zk maxj=k |Sjk | ≤ N −1 , equivalent to(1a). 4) For the second term in the second sum of the right hand side of (8) Zk sgn (Skk ) j=k Sjk ≥ −ρ4 , ρ4 > 0 for 4 , equivalent to step (1a). Zk maxj=k |Sjk | ≤ Nρ−1 5) For the third term in the second sum of the right hand side of (8) 3 + Tkk j=k Tjk ≥ ρ5 , 0 < ρ5 < 3 for 3 −ρ5 |Tkk | maxj=k |Tjk | ≤ N −1 ≡ κ1 . |Tkk | maxj=k |Tjk | ≤ κ1 for |Tkk | ≤ κ2 ∧ |Tjk | ≤ κ3 ∀j = k ∧ κ2 κ3 ≤ κ1 . |Tkk | ≤ κ2 is equivalent to |Tjk | ≤ κ3 which is equivalent to step (1b). 6) For the fourth termin the second sum of the right hand side of (8) Tkk j=k Tjk ≥ −ρ6 , ρ6 > 0 for 6 |Tkk | maxj=k |Tjk | ≤ Nρ−1 which is equivalent to step (5).
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
Assuming that at least one agent i fulfills Zi |Sii | > 2 or 2 |Tii | > 3 , then there exists an 1 such that |Δi | > 1 . Assuming also that all the bounds found in steps (1)-(6) are fulfilled gives the two cases
0.9
0.7 0.6
y
0.5 0.4 0.3 0.2 0.1
1) Δi ≤ 0 ⇒ V˙ ≤ −ρ1 + (N − 1)2 ρ2 < 0 for 0 < (N − 1)2 ρ2 < ρ1 < 1 2) Δi > 0 ⇒ V˙ ≤ −ρ3 + (N − 1)2 ρ4 + Kω (−ρ5 + (N − 1)2 ρ6 ) < 0 for 0 < (N − 1)2 ρ4 < ρ3 < 2 ∧ 0 < (N − 1)2 ρ6 < ρ5 < 3
Agent 1 − leader Agent 2 − follower
0.8
0 −0.1 −1
−0.8
−0.6
−0.4
−0.2
0 x
0.2
0.4
0.6
0.8
1
Fig. 2. Collision avoidance of agent 2 for control (7) 4. CONNECTIVITY CONSTRAINED FORMATION CONTROL FOR MULTI-AGENT SYSTEMS
2
This implies that Zi |Sii | and |Tii | converge to arbitrarily small values 2 and 3 , for all i. However, it was shown in de Wit and Sordalen [1992] that |Sii | = 0 is non-invariant when the system does not fulfill ϕixi = ϕiyi = 0. This means that the system converges to an arbitrarily small region of ϕixi = ϕiyi = 0, and ϕiθi = 0. Using similar arguments as in Loizou et al. [2004], we know that points of this form not coinciding with the desired destination have attractors of measure zero for sufficiently large k. Thus, ϕixi = ϕiyi = ϕiθi = 0 only for xi = xdi , yi = ydi and θi = θdi , i.e., the desired destination configuration. Thus the system trajectories converge to the arbitrarily small region around the desired destination configuration.
3.3 Simulations The developed control strategies are next simulated for the two-agent case. In all plots, red arrows correspond to the leader-agent (agent 1) and green arrows to the followeragent (agent 2). Initial and final positions are shown with blue arrows and obstacles as blue circles. All agents are considered as points in this article so the arrows merely show the orientation of the agents and not area. The position of the point agent is shown by the tip of the arrow. The objective is to enclose the obstacle that is located at the origin. Therefore, the destination configuration for agent 1, pd1 = [r 0 − π]T where r is the obstacle’s radius. The desired formation is to have agent 2 mirror agent 1 around the y-axis. Therefore, the formation vector is defined as c21 = [−2x1 0 −(2θ1 +π)]T which makes the destination configuration for agent 2, pd2 = [−x1 y1 − (θ1 + π)]T . Figure 1 shows convergence of two agents to their respective destinations. In Figure 2, obstacles are included to illustrate the collision avoidance. In all cases the tuning parameters used were k = 80, h = 1, λ = 0.01, Ku = 10, Kω = 100 and Kz = 50 and the initial positions were p1 = [0.8 0.8 − π/2]T and p2 = [−0.8 0.8 − π/2]T .
4.1 Control strategy The proposed extension of Gi from Dimarogonas and Johansson [2010] is given by Gi =
j∈Ni
=
βij =
3
βijk =
j∈Ni k=1
(d2maxi − qi − qj 2 )(qi − qj 2 − d2mini ) ·
j∈Ni
·(d2θi − |θi − θj |2 )
(10)
βij1 corresponds to the connectivity and was given in the original constraint function. βij2 corresponds to the constraint that the agent i is not allowed to come closer to agent j than the distance dmini and is implemented as collision avoidance. βij3 handles the connectivity constraints on the angle. If the desired case is the second one, this factor has to be replaced with βij3 = dθi − (θi − θj ). The control strategy is given by ui = −Kϕiqi
(11)
where K > 0 is a tuning parameter, ϕi is defined by (4) with Gi changed to the extended version given in (10). In the general case, the set of neighboring agents can be predefined, or based on distance, depending on the application. For the finger case, the constraints on each finger is defined using the relative position to the hand. Assuming that the fingers are stiffly attached to the hand, their relative positions will not change given a motion in p0 . Thus for all agents i = 0, Ni = {0} and a controller ui0 can be defined using the relative position qi0 = qi − q0 such that qi0 ˙ = ui0 . (12) ui0 = −Kϕiqi0 It can easily be seen that for Ni = {0} these two controllers are equal since ui already uses only relative position. 4.2 Analysis
0.9 Agent 1 − leader Agent 2 − follower
0.8 0.7 0.6
y
0.5 0.4 0.3 0.2 0.1 0 −0.1 −1
−0.8
−0.6
−0.4
−0.2
0 x
0.2
0.4
0.6
0.8
1
Fig. 1. Two agents with control (7) converging with the right orientation
244
1)Connectivity preservation If it can be shown that the set Q = {q ∈ R2N | Gi (q) > 0 ∀i ∈ N } is invariant for the closed-loop system given by (3) and the controller (11), then the connectivity preservation has been proven. This is investigated by following the proof in Dimarogonas and Johansson [2010] but using the extended definition of Gi that was given in (10). Theorem 4.1. The set Q = {q ∈ R2N | Gi (q) > 0 ∀i ∈ N } is invariant for the closed-loop system (3) a, (11).
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
2 −K ϕiqi < 0 ∀ qi = qdi with qi0 the relative position that was defined in section 4.1. It also follows from (11) that the only invariant set where V˙ = 0 is the desired destination qdi . LaSalle’s invariance principle then gives that the system converges to the desired destination. γi (q0 ) k 0 −1/k−1 0 ((γi (q ))) (− k ∇i Gi (q )). ∇i Gi is computed as Theorem 4.3. The control (11) drives (3) with θi defined ∇i Gi = j∈Ni β ij ∇i βij where β ij = l∈Ni βil and ∇i βij = in (6) to an arbitrarily small region around the target. l=j 3 β ∇ β where β = β and i ijk ijm m∈{1,2,3} ijk k=1 ijk Proof: The constraints and destination of an agent i are m=k ∇i βijk = −(qi − qj ), k = 1, ∇i βijk = (qi − qj ), k = 2 defined by the relative position to other agents j ∈ Ni , 0∈ / Ni . The time derivative of the Lyapunov candidate is and ∇i βijk = −(θi − θj )∇i θi , k = 3.
2 ˙ = V ϕ˙ i = i [ϕiqi q˙i + j∈Ni ϕiqj q˙j ] = i [−K ϕiqi − i 0 0 Since Gi (q ) = 0 then βij (q ) = 0 for at least one i j j∈Ni Kϕqj ϕqj ]. Like in previous sections it can be shown j ∈ Ni . Assuming βij (q 0 ) = 0 and βil (q 0 ) = 0 for all
i 2 < l ∈ Ni , l = j, then β ij (q 0 ) > 0 and β il (q 0 ) = 0. Since that all agents i converge to regions where ϕqi and > 0 is arbitrarily small. To do this, the notation 0 0 βij (q ) = 0 then βijk (q ) = 0 for at least one k ∈ {1, 2, 3}. i ˙ ˙ Assuming βijk (q 0 ) = 0 for only one k ∈ {1, 2, 3} gives Pij != ϕqj is used and V can then be written as V = 2 sgn (∇i Gi ) = sgn (∇i βijk ). This gives three cases: −K 2 ≥ (Pii + i:Pii j∈Ni Pij Pjj )+ " 0 2 1) βij1 (q ) = 0. This happens when qi − qj = dmax 2 < (Pii + i:Pii j∈Ni Pij Pjj ) ≤ ! which means that qi − qj is on the limit of becoming ≤ −K too large. The direction of the controller then becomes 2 ≥ ( + i:Pii j∈Ni Pij Pjj )+
" sgn −ϕiqi (q 0 ) = sgn ∇i Gi (q 0 ) = sgn ∇i βij1 (q 0 ) = ( P P ) . It can be shown that given a 2 ij jj i:Pii < j∈Ni sgn (−(qi − qj )), meaning that the controller points in the direction that decreases qi − qj , i.e., towards the set Q. bounded workspace, −π < θ ≤ π, γi > γmin > 0 and k > 1 the terms can be bounded as: 2) βij2 (q 0 ) = 0. This happens when qi − qj = dmin which means that qi − qj is on the limit of becoming 1) + j∈Ni Pij Pjj ≥ ρ1 > 0 for maxj∈Ni |Pij Pjj | ≤ −ρ1 too small. The direction
of the controller then becomes Ni ≡ σ1 . |Pij Pjj | ≤ σ1 for |Pij | ≤ σ2 ∧ |Pjj | ≤ σ3 ∧ i 0 0 0 sgn −ϕqi (q ) = sgn ∇i Gi (q ) = sgn ∇i βij2 (q ) = β ij (βij |qi − qj | 2 sgn (qi − qj ), which means that the controller points in the σ2 σ3 ≤ σ1 . First we have |Pij | ≤ γmin direction that increases qi − qj , that is towards the set + γki (βij2 βij3 |qi − qj | + βij1 βij3 |qi − qj | Q. + βij1 βij2 |(θi − θj )(−∇j θj )|)) ≤ σ2 for k ≥ max(γi ((βij2 βij3 + βij1 βij3 ) |qi − qj | 0 3) βij3 (q ) = 0. This happens when |θi − θj | = dθ j∈Ni 2 which means that |θi − θj | is on the limit of becom- +β β |(θ − θ )(−∇ θ )|))/ min ( σ2 γmin −βij |qi − qj |). ij1 ij2 i j j j β j∈Ni ing too large. The direction can be ij
i 0 of the controller
calculated to be sgn −ϕqi (q ) = sgn ∇i Gi (q 0 ) = ⎛ ⎡ Since k #> 0, this also defines a lower bound on γmin : ⎤⎞ −(yi − yj ) $ max(βij βij |qi −qj |) max(Gi |qi −qj |) ⎜ ⎢ (xi − xj )2 + (yi − yj )2 ⎥⎟ j∈N j∈Ni i ⎢ ⎥ ⎟ γ > = . −(θ which means sgn ⎜ − θ ) min i j ⎣ σ2 σ2 ⎝ ⎦⎠ (xi − xj ) γ (xi − xj )2 + (yi − yj )2 Then, |Pjj | = (γjk + Gj )−1/k−1 Gj ∇j γj − kj ∇j Gj ≤ that the controller points in the direction that decreases 1 2 l∈Nj β jl (βjl |qj − ql | + |θi − θj |, that is towards the set Q. Thus, for all these γmin γj three cases, Q is invariant. If the assumptions above do not ((β jl2 βjl3 +βjl1 βjl3 ) |qj − ql |+βjl1 βjl2 |(θj − θl )(−∇j θj )| k hold, and βijk (q 0 ) = 0 for more than one k ∈ {1, 2, 3} or ≤ σ3 for k ≥ max(γj ((βjl2 βjl3 + l∈Nj if βij (q 0 ) = 0 for more than one j ∈ Ni , then ∇i Gi (q 0 ) = βjl1 βjl3 ) |qj − ql | + βjl1 βjl2 |(θj − θl )(−∇j θj )|))/ i 0 0 0 k 0 0 ⇒ ϕqi (q ) = 0. However, ϕi (q ) = γi (q )/(γi (q ) + σ γ2 − βjl |qj − ql |). min ( N3 min Gi (q 0 ))1/k = 1 and since ϕi : R2N → [0, 1] this means l∈N j β jl j 0 that ϕi reaches its maximum at q . Following the negated gradient of ϕi , the maxima can not be reached from a set of Like for#|Pij | this also gives a lower bound on γmin : $ open initial conditions Dimarogonas and Johansson [2010]. max(βjl βjl |qj −ql |) max(Gj |qj −ql |) l∈Nj The conclusion is then that if the set of initial conditions γmin > l∈Nj = . σ3 σ3 is open, Q is invariant also for this case. The limitations on the workspace ensures that the bounds 2)Formation convergence presented above are finite and greater than zero. Theorem 4.2. The control law (12) drives (3) with θi ρ2 2) j∈Ni Pij Pjj ≥ −ρ2 < 0 if maxj∈Ni |Pij Pjj | ≤ N ≡ defined in (5) to the desired configuration. i σ4 . This leads to lower bounds on k and γmin that are Proof: Consider V = i ϕi . For the case where the equivalent to the ones given in step (1). constraints and destination point for a finger are defined by the relative position to the hand, the time derivative of Assuming that the bounds derived in steps (1) and (2) 2
V becomes V˙ = i ϕ˙ i = i ϕiqi0 q˙i0 = i −K ϕiqi0 = are fulfilled and that there exists at least one agent i Proof: Denote N = {1, . . . , N } and consider i ∈ N and a point q 0 such that Gi (q 0 ) = 0. With the no∂f the derivative of the navigation tation ∇i f = ∂q i function in this point can be written as ϕiqi (q 0 ) =
245
i
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
4.3 Simulations
0.02
0.015
0.01
y
such that Pii2 ≥ the resulting bound on V˙ becomes V˙ ≤ −K(ρ1 − (N − 1)2 ρ2 ) < 0 for 0 < (N − 1)2 ρ2 < ρ1 . This implies that Pii converge to arbitrarily small for all i. Using similar arguments as in Dimarogonas and Frazzoli [2010] we know that that critical points of ϕi not coinciding with the desired destination have attractors of measure zero for sufficiently large k. Thus the system trajectories converge to the arbitrarily small region around the desired destination configuration.
0.005
0
−0.005
−0.01 −0.02
−0.015
−0.01
−0.005
0 x
0.005
0.01
0.015
0.02
Fig. 5. Simulation showing that the constraint on the maximum angle is fulfilled for all fingers 5. CONCLUSIONS
Simulations of the controller (12) for fingers is shown in two dimensions. We consider the allowed area of an agent to only be defined by its relation to the hand. It is a segment of a circle sector, defined by its minimum and maximum radius and by the maximum angle span. It is illustrated by blue lines in the simulation plots. The agents are shown as colored circles, and the hand is denoted by a blue arrow with the tip in the hand’s position and pointing in the direction of the orientation of the hand. Three simulations are presented that show connectedness in the limits corresponding to each of the three constraints. First, Figure 3 shows that each finger stays a distance dmin = 0.01 from the hand. The starting positions, shown as blue circles, and the desired destinations, shown as blue x’s, were chosen such that if there were no constraints the agents would follow a straight line and thereby enter the forbidden area where their distance to the hand would be less than dmin , see Figure 3. As can be seen, the agents 0.02
0.015
y
0.01
0.005
0
−0.005
−0.01 −0.02
−0.015
−0.01
−0.005
0 x
0.005
0.01
0.015
0.02
Fig. 3. Simulation showing that the constraint on the minimum distance is fulfilled for all fingers do not enter this forbidden area, their trajectories follow the arch shaped boundary. For the other two constraints, the desired destination is placed outside the allowed area, thus violating the assumption made in section 2.2. The fingers then stop when they have reached the limit of the active constraint and do not converge to their destinations. The constraints on maximum distance dmax = 0.02 and maximum angle dθ = 15◦ are shown in Figure 4 and Figure 5 respectively. 0.025
0.02
0.015
y
0.01
0.005
0
−0.005
−0.01
−0.02
−0.01
0 x
0.01
0.02
Fig. 4. Simulation showing that the constraint on the maximum distance is fulfilled for all fingers
246
We extended centralized controllers for non-holonomic multi-agent formation navigation to the decentralized case. For simplicity the modification was made for the two agent case, but the proof showed that the new strategy was valid for multiple agents. The result was supported by simulations. The second task was to investigate if connectivity constraints on a multi-agent system could be extended to include angle connectivity. This was shown to be possible for a system with a central unit from which the angles associated with the agents could be defined. For our case, where the agents represent fingers, this central unit was the hand. Note that the central unit is only used for definition of the angles and that all the agents still have their own controller. Connectivity and convergence for this strategy was shown to hold in theory and could also be visualized in simulations. REFERENCES de Wit, C.C. and Sordalen, O.J. (1992). Exponential stabilization of mobile robots with nonholonomic constraints. IEEE Transactions on Automatic Control, 37(11), 1791– 1797. Dimarogonas, D. and Frazzoli, E. (2010). Analysis of decentralized potential field based multi-agent navigation via primal-dual lyapunov theory. In IEEE Conference on Decision Control, 1215–1220. Dimarogonas, D. and Johansson, K. (2010). Bounded control of network connectivity in multi-agent systems. IET Control Theory and Applications, 4(8), 1330–1338. Do, K. and Lau, M. (2011). Practical formation control of multiple unicycle-type mobile robots with limited sensing ranges. Journal of Intelligent and Robotic Systems. To appear. Loizou, S., Dimarogonas, D., and Kyriakopoulos, K. (2004). Decentralized feedback stabilization of multiple nonholonomic agents. In IEEE International Conference on Robotics and Automation, 3012–3017. Loizou, S. and Kyriakopoulos, K. (2003). Closed Loop Navigation for Multiple Non-Holonomic Vehicles. In IEEE International Conference on Robotics and Automation, 420–425. Mastellone, S., Stipanovic, D., Graunke, C., Intlekofer, K., and Spong, M. (2008). Formation control and collision avoidance for multi-agent non-holonomic systems: Theory and experiments. International Journal of Robotics Research, 27(1), 107–126. Tanner, H.G., Loizou, S.G., and Kyriakopoulos, K.J. (2003). Nonholonomic navigation and control of cooperating mobile manipulators. IEEE Trans. on Robotics and Automation, 19(1), 53–64.
Modeling and Control of Dual Arm Robotic Manipulators using Decentralized Navigation Functions Kim Vizins ∗ Dimos V. Dimarogonas ∗ Bo Wahlberg ∗ ∗ ACCESS Linnaeus Center, School of Electrical Engineering, KTH Royal Institute of Technology, SE-100 44,Stockholm, Sweden (e-mail: vizins,dimos,[email protected]).
Abstract: We apply multi-agent systems’ techniques to the control of dual arm robotic manipulators. The paper is divided into two separate subproblems: the first one considers decentralized navigation of a non-holonomic two-agent system with collision avoidance, corresponding to the end effectors of the robotic arms. For this problem, a controller that guarantees convergence to an arbitrarily small region around the desired destination is proposed. The second subproblem considers decentralized connectivity preserving control of a holonomic multi-agent system of fingers associated with the end effectors. Here, the goal is to account for the angle constraints that are necessary to describe the relationship between fingers connected to the same hand. For this case, both connectivity and convergence are shown with the proposed controller. 1. INTRODUCTION In this paper multi-agent systems’ tools are developed for the modeling and control of dual arm robotic manipulators. We consider a dual arm system where each arm has fingers associated with the end effectors, and associate one agent to each end effector and one agent to each fingertip, resulting in a multi-agent system. This system can then be divided into a two-agent system considering only the end effectors, and a multi-agent system considering the fingers. We consider two subproblems for the different systems described above. The first problem is design of a decentralized formation controller for two agents with collision avoidance. These agents correspond to the end effectors of the robotic arms. The movement of the end effectors is mechanically constrained and therefore they are considered as non-holonomic agents. Decentralized navigation of nonholonomic agents has been considered in Mastellone et al. [2008],Do and Lau [2011]. The controller is using a leader-follower approach and one of the tasks that it is able to perform is enclosing of an object. The decentralization is done by starting from an existing centralized controller and investigating the convergence and stability properties when it is modified for the decentralized case. Once convergence and stability has been shown in theory the controller is tested in simulations for confirmation. This controller is then generalized to include multiple agents. The second problem is to investigate vector connectivity constraints on multiple agents. These agents correspond to the fingers of the robotic arms. The finger tips are considered as holonomic agents since it is assumed that their movement can be described by a single integrator. To make sure that the agents fulfill the natural constraints on fingers, the connectivity preserving tools from Dimarog The second author is also affiliated with the Center of Autonomous Systems and is supported by the VR contract 2009-3948.
978-3-902823-11-3/12/$20.00 © 2012 IFAC
241
onas and Johansson [2010] are used. Since Dimarogonas and Johansson [2010] only handles distance constraints this approach is extended to include vector constraints to account for the angle relationship between fingers. Even though a general solution is preferable, the main consideration is to construct a controller that is adapted to the special problem of a hand with fingers associated with it. This controller is tested in simulations once convergence has been shown theoretically. The rest of the paper is organized as follows: section II includes some necessary background and described the problem in hand. Section III tackles the two agent nonholonomic system that models the hands, while section IV presents the connectivity maintenance control design for the fingers. Both sections are supported by relevant computer simulations. Finally, section V summarizes the contributions of this paper. 2. PROBLEM STATEMENT 2.1 Multi-agent non-holonomic system In a multi-agent non-holonomic system, the state vector for agent i in a plane is pi = [qiT θi ]T where qi = [xi yi ]T ∈ R2 denotes the agent’s spatial coordinates and θi denotes the agent’s orientation. The state space model is given by ⎡ ⎤ x˙ i ui cos θi q˙i (1) p˙ i = ˙ = ⎣ y˙ i ⎦ = ui sin θi θi ωi θ˙i Agent i is thus driven by the translational and angular velocities ui and ωi . The criterion for a decentralized controller is that ui and ωi cannot depend on the desired position for agent j, pdj for i = j. To make the implementation easier we also want the controller for agent i to depend only on the position of agent j, not its velocity. 10.3182/20120905-3-HR-2030.00082
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
The controllers are designed using navigation functions. They are based on the dipolar potential function ϕ in Loizou and Kyriakopoulos [2003],Tanner et al. [2003] where it is being used in centralized control. A decentralized version is found in Loizou et al. [2004] and is modified to be used here. The resulting navigation function ϕi for agent i is shown below. γi ϕi =
(2) 1/k k γi + Hnhi Gi where k > 1 is a positive tuning parameter. As stated in Loizou and Kyriakopoulos [2003], Hnhi has the form of a pseudo-obstacle Hnhi = nhi + hi (xi − xdi ) cos θdi + (yi − ydi ) sin θdi 2 where nhi and hi are positive tuning parameters and xdi , ydi and θdi denote the desired destination position and orientation. G i is the obstacle function 2 2 and is defined as Gi = − q − (r + r ) q i j i j j=i where ri is the radius of agent i. In this case, ri = 0 for agents, but if j represents an obstacle, rj > 0. Every agent j = i is treated as an obstacle by agent i, but obstacles can also be treated in this function by multiplying over j ∈ Ni where Ni is the set of all agents and obstacles that affect agent i. We assume that agents can sense the position of other agents and obstacles. Considering the two-agent system, it is a special case of the multi-agent system, with N = 2. That means that the state equations are given by (1) and the navigation function used is the one defined in (2), with i = 1, 2. In the case of the robotic arms, the controllers should drive the agents to a desired position and formation. To do this a leader-follower approach is used. This means that only one agent, the leader, needs information about the desired destination of the formation. Here, the leader is denoted agent 1 and its controller is based on desired formation but also on desired position for the formation. The follower, agent 2, is only controlled to keep formation, and the formation constraints are designed such that agent 2 converges to its desired position as agent 1 converges. The goal functions are given by γ1 = p1 − pd1 2λ + Kc p1 − p2 − c12 2λ and γ2 = p2 − p1 − c21 2λ where Kc > 0 is a tuning parameter and c12 (q) = −c21 (q) defines the formation constraints. For agent 2, the destination is given by pd2 = p1 + c21 . Note that pd2 will, in contrast to pd1 , change during the control scenario, as the leader moves. Note also that if there is a desired path for the agents to take, c12 can vary with the configuration q of the agents to adapt to that path. Thus c12 changes over time if the agents’ positions or directions change. The norm that depends on both position and angle is here denoted as p λ and, like in Tanner et al. [2003], is defined as pλ = x2 + y 2 + λθ2 where λ is a positive tuning parameter. 2.2 Multi-agent holonomic system In this case, the dynamics of the system are modeled as single integrators. The state space model is thus defined as q˙i = ui (3) where ui is the translational velocity vector. 242
The controller for agent i is thus given by the velocity ui . As stated above, ui cannot depend on the desired position for agent j, qdj for i = j. The type of navigation function used in the case of the holonomic fingers is based on the one in Dimarogonas and Johansson [2010]. This navigation function is given by γi ϕi =
(4) 1/k k γi + Gi where k > 1 is a positive tuning parameter. The goal2 function γi is defined as γi = j∈Ni (qi − qj − cij ) Dimarogonas and Johansson [2010]. Ni denotes the neighboring set of i, defined as all agents j within distance di from i. The function Gi used in this paper is based on Gi in Dimarogonas and Johansson [2010] and takes the connectivity constraints into account. The modification is shown in section 4.1. The agents considered here are fingers connected to a hand. The design of the connectivity constraints is specific to this case. The hand is denoted as agent 0 and has position and orientation, in two dimensions p0 = [q0T θ0 ]T . The fingers are denoted as agent i, i = 1, . . . N where N is the number of fingers. Each finger has a position and an angle associated with them. This angle is derived from the finger’s relative position to the center of the hand and is used to define the allowed area for that finger. The state vector for agent i is thus pi = [qiT θi ]T where θi = atan2 (yi − y0 , xi − x0 ) − θni
(5)
and θni defines the center of the allowed circle sector in relation to the hand. It is also possible to define the allowed area based on other fingers, but a central agent is still needed to define the angles. The definition of the angle for agent i is then θi = atan2 (yi − y0 , xi − x0 )
(6)
The allowed area is different in this case, and also depend on if the constraints on the angle consider only the absolute value (as in the previous case) or the sign of the angle. 3. DECENTRALIZED CONTROL FOR TWO-AGENT SYSTEMS From here on, we will use the abbreviation ϕixj =
∂ϕi ∂xj .
3.1 Control strategy Considering system (1) and decentralizing the centralized controller in Loizou and Kyriakopoulos [2003], the following law is proposed.
ui = −sgn ϕixi cos θi + ϕiyi sin θi Zi (7a) Kω (θnhi − θi ) , Δi < 0 (7b) ωi = −Kω ϕiθi , Δi ≥ 0 where Δi = Kω ϕiθi (θnhi − θi ) − ϕixi cos θi + ϕiyi sin θi Zi , Zi = Ku ((ϕixi )2 + (ϕiyi )2 ) + Kz ((xi − xdi )2 + (yi − ydi )2 ),
θnhi = atan2 sgn ((xi − xdi ) cos θdi + (yi − ydi ) sin θdi ) ϕiyi , sgn ((xi − xdi ) cos θdi + (yi − ydi ) sin θdi ) ϕixi , and sgn (x) = 1 if x ≥ 0 and sgn (x) = −1 if x < 0.
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
2
2
i 2 i 2 ϕxi + ϕyi ≤ τ1 for ϕixi ≤ μ1 ∧ ϕiyi ≤ μ2 ∧ μ1 + μ2 ≤ τ1 Let V = √
i 2 i ϕi = ϕ1 + ϕ2 with the ϕi defined in (2). ϕxi ≤ μ1 if ϕixi ≤ μ1 Its time derivative is V˙ = u1 (Vx1 cos θ1 + Vy1 sin θ1 ) + d u2 (Vx2 cos θ2 + Vy2 sin θ2 ) + +ω1 Vθ1 + ω2 Vθ2 . γ di H G − γdi H i G +H Gi γxii Hi Gi i ( ) i i i i xi x x k i i Theorem 3.1. The control (7) drives (1) to an arbitrarily
k 1/k+1 ϕxi = + ≤ 2 γmin γd +Hi Gi small region around the destination configuration. i γdi (|Hxi |Gi +Hi |Gix |) √ i i ≤ μ1 for 2 Proof: Denoting Qi = (ϕixi )2 + (ϕiyi )2 , Sii = ϕixi cos θi + kγmin i i √ 2 μ1 γmin ϕiyi sin θi , Sji = ϕjxi cos θi +ϕjyi sin θi , Ai = |θnhi − (θi − θdi )|, k ≥ 2 max{γdi (|√Hxi |Gi +Hi |Gxi |)} ∧ H ≤ di . 2 i μ γ 1 min 2 max γxi Gi Tii = ϕiθi , Tji = ϕjθi , it can be shown after some calculations that The bounded workspace defined above ensures that these bounds are finite and not equal to zero. Equivalent bounds
2 V˙ ≤ − [ (1 + Zk sgn (Skk ) Sjk − are found on k and Hi for ϕiyi ≤ μ2 . Since the i:Δi ≤0 k:|Δk |>1 j=k workspace is bounded, there exists a τ2 for which (xk − xdk )2 + (yk − ydk )2 ≤ τ2 . −Kω (θnhk − θk ) Tjk ) + j=k | = ϕjxi cos θi + ϕjyi sin θi ≤ ϕjxi + ϕjyi ≤ σ3 for |S jk ϕj ≤ τ3 ∧ ϕj ≤ τ4 ∧ τ3 + τ4 ≤ σ3 + (Zk sgn (Skk ) Sjk − xi yi k:Δk ≤1 j=k dj j γxdij Hj Gj − γdkj Hj Gjx γxi Hj Gj γdj Hj |Gjx | i ϕ = Tjk )] − −Kω (θnhk − θk ) 1/k+1 ≤ γ 2 + kγ 2 i ≤ τ3 xi min min k γdj +Hj Gj j=k 2 2 maxj=i {γdj Hj |Gjxi |} τ3 γmin − [ (2 + Zk sgn (Skk ) Sjk ) + . for k ≥ ∧ Hj ≤ 2 τ γ d 3 min i:Δi >0 k:Zk |Skk |>2 j=k 2 maxj=i γxij Gj + Zk sgn (Skk ) Sjk + The bounded workspace defined above ensures that these j=k k:Zk |Skk |≤2 bounds are finite and not equal j to zero. Equivalent bounds ϕy ≤ τ4 . are found on k and H for j (3 + Tkk Tjk ) + +Kω ( i 3.2 Analysis
k:|Tkk |2 >3
+
2
|Tkk | Tkk
k:|Tkk | ≤3
j=k
Tjk )]
j=k
(8) Given a bounded workspace with −π < θ ≤ π, and assuming that γdi > γmin > 0 and k > 1, these terms can be bounded as follows. 1) For the first term in the first sum of the right hand side of (8) 1 + Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≥ ρ1 , 0 < ρ1 < 1 for |Zk sgn (Skk )
j=k
Sjk − Kω (θnhk − θk )
Tjk | ≤ 1 − ρ(9) 1
j=k
We have Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≤ Zk (N − 1) maxj=k (|Sjk |) + Kω (N − 1)π maxj=k (|Tjk |) which makes (9) equivalent to Zk maxj=k |Sjk | + 1 −ρ1 Kω π maxj=k |Tjk | ≤ N ≡ σ1 . This is true for −1 Zk maxj=k |Sjk | ≤ σ2 σ3 ∧ Kω π maxj=k |Tjk | ≤ σ4 ∧ σ2 σ3 + σ4 ≤ σ1 . a) Zk maxj=k |Sjk | ≤ σ2 σ3 for Zk ≤ σ2 ∧ |Sjk | ≤ σ3 ∀j = k. Zk = Ku ((ϕkxk )2 + (ϕkyk )2 ) + Kz ((xk − xdk )2 + (yk − ydk )2 ) ≤ σ2 for (ϕkxk )2 +(ϕkyk )2 ≤ τ1 ∧ (xk −xdk )2 +(yk −ydk )2 ≤ τ2 Ku τ1 + Kz τ2 ≤ σ2
∧
243
b) Kω π maxj=k |Tjk | ≤ σ4 for |Tjk | ≤ Kσω4π ≡ τ5 ∀j = k. γ dj H G dj max θi j j γθ Hj Gj j i 1/k+1 ≤ |Tjk | = ϕθi = ≤ 2 γmin γdkj +Hj Gj τ5 for Hj ≤
2
τ5 γmin . d maxj=i γθ j Hj Gj i
2) For the second term in thefirst sum of the right hand side of (8) Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk ≥ −ρ2 < 0 for |Zk sgn (Skk ) j=k Sjk − Kω (θnhk − θk ) j=k Tjk | ≤ ρ2 which is equivalent to (1). 3) For the first term in the second sum of the right hand side of (8) 2 + Zk sgn (Skk ) j=k Sjk ≥ ρ3 , 0 < ρ3 < 2 2 −ρ3 for Zk maxj=k |Sjk | ≤ N −1 , equivalent to(1a). 4) For the second term in the second sum of the right hand side of (8) Zk sgn (Skk ) j=k Sjk ≥ −ρ4 , ρ4 > 0 for 4 , equivalent to step (1a). Zk maxj=k |Sjk | ≤ Nρ−1 5) For the third term in the second sum of the right hand side of (8) 3 + Tkk j=k Tjk ≥ ρ5 , 0 < ρ5 < 3 for 3 −ρ5 |Tkk | maxj=k |Tjk | ≤ N −1 ≡ κ1 . |Tkk | maxj=k |Tjk | ≤ κ1 for |Tkk | ≤ κ2 ∧ |Tjk | ≤ κ3 ∀j = k ∧ κ2 κ3 ≤ κ1 . |Tkk | ≤ κ2 is equivalent to |Tjk | ≤ κ3 which is equivalent to step (1b). 6) For the fourth termin the second sum of the right hand side of (8) Tkk j=k Tjk ≥ −ρ6 , ρ6 > 0 for 6 |Tkk | maxj=k |Tjk | ≤ Nρ−1 which is equivalent to step (5).
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
Assuming that at least one agent i fulfills Zi |Sii | > 2 or 2 |Tii | > 3 , then there exists an 1 such that |Δi | > 1 . Assuming also that all the bounds found in steps (1)-(6) are fulfilled gives the two cases
0.9
0.7 0.6
y
0.5 0.4 0.3 0.2 0.1
1) Δi ≤ 0 ⇒ V˙ ≤ −ρ1 + (N − 1)2 ρ2 < 0 for 0 < (N − 1)2 ρ2 < ρ1 < 1 2) Δi > 0 ⇒ V˙ ≤ −ρ3 + (N − 1)2 ρ4 + Kω (−ρ5 + (N − 1)2 ρ6 ) < 0 for 0 < (N − 1)2 ρ4 < ρ3 < 2 ∧ 0 < (N − 1)2 ρ6 < ρ5 < 3
Agent 1 − leader Agent 2 − follower
0.8
0 −0.1 −1
−0.8
−0.6
−0.4
−0.2
0 x
0.2
0.4
0.6
0.8
1
Fig. 2. Collision avoidance of agent 2 for control (7) 4. CONNECTIVITY CONSTRAINED FORMATION CONTROL FOR MULTI-AGENT SYSTEMS
2
This implies that Zi |Sii | and |Tii | converge to arbitrarily small values 2 and 3 , for all i. However, it was shown in de Wit and Sordalen [1992] that |Sii | = 0 is non-invariant when the system does not fulfill ϕixi = ϕiyi = 0. This means that the system converges to an arbitrarily small region of ϕixi = ϕiyi = 0, and ϕiθi = 0. Using similar arguments as in Loizou et al. [2004], we know that points of this form not coinciding with the desired destination have attractors of measure zero for sufficiently large k. Thus, ϕixi = ϕiyi = ϕiθi = 0 only for xi = xdi , yi = ydi and θi = θdi , i.e., the desired destination configuration. Thus the system trajectories converge to the arbitrarily small region around the desired destination configuration.
3.3 Simulations The developed control strategies are next simulated for the two-agent case. In all plots, red arrows correspond to the leader-agent (agent 1) and green arrows to the followeragent (agent 2). Initial and final positions are shown with blue arrows and obstacles as blue circles. All agents are considered as points in this article so the arrows merely show the orientation of the agents and not area. The position of the point agent is shown by the tip of the arrow. The objective is to enclose the obstacle that is located at the origin. Therefore, the destination configuration for agent 1, pd1 = [r 0 − π]T where r is the obstacle’s radius. The desired formation is to have agent 2 mirror agent 1 around the y-axis. Therefore, the formation vector is defined as c21 = [−2x1 0 −(2θ1 +π)]T which makes the destination configuration for agent 2, pd2 = [−x1 y1 − (θ1 + π)]T . Figure 1 shows convergence of two agents to their respective destinations. In Figure 2, obstacles are included to illustrate the collision avoidance. In all cases the tuning parameters used were k = 80, h = 1, λ = 0.01, Ku = 10, Kω = 100 and Kz = 50 and the initial positions were p1 = [0.8 0.8 − π/2]T and p2 = [−0.8 0.8 − π/2]T .
4.1 Control strategy The proposed extension of Gi from Dimarogonas and Johansson [2010] is given by Gi =
j∈Ni
=
βij =
3
βijk =
j∈Ni k=1
(d2maxi − qi − qj 2 )(qi − qj 2 − d2mini ) ·
j∈Ni
·(d2θi − |θi − θj |2 )
(10)
βij1 corresponds to the connectivity and was given in the original constraint function. βij2 corresponds to the constraint that the agent i is not allowed to come closer to agent j than the distance dmini and is implemented as collision avoidance. βij3 handles the connectivity constraints on the angle. If the desired case is the second one, this factor has to be replaced with βij3 = dθi − (θi − θj ). The control strategy is given by ui = −Kϕiqi
(11)
where K > 0 is a tuning parameter, ϕi is defined by (4) with Gi changed to the extended version given in (10). In the general case, the set of neighboring agents can be predefined, or based on distance, depending on the application. For the finger case, the constraints on each finger is defined using the relative position to the hand. Assuming that the fingers are stiffly attached to the hand, their relative positions will not change given a motion in p0 . Thus for all agents i = 0, Ni = {0} and a controller ui0 can be defined using the relative position qi0 = qi − q0 such that qi0 ˙ = ui0 . (12) ui0 = −Kϕiqi0 It can easily be seen that for Ni = {0} these two controllers are equal since ui already uses only relative position. 4.2 Analysis
0.9 Agent 1 − leader Agent 2 − follower
0.8 0.7 0.6
y
0.5 0.4 0.3 0.2 0.1 0 −0.1 −1
−0.8
−0.6
−0.4
−0.2
0 x
0.2
0.4
0.6
0.8
1
Fig. 1. Two agents with control (7) converging with the right orientation
244
1)Connectivity preservation If it can be shown that the set Q = {q ∈ R2N | Gi (q) > 0 ∀i ∈ N } is invariant for the closed-loop system given by (3) and the controller (11), then the connectivity preservation has been proven. This is investigated by following the proof in Dimarogonas and Johansson [2010] but using the extended definition of Gi that was given in (10). Theorem 4.1. The set Q = {q ∈ R2N | Gi (q) > 0 ∀i ∈ N } is invariant for the closed-loop system (3) a, (11).
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
2 −K ϕiqi < 0 ∀ qi = qdi with qi0 the relative position that was defined in section 4.1. It also follows from (11) that the only invariant set where V˙ = 0 is the desired destination qdi . LaSalle’s invariance principle then gives that the system converges to the desired destination. γi (q0 ) k 0 −1/k−1 0 ((γi (q ))) (− k ∇i Gi (q )). ∇i Gi is computed as Theorem 4.3. The control (11) drives (3) with θi defined ∇i Gi = j∈Ni β ij ∇i βij where β ij = l∈Ni βil and ∇i βij = in (6) to an arbitrarily small region around the target. l=j 3 β ∇ β where β = β and i ijk ijm m∈{1,2,3} ijk k=1 ijk Proof: The constraints and destination of an agent i are m=k ∇i βijk = −(qi − qj ), k = 1, ∇i βijk = (qi − qj ), k = 2 defined by the relative position to other agents j ∈ Ni , 0∈ / Ni . The time derivative of the Lyapunov candidate is and ∇i βijk = −(θi − θj )∇i θi , k = 3.
2 ˙ = V ϕ˙ i = i [ϕiqi q˙i + j∈Ni ϕiqj q˙j ] = i [−K ϕiqi − i 0 0 Since Gi (q ) = 0 then βij (q ) = 0 for at least one i j j∈Ni Kϕqj ϕqj ]. Like in previous sections it can be shown j ∈ Ni . Assuming βij (q 0 ) = 0 and βil (q 0 ) = 0 for all
i 2 < l ∈ Ni , l = j, then β ij (q 0 ) > 0 and β il (q 0 ) = 0. Since that all agents i converge to regions where ϕqi and > 0 is arbitrarily small. To do this, the notation 0 0 βij (q ) = 0 then βijk (q ) = 0 for at least one k ∈ {1, 2, 3}. i ˙ ˙ Assuming βijk (q 0 ) = 0 for only one k ∈ {1, 2, 3} gives Pij != ϕqj is used and V can then be written as V = 2 sgn (∇i Gi ) = sgn (∇i βijk ). This gives three cases: −K 2 ≥ (Pii + i:Pii j∈Ni Pij Pjj )+ " 0 2 1) βij1 (q ) = 0. This happens when qi − qj = dmax 2 < (Pii + i:Pii j∈Ni Pij Pjj ) ≤ ! which means that qi − qj is on the limit of becoming ≤ −K too large. The direction of the controller then becomes 2 ≥ ( + i:Pii j∈Ni Pij Pjj )+
" sgn −ϕiqi (q 0 ) = sgn ∇i Gi (q 0 ) = sgn ∇i βij1 (q 0 ) = ( P P ) . It can be shown that given a 2 ij jj i:Pii < j∈Ni sgn (−(qi − qj )), meaning that the controller points in the direction that decreases qi − qj , i.e., towards the set Q. bounded workspace, −π < θ ≤ π, γi > γmin > 0 and k > 1 the terms can be bounded as: 2) βij2 (q 0 ) = 0. This happens when qi − qj = dmin which means that qi − qj is on the limit of becoming 1) + j∈Ni Pij Pjj ≥ ρ1 > 0 for maxj∈Ni |Pij Pjj | ≤ −ρ1 too small. The direction
of the controller then becomes Ni ≡ σ1 . |Pij Pjj | ≤ σ1 for |Pij | ≤ σ2 ∧ |Pjj | ≤ σ3 ∧ i 0 0 0 sgn −ϕqi (q ) = sgn ∇i Gi (q ) = sgn ∇i βij2 (q ) = β ij (βij |qi − qj | 2 sgn (qi − qj ), which means that the controller points in the σ2 σ3 ≤ σ1 . First we have |Pij | ≤ γmin direction that increases qi − qj , that is towards the set + γki (βij2 βij3 |qi − qj | + βij1 βij3 |qi − qj | Q. + βij1 βij2 |(θi − θj )(−∇j θj )|)) ≤ σ2 for k ≥ max(γi ((βij2 βij3 + βij1 βij3 ) |qi − qj | 0 3) βij3 (q ) = 0. This happens when |θi − θj | = dθ j∈Ni 2 which means that |θi − θj | is on the limit of becom- +β β |(θ − θ )(−∇ θ )|))/ min ( σ2 γmin −βij |qi − qj |). ij1 ij2 i j j j β j∈Ni ing too large. The direction can be ij
i 0 of the controller
calculated to be sgn −ϕqi (q ) = sgn ∇i Gi (q 0 ) = ⎛ ⎡ Since k #> 0, this also defines a lower bound on γmin : ⎤⎞ −(yi − yj ) $ max(βij βij |qi −qj |) max(Gi |qi −qj |) ⎜ ⎢ (xi − xj )2 + (yi − yj )2 ⎥⎟ j∈N j∈Ni i ⎢ ⎥ ⎟ γ > = . −(θ which means sgn ⎜ − θ ) min i j ⎣ σ2 σ2 ⎝ ⎦⎠ (xi − xj ) γ (xi − xj )2 + (yi − yj )2 Then, |Pjj | = (γjk + Gj )−1/k−1 Gj ∇j γj − kj ∇j Gj ≤ that the controller points in the direction that decreases 1 2 l∈Nj β jl (βjl |qj − ql | + |θi − θj |, that is towards the set Q. Thus, for all these γmin γj three cases, Q is invariant. If the assumptions above do not ((β jl2 βjl3 +βjl1 βjl3 ) |qj − ql |+βjl1 βjl2 |(θj − θl )(−∇j θj )| k hold, and βijk (q 0 ) = 0 for more than one k ∈ {1, 2, 3} or ≤ σ3 for k ≥ max(γj ((βjl2 βjl3 + l∈Nj if βij (q 0 ) = 0 for more than one j ∈ Ni , then ∇i Gi (q 0 ) = βjl1 βjl3 ) |qj − ql | + βjl1 βjl2 |(θj − θl )(−∇j θj )|))/ i 0 0 0 k 0 0 ⇒ ϕqi (q ) = 0. However, ϕi (q ) = γi (q )/(γi (q ) + σ γ2 − βjl |qj − ql |). min ( N3 min Gi (q 0 ))1/k = 1 and since ϕi : R2N → [0, 1] this means l∈N j β jl j 0 that ϕi reaches its maximum at q . Following the negated gradient of ϕi , the maxima can not be reached from a set of Like for#|Pij | this also gives a lower bound on γmin : $ open initial conditions Dimarogonas and Johansson [2010]. max(βjl βjl |qj −ql |) max(Gj |qj −ql |) l∈Nj The conclusion is then that if the set of initial conditions γmin > l∈Nj = . σ3 σ3 is open, Q is invariant also for this case. The limitations on the workspace ensures that the bounds 2)Formation convergence presented above are finite and greater than zero. Theorem 4.2. The control law (12) drives (3) with θi ρ2 2) j∈Ni Pij Pjj ≥ −ρ2 < 0 if maxj∈Ni |Pij Pjj | ≤ N ≡ defined in (5) to the desired configuration. i σ4 . This leads to lower bounds on k and γmin that are Proof: Consider V = i ϕi . For the case where the equivalent to the ones given in step (1). constraints and destination point for a finger are defined by the relative position to the hand, the time derivative of Assuming that the bounds derived in steps (1) and (2) 2
V becomes V˙ = i ϕ˙ i = i ϕiqi0 q˙i0 = i −K ϕiqi0 = are fulfilled and that there exists at least one agent i Proof: Denote N = {1, . . . , N } and consider i ∈ N and a point q 0 such that Gi (q 0 ) = 0. With the no∂f the derivative of the navigation tation ∇i f = ∂q i function in this point can be written as ϕiqi (q 0 ) =
245
i
IFAC SYROCO 2012 September 5-7, 2012. Dubrovnik, Croatia
4.3 Simulations
0.02
0.015
0.01
y
such that Pii2 ≥ the resulting bound on V˙ becomes V˙ ≤ −K(ρ1 − (N − 1)2 ρ2 ) < 0 for 0 < (N − 1)2 ρ2 < ρ1 . This implies that Pii converge to arbitrarily small for all i. Using similar arguments as in Dimarogonas and Frazzoli [2010] we know that that critical points of ϕi not coinciding with the desired destination have attractors of measure zero for sufficiently large k. Thus the system trajectories converge to the arbitrarily small region around the desired destination configuration.
0.005
0
−0.005
−0.01 −0.02
−0.015
−0.01
−0.005
0 x
0.005
0.01
0.015
0.02
Fig. 5. Simulation showing that the constraint on the maximum angle is fulfilled for all fingers 5. CONCLUSIONS
Simulations of the controller (12) for fingers is shown in two dimensions. We consider the allowed area of an agent to only be defined by its relation to the hand. It is a segment of a circle sector, defined by its minimum and maximum radius and by the maximum angle span. It is illustrated by blue lines in the simulation plots. The agents are shown as colored circles, and the hand is denoted by a blue arrow with the tip in the hand’s position and pointing in the direction of the orientation of the hand. Three simulations are presented that show connectedness in the limits corresponding to each of the three constraints. First, Figure 3 shows that each finger stays a distance dmin = 0.01 from the hand. The starting positions, shown as blue circles, and the desired destinations, shown as blue x’s, were chosen such that if there were no constraints the agents would follow a straight line and thereby enter the forbidden area where their distance to the hand would be less than dmin , see Figure 3. As can be seen, the agents 0.02
0.015
y
0.01
0.005
0
−0.005
−0.01 −0.02
−0.015
−0.01
−0.005
0 x
0.005
0.01
0.015
0.02
Fig. 3. Simulation showing that the constraint on the minimum distance is fulfilled for all fingers do not enter this forbidden area, their trajectories follow the arch shaped boundary. For the other two constraints, the desired destination is placed outside the allowed area, thus violating the assumption made in section 2.2. The fingers then stop when they have reached the limit of the active constraint and do not converge to their destinations. The constraints on maximum distance dmax = 0.02 and maximum angle dθ = 15◦ are shown in Figure 4 and Figure 5 respectively. 0.025
0.02
0.015
y
0.01
0.005
0
−0.005
−0.01
−0.02
−0.01
0 x
0.01
0.02
Fig. 4. Simulation showing that the constraint on the maximum distance is fulfilled for all fingers
246
We extended centralized controllers for non-holonomic multi-agent formation navigation to the decentralized case. For simplicity the modification was made for the two agent case, but the proof showed that the new strategy was valid for multiple agents. The result was supported by simulations. The second task was to investigate if connectivity constraints on a multi-agent system could be extended to include angle connectivity. This was shown to be possible for a system with a central unit from which the angles associated with the agents could be defined. For our case, where the agents represent fingers, this central unit was the hand. Note that the central unit is only used for definition of the angles and that all the agents still have their own controller. Connectivity and convergence for this strategy was shown to hold in theory and could also be visualized in simulations. REFERENCES de Wit, C.C. and Sordalen, O.J. (1992). Exponential stabilization of mobile robots with nonholonomic constraints. IEEE Transactions on Automatic Control, 37(11), 1791– 1797. Dimarogonas, D. and Frazzoli, E. (2010). Analysis of decentralized potential field based multi-agent navigation via primal-dual lyapunov theory. In IEEE Conference on Decision Control, 1215–1220. Dimarogonas, D. and Johansson, K. (2010). Bounded control of network connectivity in multi-agent systems. IET Control Theory and Applications, 4(8), 1330–1338. Do, K. and Lau, M. (2011). Practical formation control of multiple unicycle-type mobile robots with limited sensing ranges. Journal of Intelligent and Robotic Systems. To appear. Loizou, S., Dimarogonas, D., and Kyriakopoulos, K. (2004). Decentralized feedback stabilization of multiple nonholonomic agents. In IEEE International Conference on Robotics and Automation, 3012–3017. Loizou, S. and Kyriakopoulos, K. (2003). Closed Loop Navigation for Multiple Non-Holonomic Vehicles. In IEEE International Conference on Robotics and Automation, 420–425. Mastellone, S., Stipanovic, D., Graunke, C., Intlekofer, K., and Spong, M. (2008). Formation control and collision avoidance for multi-agent non-holonomic systems: Theory and experiments. International Journal of Robotics Research, 27(1), 107–126. Tanner, H.G., Loizou, S.G., and Kyriakopoulos, K.J. (2003). Nonholonomic navigation and control of cooperating mobile manipulators. IEEE Trans. on Robotics and Automation, 19(1), 53–64.