Modeling and Control of Dual Arm Robotic Manipulators

(1)

Modeling and Control of Dual Arm

Robotic Manipulators

KIM VIZINS

(2)

(3)

Abstract

In this thesis, navigation control of multi-agent systems is studied. In particular, the problem statement is based on the application of multi-agent systems’ techniques to the control of dual arm robotic manipulators. The thesis is divided into two separate subproblems: the first one considers decentralized navigation control of a non-holonomic two-agent system with collision avoidance, corresponding to the end effectors of the robotic arms. For this problem, two controllers were found for which the analysis showed stability and convergence to an arbitrarily small region around the desired destinations.

(4)

Acknowledgements

First of all I would like to thank my advisor Dr. Dimos V. Dimarogonas for the guidance and support that made this thesis possible. I would also like to thank the Talent scholarship committee for assigning me this year’s scholar-ship for Master’s thesis and everyone at the Automatic Control Laboratory for welcoming me into the group.

(5)

1 Introduction

In this M.S. thesis project the control of multi-agent systems is studied. The problem formulations are constructed with the motivation that the theory could be applied to robotic arm manipulators. In particular, the focus lies on a dual arm system where each arm has fingers associated with the end effectors. The system would then consist of one agent for each end effector and one agent for 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.

The thesis is divided into two main subproblems that consider the two differ-ent systems described above. The first problem is the proposal and analysis of a decentralized formation controller for two non-holonomic agents with collision avoidance. These agents would correspond to the end effectors of the robotic arms. 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 decen-tralization is done by starting from an already existing centralized controller and investigating the convergence and stability properties when it is modi-fied for a decentralized case. More detailed information about decentralized versus centralized controllers is presented in Section 2.2. Once convergence and stability has been shown in theory the controller is tested in simula-tions for confirmation. This controller is also generalized to include more than two agents.

The second problem is to investigate vector connectivity constraints on mul-tiple holonomic agents. These agents would correspond to the fingers of the robotic arms. At present time, there exist several methods of distance con-nectivity constraints. One of these is chosen for investigating if direction can be included in the constraints while still ensuring both connectivity and convergence. Even though a general solution is preferable, the main consid-eration is to construct a controller that is adapted to the special problem of a hand with fingers associated with it. This controller is also tested in simulations once the convergence has been shown theoretically.

The theoretical work, which is the main part of the thesis, was done at the Automatic Control Laboratory at KTH. There is also a planned demonstra-tion of some of the results, in cooperademonstra-tion with the Center for Autonomous Systems (CAS) at KTH, on their mobile dual arm manipulation platform, known as Dumbo.

(7)

2 Background

The field of multi-agent navigation is of great interest both to the control community due to the interesting theoretical control problems that arise and to the robotics community due to its possible applications in that field. In recent years, there has been several papers discussing different control strategies for formation control [1–4] and collision avoidance [3, 5–9] for multi-agent systems. Some of these will be presented briefly in sections 2.3 and 2.5. The problems that these papers deal with can be divided into categories by two discerned qualities; (i) holonomic or non-holonomic agents and (ii) centralized or decentralized controllers. The differences between these will be explained briefly in the following sections.

2.1 Holonomic and non-holonomic agents

The definition of a holonomic agent is that it has integrable constraints. Its movements are not constrained by its orientation so the dynamics are usually represented by a single integrator:

˙ q = u

where q is the spatial coordinates of the agent.

A non-holonomic agent on the other hand has non-integrable constraints and is represented by spatial coordinates and orientation, p = [qT θ]T and its dynamics are usually modeled as a unicycle. In two dimensions the model can be written as:

˙ p =   ˙ x ˙ y ˙ θ  =   v cos θ v sin θ ω  

where v is the translational velocity and ω is the rotational velocity.

2.2 Centralized and decentralized control

In a centralized control strategy there is a central unit that has all the information about all agents and controls them all. This is usually a com-putationally complex method and is also very vulnerable to agent failure. A decentralized controller does not have these disadvantages since in a decen-tralized control strategy each agent has its own controller which will make each controller much simpler. Furthermore, they only have information about their own destination point. The only information available about the other agents and obstacles is their current coordinates. Clearly, a de-centralized controller would be preferable, but it is not always trivial how these controllers can be constructed.

2.3 Formation control

(8)

2. Background 2.4. Connectivity maintenance

made in [3] where a formation control strategy for holonomic agents was presented that would make N agents converge to a predefined formation. The formation was specified by the vectors cij that correspond to the desired

relationship between agent i and agent j. The strategy in [2] then becomes the special case when cij = 0 for all i, j.

2.4 Connectivity maintenance

An interesting kind of formation control that is useful when studying robotic arms, in particular robotic fingers, are connectivity preserving methods for multi-agent systems. [4,10,11] present different control schemes for keeping agents that start out within a certain distance from each other connected throughout the task performance. Here, connected means that the con-nected agents will remain within the starting distance from each other. In [4] the control law guarantees convergence and connectivity even for upper bounds on the control input which is an advantage when implementing.

2.5 Navigation functions

A navigation function is a special kind of potential field generating functions that can incorporate obstacles and singularities as local maxima and van-ishes at the destination. The agent can then navigate through the workspace by following the negated gradient of the potential function, thus avoiding obstacles and singularities and since no local minima exist, converge to the destination configuration.

[9] presents a collision avoidance scheme for N holonomic agents using a decentralized navigation function. Going into the field of non-holonomic agents, dipolar navigation functions are used in [6] among others to con-struct centralized control strategies for N agents with collision avoidance. The potential field generated by dipolar navigation functions mimic the be-havior of magnetic fields and drives the agents to close in on the destination configuration with the right orientation. It also guarantees that turning in place will only occur in the initial position.

There has also been some work on non-holonomic stabilization using dipo-lar inverse navigation functions [5, 7]. Some of the advantages with these are that they converge faster and are easier to tune than classical dipolar navigation functions.

A first step towards constructing a decentralized controller from a dipolar navigation function similar to the one in [6] was made in [8]. However, since it was assumed that each agent treats the other agents’ destination points as obstacles in this control scheme, every agent must know the destination points of all other agents. Thus the strategy is not completely decentralized. Furthermore, the controller includes the time derivative of the navigation function which makes it difficult to implement.

Kim Vizins June 16, 2011

3 Engineering Physics

(9)

3 Problem formulation

The main focus of this thesis is the case of two agents with the motivation that the two agents represent the end effectors of the two robotic arms. Since the movement of the end effectors will depend on how the joints in the arms are able to move, the movement will be constrained and therefore they will be considered as non-holonomic agents. For this part, the agents will also be constrained to move in a plane so the position will be expressed in two dimensions. The primary task is to find and examine the stability of a decentralized formation control strategy, that also incorporates collision avoidance between the agents and with objects, for this special case. Eventually more agents will be added to the system, motivated by the con-sideration of fingers related to each robotic arm. To make sure that these agents fulfill the natural constraints on fingers, the connectivity preserving tools from [4] will be used. Since [4] only handles distance constraints this approach will be extended to include vector constraints to account for the angle relationship between fingers.

In both cases the agents are considered as points. The following sections describe these cases in more detail.

3.1 Multi-agent non-holonomic system

In a multi-agent non-holonomic system, the state vector for agent i in a plane is defined as pi = [qTi θi]T where qi = [xi yi]T denotes the agent’s

spatial coordinates and θi denotes the agent’s orientation.

The dynamics of the system are modeled as unicycles. The movement is described by a translational velocity ui and a rotational velocity ωi. The

state space model is shown below.

˙ pi= ˙ qi ˙ θi =   ˙ xi ˙ yi ˙ θi  =   uicos θi uisin θi ωi   (3.1)

The controller for agent i is thus given by the velocities ui and ωi. To obtain

a decentralized controller, the criterium is that ui and ωi cannot depend on

the desired position for agent j, pdj for i 6= j. To make the implementation

easier it is also desirable that the controller for agent i does not depend on the velocities of agent j, only the position.

The decentralized controllers in this thesis will be designed using navigation functions. Here, the type of navigation function used will be based on the dipolar potential function ϕ in [6] where it is being used in a centralized controller. A decentralized version of this ϕ can be found in [8] and will be modified slightly to be used in this thesis. The definition of the navigation function ϕi for agent i is shown below.

ϕi=

γi

γk

i + HnhiGi

1/k (3.2)

where k > 1 is a positive tuning parameter, Hnhi and Gi are defined below

(10)

3. Problem formulation 3.2. Two-agent non-holonomic system

As stated in [6], Hnhi has the form of a pseudo-obstacle

Hnhi = nhi+ hik (xi− xdi) cos θdi+ (yi− ydi) sin θdik

2 _(3.3)

where nhi and hiare positive tuning parameters and xdi, ydi and θdi denotes

the desired destination position and orientation. Gi denotes the obstacle function and is defined as

Gi = Y j6=i kq_i− q_jk2_{− (r} i+ rj)2 (3.4)

where ri is the radius of agent i. In this case, ri = 0 for agents, but rj can

be larger than zero if j represents an obstacle.

This means that every agent j 6= i is treated as an obstacle by agent i. Ob-jects that are not agents can also be treated in this function by multiplying over j ∈ Ni where Ni is the set of all agents and objects that affect agent i.

In this thesis, it is assumed that the agents are equipped with some kind of sensors that makes the position of agents and obstacles known.

3.2 Two-agent non-holonomic system

Considering the two-agent system, it is a special case of the multi-agent system, with the number of agents N = 2. That means that the state equations will be the same as defined in (3.1) and the navigation function used for the two agent system will be the one defined in (3.2), with i = 1, 2. In the case of the robotic arms, the controllers should drive the agents to a desired position as well as a desired formation. To make this possible a leader-follower approach will be used. This means that only one of the agents, the leader, needs information about the desired final destination of the formation. Agent 1 is considered to be the leader and its controller will be based on desired formation but also on desired position for the entire formation. Agent 2, being the follower, will only be controlled to keep the formation, and the formation constraints are designed such that agent 2 will converge to its desired position as agent 1 converges to its desired position. Since agent 1 is leading the formation, the definition of the goal function γi

will be different for agent 1 and agent 2. γ1= kp1− pd1k

2

λ+ Kckp1− p2− c12k2λ (3.5)

γ2= kp2− p1− c21k2λ (3.6)

where Kc> 0 is a tuning parameter and c12(q) = −c21(q) defines the

forma-tion constraints. For agent 2, the destinaforma-tion configuraforma-tion 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, the vector that defines the formation can vary with the configuration of the agents to adapt to that path. Thus c12 changes with time if the agents’

positions or directions change. The norm that depends on both position and angle is here denoted as kpkλ and, like in [5], is defined as

kpk_λ =px2_{+ y}2_{+ λθ}2

where λ is a small, positive tuning parameter.

(11)

3. Problem formulation 3.3. Multi-agent holonomic system

3.3 Multi-agent holonomic system

In a general multi-agent holonomic system, the state vector for agent i in a plane is defined as qi= [xi yi]T which denotes the agent’s spatial coordinates.

The dynamics of the system are modeled as single integrators. The state space model is thus defined as

˙

qi= ui (3.7)

where ui is the translational velocity vector.

The controller for agent i is thus given by the velocity ui. As stated above, to

obtain a decentralized controller, the criterium is that ui cannot depend on

the desired position for agent j, qdj for i 6= j and for easier implementation

the controller for agent i should not depend on the velocities of agent j, only on the position.

The type of navigation function used in the case of the holonomic fingers will be based on the potential function ϕi in [4]. It is similar in structure to

the navigation function used in the previous cases but since the fingers are considered as holonomic there is no need for a dipolar potential function. The definition of this navigation function is shown below.

ϕi=

γi

γk

i + Gi

1/k (3.8)

where k > 1 is a positive tuning parameter and γiand Gi are defined below.

In [4] a difference in γi and Gi is made between a controller for consensus

and for formation control. In this case the formation controller will be used since it is more general. The consensus controller is actually a special case of the formation controller with the formation vector equal to zero. So for the formation control, the goal function γi is defined as

γi =

X

j∈Ni

kq_i− q_j− c_ijk2 (3.9)

Ni denotes the set of neighbors of agent i which is defined here as all agents

j within distance di from agent i.

Gi denotes the constraint function and is defined as

Gi= Y j∈Ni di− kcijk 2 − kq_i− q_j− c_ijk2 (3.10)

which means that every agent j 6= i that is within the distance difrom agent

i at time t = 0 will remain within that distance for any t. The formation is defined by cij = [cijx cijy]

T _{which is the formation vector in the xy-space.}

To make the problem feasible, the formation is assumed to define a goal position that lies within the allowed region for the agent in question. The function Gi is also taken from [4] and takes only the euclidian distance

into account for the connectivity constraints. The task now is to extend this function so that it can also take orientation into account.

(12)

hand. The design of the connectivity constraints will be specific to this case. The hand will be denoted as agent 0 and will have both position and orientation, in two dimensions p0 = [qT0 θ0]T. The fingers will be denoted as

agent i, i = 1, . . . N where N is the number of fingers. Each finger will also have a position and an angle associated with them. This angle is derived from the relative position of the finger to the center of the hand and is used to define the allowed area for that finger. In Figure 1 this allowed area is shown as a segment of a circle sector that is defined by the maximum and minimum distance to the center of the hand and on the maximum angle deviation from a predefined base angle. The state vector for agent i is thus

dmin,i

dmax,i

θni

θ0

dθi

Figure 1: The allowed area for agent i in relation to agent 0, whose position and orientation are denoted by the red arrow

pi = [qTi θi]T where

θi= atan2 (yi− y0, xi− x0) − θni (3.11)

and θni defines the center of the allowed circle sector in relation to the hand,

see Figure 1.

It is also possible to define the allowed area based on other finger agents. However, it is still necessary to have a central agent (in this case the hand) to define the angles. A possible definition of the angle of each agent i is then

θi = atan2 (yi− y0, xi− x0) (3.12)

The allowed area defined by angle and minimum and maximum distance would look different in this case, and also depend on if the constraints on the angle consider only the absolute value (like in the previous case) or if the sign of the angle also matters. The two cases can be seen in Figures 2 and 3.

(13)

dmin,ij

dmax,ij

dθij

θj

Figure 2: The allowed area for agent i in relation to agent j, whose position is denoted by the red circle, with angle span specified by absolute value

dmin,ij

dmax,ij

dθij

θj

(14)

4 Decentralized control for two-agent systems

4.1 Control strategies

From the theory of navigation functions explained in section 2.5 it is clear that the controller must depend on the negated gradient of the navigation function.

Using the theory from [12] the following feedback law is proposed.

ui = −Kusgn ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi ∂ϕi ∂xi 2 + ∂ϕi ∂yi 2! (4.1a) ωi = −Kωsgn ∂ϕi ∂θi |θ_nh_i− (θi− θdi)| (4.1b)

with Ku > 0 and Kω > 0 positive control gains, the angle θnhi is given by

θnhi = atan2 sgn (xi− xdi) ∂ϕi ∂yi , sgn (xi− xdi) ∂ϕi ∂xi (4.2)

and the sign function is defined below.

sgn (x) =

1 , if x ≥ 0

−1 , if x < 0 (4.3)

Because of this controller’s discontinuity in ωi, it might be difficult to

im-plement due to numerical errors. Therefore, another controller will also be investigated. It is taken from [6], where it is used in a centralized control setup, and will be modified here for implementation in the decentralized con-trol setup considered here. The second proposed feedback law is as follows.

ui = −sgn ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi Zi (4.4a) ωi = Kω(θnhi − θi) , ∆i< 0 −K_ω∂ϕi ∂θi , ∆i≥ 0 (4.4b) where ∆i= Kω ∂ϕi ∂θi (θnhi − θi) − ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi Zi (4.5) and Zi = Ku ∂ϕi ∂xi 2 + ∂ϕi ∂yi 2! + Kz xi− xdi 2 + yi− ydi 2 (4.6)

The sign function in (4.4) is also defined by (4.3) but θnhi is redefined as

(15)

4. Decentralized control for two-agent systems 4.2. Analysis

4.2 Analysis

The convergence of the system is analyzed in this section using the theory of Lyapunov functions [13]. It will be shown that using both the proposed control strategies (4.1) and (4.4) the system will converge to a region around the destination configuration defined by an arbitrarily small , following the procedure presented in [14].

The Lyapunov candidate for both controllers is chosen to be

V =X

i

ϕi= ϕ1+ ϕ2 (4.8)

with the ϕi defined in (3.2).

Since ϕi > 0 ∀(pi− pdi) 6= 0 the Lyapunov candidate is a positive function

that is only equal to zero when p1 = pd1 and p2 = pd2. Taking the time

derivative gives ˙ V = _∂x∂V 1x˙1+ ∂V ∂y1y˙1+ ∂V ∂θ1 ˙ θ1_∂x∂V₂x˙2+_∂y∂V₂y˙2+_∂θ∂V₂θ˙2 = = u1 ∂V ∂x1 cos θ1+ ∂V ∂y1sin θ1 + u2 ∂V ∂x2 cos θ2+ ∂V ∂y2 sin θ2 + +ω1_∂θ∂V₁ + ω2_∂θ∂V₂ (4.9)

4.2.1 First feedback law

Theorem 4.1. The control law (4.1) drives the system (3.1) to converge to a region around the destination configuration defined by an arbitrarily small > 0.

Proof. Introducing the first proposed feedback law (4.1), the time derivative of the Lyapunov candidate becomes

(16)

4. Decentralized control for two-agent systems 4.2. Analysis = −Ku " ∂ϕ1 ∂x1 2 +∂ϕ1 ∂y1 2 ∂ϕ1 ∂x1 cos θ1+ ∂ϕ1 ∂y1 sin θ1 + +sgn ∂ϕ1 ∂x1 cos θ1+ ∂ϕ1 ∂y1 sin θ1 ∂ϕ2 ∂x1 cos θ1+ ∂ϕ2 ∂y1 sin θ1 + + ∂ϕ2 ∂x2 2 + ∂ϕ2 ∂y2 2 ∂ϕ2 ∂x2 cos θ2+ ∂ϕ2 ∂y2 sin θ2 + +sgn∂ϕ2 ∂x2 cos θ2+ ∂ϕ2 ∂y2 sin θ2 ∂ϕ1 ∂x2 cos θ2+ ∂ϕ1 ∂y2 sin θ2 # − −Kω " |θnh1 − (θ1− θd1)| ∂ϕ1 ∂θ1 + sgn ∂ϕ1 ∂θ1 ∂ϕ2 ∂θ1 + + |θnh2− (θ2− θd2)| ∂ϕ2 ∂θ2 + sgn ∂ϕ2 ∂θ2 ∂ϕ1 ∂θ2 # (4.10)

For brevity the following variables are defined:

(17)

(4.10) can now be written as

˙ V = −Ku " P i:Qi|Sii|>1 Qi |Sii| +P_j6=isgn (Sii) Sji + + P i:Qi|Sii|≤1 Qi |S_ii| +P j6=isgn (Sii) Sji # − −K_ω " P i:Ai|Tii|>2 Ai |T_ii| +P j6=isgn (Tii) Tji + + P i:Ai|Tii|≤2 Ai |T_ii| +P j6=isgn (Tii) Tji # ≤ ≤ −K_u " P i:Qi|Sii|>1 1+Pj6=iQisgn (Sii) Sji + + P i:Qi|Sii|≤1 P j6=iQisgn (Sii) Sji # − −Kω " P i:Ai|Tii|>2 2+Pj6=iAisgn (Tii) Tji + + P i:Ai|Tii|≤2 P j6=iAisgn (Tii) Tji # (4.17)

where 1 and 2 are arbitrarily small and positive. This holds also for a

general system of N agents, thus the rest of the proof will be on the general form where i = 1, . . . , N .

Given a bounded workspace with −π < θ ≤ π, γdi > γmin > 0 and k > 1,

the terms in (4.17) can be shown to be bounded as follows:

(i) For the first term

(18)

4. Decentralized control for two-agent systems 4.2. Analysis ∂ϕj ∂xi = _∂γ dj ∂xi HjGj− γ_dj k Hj _∂G j ∂xi γ_dk j+ HjGj 1/k+1 ≤ ≤ ∂γ_dj ∂xi HjGj γ_min2 + γdjHj ∂Gj ∂xi kγ2_min ≤ σ1 if k ≥ 2 maxj6=i n γdjHj ∂Gj ∂xi o σ1γ2min (4.18) ∧ Hj ≤ σ1γ_min2 2 maxj6=i n ∂γ_dj ∂xi Gj o (4.19)

The bounded workspace defined above ensures that these bounds are finite and not equal to zero. Equivalent bounds are found on k and Hj

for ∂ϕj ∂yi ≤ σ1. ∂ϕi ∂xi 2 + ∂ϕi ∂yi 2 ≤ 2σ1if ∂ϕi ∂xi 2 ≤ σ1 ∧ ∂ϕi ∂yi 2 ≤ σ1 ∂ϕi ∂xi 2 ≤ σ1 if ∂ϕi ∂xi ≤√σ1 ∂ϕi ∂xi = _∂γ di ∂xi HiGi− γ_di k ∂Hi ∂xi Gi+ Hi ∂Gi ∂xi γk di+ HiGi 1/k+1 ≤ ≤ ∂γ_di ∂xi HiGi γ2 min + γdi ∂Hi ∂xi Gi+ Hi ∂Gi ∂xi kγ2 min ≤√σ1 if k ≥ 2 max n γdi ∂Hi ∂xi Gi+ Hi ∂Gi ∂xi o √ σ1γmin2 (4.20) ∧ Hi ≤ √ σ1γ2min 2 max n ∂γ_di ∂xi Gi o (4.21)

The bounded workspace defined above ensures that these bounds are finite and not equal to zero. Equivalent bounds are found on k and Hi

for ∂ϕi ∂yi 2 ≤ σ1.

(ii) For the second term X j6=i Qisgn (Sii) Sji ≥ −ρ2 < 0 if max j6=i {|Sji|} Qi ≤ ρ2 N − 1

The proof and resulting bounds on k and Hj are equivalent to (i) with

(19)

(iii) For the third term

2+ X j6=i Aisgn (Tii) Tji ≥ ρ3 > 0 if max j6=i {|Tji|} Ai≤ 2− ρ3 N − 1 ≡ σ2 max

j6=i {|Tji|} Ai = maxj6=i

∂ϕj ∂θi · |θ_nh_i− (θi− θdi)| ≤ ≤ (|θ_nh_i| + |θ_i| + |θ_d_i|) max j6=i ∂ϕj ∂θi ≤ ≤ 3π max j6=i ∂ϕj ∂θi ≤ σ2 if max j6=i ∂ϕj ∂θi ≤ σ2 3π ∂ϕj ∂θi = _∂γ dj ∂θi HjGj γk dj+ HjGj 1/k+1 ≤ max n ∂γ_dj ∂θi HjGj o γ2 min ≤ σ2 3π if Hj ≤ σ2γmin2 3π maxj6=i n ∂γ_dj ∂θi HjGj o (4.22)

(iv) For the last term

X j6=i Aisgn (Tii) Tji ≥ −ρ4 < 0 if max j6=i {|Tji|} Ai≤ ρ4 N − 1

The proof and resulting bound on Hj is equivalent to (iii) with σ2 = ρ4

N −1

Putting (i)-(iv) together and assuming there exists at least one agent such that Qi|Sii| > 1 and one agent such that Ai|Tii| > 2 the resulting bound

on ˙V becomes

˙

V ≤ −Kuρ1+ Ku(N − 1)2ρ2− Kωρ3+ Kω(N − 1)2ρ4 (4.23)

The right hand side is strictly negative if k and H satisfy the criteria above and if 0 < (N − 1)2ρ2 < ρ1 and 0 < (N − 1)2ρ4 < ρ3.

This implies that Qi|Sii| and Ai|Tii| converge to arbitrarily small 1 and 2

respectively, for all i. As 1 and 2 go to zero, Qi|Sii| and Ai|Tii| converge

to zero. Qi|Sii| = 0 can be fulfilled by either Qi = 0 or |Sii| = 0. However,

it was shown in [15] that |Sii| = 0 is non-invariant when the system does

not fulfill ∂ϕi

∂xi =

∂ϕi

∂yi = 0. This means that the system will converge to ∂ϕi

∂xi =

∂ϕi

∂yi = 0 and ∂ϕi

∂θi = 0 or θi = θdi which can only be fulfilled at the

desired destination.

It can also be shown that the only invariant set for which ˙V = 0 is the desired destination. For a set to be invariant it is required that

(20)

But the only configuration that gives ∂ϕi

∂xi = ∂ϕi ∂yi = 0 is when γdi = ∂γ_di ∂xi = ∂γ_di

∂yi = 0 which is only true for xi = xdi, yi = ydi and θi = θdi, i.e. the

desired destination configuration. It also follows from ∂ϕi

∂xi =

∂ϕi

∂yi = 0 that

θnhi = 0 which means that the only invariant set is the desired destination

and that ˙V = 0 at that point. LaSalle’s invariance principle then states that the system trajectories converge to the arbitrarily small region around the desired destination configuration [13].

4.2.2 Second feedback law

Theorem 4.2. The control law (4.4) drives the system (3.1) to converge to a region around the destination configuration defined by an arbitrarily small > 0.

Proof. Inserting the second feedback law (4.4) into the time derivative of the Lyapunov function (4.9) gives

˙ V = −X i Zi ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi + +sgn ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi X j6=i ∂ϕj ∂xi cos θi+ ∂ϕj ∂yi sin θi ! + +X i ωi ∂ϕi ∂θi +X j6=i ∂ϕj ∂θi (4.25)

As can be seen this holds for a general system of N agents, thus the rest of the proof will be on the general form where i = 1, . . . , N .

The expression can then be divided into the two cases for each agent that determines the value of ωi.

˙ V = X i:∆i≤0 " ∆i− Zisgn ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi · ·X j6=i ∂ϕj ∂xi cos θi+ ∂ϕj ∂yi sin θi + Kω θnhi− θi X j6=i ∂ϕj ∂θi # − − X i:∆i>0 " Zi ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi + +sgn ∂ϕi ∂xi cos θi+ ∂ϕi ∂yi sin θi X j6=i ∂ϕj ∂xi cos θi+ ∂ϕj ∂yi sin θi ! + +Kω ∂ϕi ∂θi 2 +∂ϕi ∂θi X j6=i ∂ϕj ∂θi !# (4.26)

Using the notation defined in (4.11)-(4.16) in section 4.2.1 and using the

(21)

same method as in (4.17), (4.26) can be written as

˙ V = X i:∆i≤0 " X k:|∆k|>1 ∆k− Zksgn (Skk) X j6=k Sjk+ Kω θnhk− θk X j6=k Tjk + + X k:∆k≤1 ∆k− Zksgn (Skk) X j6=k Sjk+ Kω θnhk − θk X j6=k Tjk # − − X i:∆i>0 " X k:Zk|Skk|>2 Zk |Skk| + sgn (Skk) X j6=k Sjk + + X k:Zk|Skk|≤2 Zk |Skk| + sgn (Skk) X j6=k Sjk + +Kω X k:|Tkk|2>3 |T_kk| |T_kk| + sgn (T_kk)X j6=k Tjk + + X k:|Tkk|2≤3 |T_kk| |T_kk| + sgn (T_kk)X j6=k Tjk !# ≤ ≤ − X i:∆i≤0 " X k:|∆k|>1 1+ Zksgn (Skk) X j6=k Sjk− Kω θnhk− θk X j6=k Tjk + + X k:∆k≤1 Zksgn (Skk) X j6=k Sjk− Kω θnhk− θk X j6=k Tjk # − − X i:∆i>0 " X k:Zk|Skk|>2 2+ Zksgn (Skk) X j6=k Sjk + + X k:Zk|Skk|≤2 Zksgn (Skk) X j6=k Sjk+ +Kω X k:|Tkk|2>3 3+ Tkk X j6=k Tjk + X k:|Tkk|2≤3 |Tkk| Tkk X j6=k Tjk !# (4.27)

Using the same limitations on the workspace as in 4.2.1 these terms can now be shown to be bounded as follows.

(i) For the first term in the first sum

(22)

4. Decentralized control for two-agent systems 4.2. Analysis Zksgn (Skk) X j6=k Sjk − Kω θnhk− θk X j6=k Tjk ≤ ≤ Zk(N − 1) max j6=k |Sjk| + Kω(N − 1)π maxj6=k |Tjk|

which makes (4.28) equivalent to

Zkmax j6=k |Sjk| + Kωπ maxj6=k |Tjk| ≤ 1− ρ1 N − 1 ≡ σ1 This is true if Zkmax j6=k |Sjk| ≤ σ2σ3 ∧ Kωπ maxj6=k |Tjk| ≤ σ4 ∧ σ2σ3+ σ4 ≤ σ1 a) Zkmax j6=k |Sjk| ≤ σ2σ3 if Zk≤ σ2 ∧ |Sjk| ≤ σ3 ∀j 6= k

|Sjk| ≤ σ3 was shown in section 4.2.1(i).

Zk = Ku ∂ϕk ∂xk 2 + ∂ϕk ∂yk 2! + +Kz xk− xdk 2 + yk− ydk 2 ≤ σ₂ if ∂ϕk ∂xk 2 + ∂ϕk ∂yk 2 ≤ τ1 ∧ xk− xdk 2 + yk− ydk 2 ≤ τ2 ∧ Kuτ1+ Kzτ2 ≤ σ2 ∂ϕk ∂xk 2 + ∂ϕk ∂yk 2

≤ τ₁ was shown in section 4.2.1(i) and since the

workspace is bounded, there exists a τ2 for which xk − xdk

2 + yk− ydk 2 ≤ τ₂. b) Kωπ max j6=k |Tjk| ≤ σ4 if |Tjk| ≤ σ4 Kωπ ≡ τ3∀j 6= k

|T_jk| ≤ τ₃ was shown in section 4.2.1(iii).

(ii) For the second term in the first sum

Zksgn (Skk) X j6=k Sjk− Kω θnhk− θk X j6=k Tjk ≥ −ρ2< 0 if Zksgn (Skk) X j6=k Sjk− Kω θnhk− θk X j6=k Tjk ≤ ρ₂

which is equivalent to (i).

(23)

(iii) For the first term in the second sum

2+ Zksgn (Skk) X j6=k Sjk ≥ ρ3 , 0 < ρ3 < 2 if Zkmax j6=k |Sjk| ≤ 2− ρ3 N − 1

which is equivalent to (ia).

(iv) For the second term in the second sum

Zksgn (Skk) X j6=k Sjk ≥ −ρ4 , ρ4 > 0 if Zkmax j6=k |Sjk| ≤ ρ4 N − 1 which is equivalent to (ia).

(v) For the third term in the second sum

3+ Tkk X j6=k Tjk ≥ ρ5 , 0 < ρ5 < 3 if |T_kk| max j6=k |Tjk| ≤ 3− ρ5 N − 1 ≡ κ1 |T_kk| max j6=k |Tjk| ≤ κ1 if |Tkk| ≤ κ2 ∧ |Tjk| ≤ κ3 ∀j 6= k ∧ κ2κ3≤ κ1

|Tkk| ≤ κ2 is equivalent to |Tjk| ≤ κ3 which is equivalent to (ib).

(vi) For the fourth term in the second sum

Tkk X j6=k Tjk ≥ −ρ6 , ρ6 > 0 if |T_kk| max j6=k |Tjk| ≤ ρ6 N − 1 which is equivalent to (v).

Assuming that at least one agent i fulfills Zi|Sii| > 2 or |Tii|2 > 3, then

there exists an 1 such that |∆i| > 1. Assuming also that all the bounds

found in (i)-(vi) are fulfilled gives the two cases

(24)

4. Decentralized control for two-agent systems 4.3. Simulations

This implies that Zi|Sii| and |Tii|2 converge to arbitrarily small 2 and 3

respectively, for all i. As 2 and 3 go to zero, Zi|Sii| and |Tii|2 converge to

zero. Zi|Sii| = 0 can be fulfilled by either Zi = 0 or |Sii| = 0. However,

it was shown in [15] that |Sii| = 0 is non-invariant when the system does

not fulfill ∂ϕi

∂xi =

∂ϕi

∂yi = 0. This means that the system will converge to ∂ϕi

∂xi =

∂ϕi

∂yi = 0, xi = xdi, yi= ydi and ∂ϕi

∂θi = 0 which can only be fulfilled at

the desired destination.

It can also be shown that the only invariant set for which ˙V = 0 is the desired destination. For a set to be invariant it is required that

ui = ωi = 0 (4.31)

ui is only equal to zero when ∂ϕ_∂x_ii = ∂ϕ_∂y_ii = 0, xi = xdi and yi = ydi. This

gives ∆i = 0 ⇒ ωi = −Kω∂ϕ_∂θ_ii which is equal to zero only for ∂ϕ_∂θ_ii = 0. The

only configuration that gives ∂ϕi

∂xi = ∂ϕi ∂yi = ∂ϕi ∂θi = 0 is when γdi = ∂γ_di ∂xi = ∂γ_di ∂yi = ∂γ_di

∂θi = 0 which is only true for xi = xdi, yi = ydiand θi= θdi, i.e., the

desired destination configuration. The only invariant set is thus the desired destination and as was shown above, this will also give ˙V = 0. LaSalle’s invariance principle then states that the system trajectories converge to the arbitrarily small region around the desired destination configuration [13].

4.3 Simulations

To verify the control strategies that have been developed and analyzed, they are being tested by simulations in Matlab for the two-dimensional two-agent case. In all the plots that are shown, the red arrows correspond to the leading agent (agent 1) and the green arrows to the follower-agent (agent 2). Initial and final positions are shown with blue arrows and obstacles are drawn as blue circles. All agents are considered as points in this thesis so the arrows do not correspond to any volume, they are merely showing the orientation of the agents. The position of the point agent is represented by the tip of the arrow.

The objective for the agents is to enclose the obstacle that is located at the origin, preparing to grab it. Therefore, the destination configuration for agent 1, pd1 = [r 0 − π]

T _{where r is the radius of the obstacle. In}

this case, the desired formation is to have agent 2 as a mirror image of 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_.

Using the first feedback law, (4.1), some problems arose with numerical er-rors. This was probably due to problems that are known to occur in some cases of implementation of the sign function. Especially, for sgn

∂ϕi ∂θi

in (4.1), where the sign function has the angle in it, which by definition of the workspace has a discontinuity at π, these problems can arise. Therefore, the stability that was shown in the analysis in section 4.2.1 could not be shown in simulation.

(25)

4. Decentralized control for two-agent systems 4.3. Simulations

The second feedback law, (4.4), works very well in simulations after some tuning. Figure 4 shows the convergence of two agents to their respective destination points. In Figure 5 and 6, extra obstacles are included to illus-trate the collision avoidance of the agents. In all of these three 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. The parameters were chosen by hand tuning so it

is possible that for a more theoretical approach, the parameters would have been slightly different and might have resulted in more optimal trajectories for the agents. In particular, in the collision avoidance examples (Figures 5 and 6) it can be seen that the agents come very close to the obstacle be-fore they take action to try to avoid it. This is because of the structure of the potential field, which depends on the tuning parameters, so for different tuning parameters they might show a more smooth behavior.

−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 x y Agent 1 − leader Agent 2 − follower

Figure 4: Two agents with decentralized control (4.4) converging to the destination positions with the right orientation

−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 x y Agent 1 − leader Agent 2 − follower

Figure 5: Illustrating collision avoidance of agent one for control strategy (4.4)

(26)

4. Decentralized control for two-agent systems 4.3. Simulations −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 x y Agent 1 − leader Agent 2 − follower

Figure 6: Illustrating collision avoidance of agent two for control strategy (4.4)

object that they are going to enclose, i.e. when they have to pass each other to reach their destination positions, was also tested. The best results were given for a change in the parameter k, to k = 72. The simulation plot for initial positions p1 = [−0.2 0.8 − π/2]T and p2 = [0.2 0.8 − π/2]T is shown

in Figure 7. In this plot the agents do not converge completely to the desired destinations. This is due to the small time step needed and limitations on the number of iterations that could be handled by Matlab in a reasonable amount of time. However, judging by the plot it is a reasonable assumption that with more iterations the agents will converge to the desired destination configurations. In future implementation, this could probably be solved with an adaptive step size.

From the simulations it can also be seen that the control strategy contains some robustness against uncertain information about for example the size of the object. If the radius of the object is underestimated, thus making the goal position end up inside the object, the obstacle avoidance part will make sure that the agents still stop at the boundary of the object. However, the robustness does not work for overestimation of the object’s size.

(27)

4. Decentralized control for two-agent systems 4.3. Simulations −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 x y Agent 1 − leader Agent 2 − follower

(28)

5 Connectivity constrained formation control for

multi-agent systems

5.1 Control strategy

The proposed extension of the constraint function Gi given in (3.10) is given

by Gi = Y j∈Ni βij = Y j∈Ni 3 Y k=1 βijk = (5.1) = Y j∈Ni d2_max_i− kq_i− q_jk2kq_i− q_jk2− d2_min i d2_θ_i− |θ_i− θ_j|2

The first factor βij1 corresponds to the connectivity and was given in the

original constraint function. The second factor βij2corresponds to the

con-straint that the agent i is not allowed to come closer to agent j than the distance dminiand is implemented as collision avoidance. The last factor βij3

handles the connectivity constraints on the angle. If the desired case is the one shown in Figure 3, this factor has to be replaced with βij3= dθi−(θi−θj)

Note that βij1 is not exactly the same as defined in the original constraint

function. Instead, the constraint function for the consensus case that was presented in [4] is used. The reason is that (3.10) will be equal to zero for some cases when kqi − qjk 6= di, i.e. not on the boundary of the allowed

region. This means that for some initial conditions that should be feasible, the system would not be able to converge.

The control strategy is given by

ui= −K

∂ϕi

∂qi

(5.2)

where K > 0 is a tuning parameter, ϕi is defined by (3.8) and with Gi

changed to the extended version given in (5.1).

In the general case, the set of neighboring agents can be predefined, or defined based on for example distance, depending on the application. For the case with the fingers, the constraints on each finger can be defined using only the relative position and orientation to the hand. Assuming that the fingers are stiffly attached to the hand, their relative positions will not change given a motion in p0. This means that for all agents i 6= 0, Ni = {0}

and a controller ui0 can be defined using the relative position qi0 = qi− q0

such that ˙qi0= ui0.

ui0= −K

∂ϕi

∂qi0

(5.3)

It can easily be seen that for Ni= {0} these two controllers are equal since

ui already uses only relative position.

5.2 Analysis

The analysis of the connectivity constrained formation controller can be divided into two parts. The first feature of the controller that is going

(29)

5. Connectivity constrained formation control 5.2. Analysis

to be analyzed is the connectivity preservation. Next, the stability of the formation convergence will be analyzed.

5.2.1 Connectivity preservation

If it can be shown that the set Q = {q ∈R2N _{| G}

i(q) > 0 ∀i ∈ N } is

invari-ant for the closed-loop system given by (3.7) and the controller (5.2), then the connectivity preservation has been proven. This will be investigated by following the proof in [4] but using the extended definition of Gi that was

given in (5.1).

Theorem 5.1. The set Q = {q ∈R2N _{| G}

i(q) > 0 ∀i ∈ N } is invariant for

the closed-loop system given by (3.7) and the controller (5.2)

Proof. Define N as the set of all agents i = 1, . . . , N and consider i ∈ N and a point q0 such that Gi(q0) = 0. With the notation ∇if = _∂q∂f_i the derivative

of the navigation function in this point can be written as

∂ϕi ∂qi (q0) = γ_ik(q0) −1/k−1 −γi(q 0₎ k ∇iGi(q 0₎ ∇_iGi is computed as ∇_iGi = X j∈Ni β_ij∇_iβij where β_ij = Y l∈Ni l6=j βil and ∇iβij = 3 X k=1 β_ijk∇_iβijk where β_ijk = Y m∈{1,2,3} m6=k βijm and ∇iβijk =    −(q_i− q_j) , k = 1 (qi− qj) , k = 2 −(θ_i− θ_j)∇iθi , k = 3

Since Gi(q0) = 0 then βij(q0) = 0 for at least one j ∈ Ni. Assuming

βij(q0) = 0 and βil(q0) 6= 0 for all l ∈ Ni, l 6= j, then βij(q0) > 0 and

β_il(q0) = 0. Since βij(q0) = 0 then βijk(q0) = 0 for at least one k ∈

{1, 2, 3}. Assuming β_ijk(q0) = 0 for only one k ∈ {1, 2, 3} gives sgn (∇iGi) =

sgn (∇iβijk). This gives three cases that need to be investigated:

(i) βij1(q0) = 0

This happens when kqi− qjk = dmax which means that kqi− qjk is on

(30)

which means that the controller points in the direction that decreases kqi− qjk, that is towards the set Q.

(ii) βij2(q0) = 0

This happens when kqi− qjk = dmin which means that kqi− qjk is on

the limit of becoming too small. The direction of the controller then becomes sgn −∂ϕi ∂qi (q0) = sgn ∇iGi(q0) = sgn ∇iβij2(q0) = sgn (qi− qj)

which means that the controller points in the direction that increases kqi− qjk, that is towards the set Q.

(iii) βij3(q0) = 0

This happens when |θi− θj| = dθ which means that |θi− θj| is on

the limit of becoming too large. The direction of the controller then becomes sgn −∂ϕi ∂qi (q0) = sgn ∇iGi(q0) = sgn ∇iβij3(q0) = sgn (−(θi− θj)∇iθi) = sgn −(θi− θj) " _∂θ i ∂xi ∂θi ∂yi #! = sgn −(θ_i− θ_j) " −(y_i−y_j) (xi−xj)2+(yi−yj)2 (xi−xj) (xi−xj)2+(yi−yj)2 #!

which means that the controller points in the direction that decreases |θ_i− θ_j|, that is towards the set Q.

Thus, for all these three cases, Q is invariant.

If the assumptions above do not hold, and βijk(q0) = 0 for more than one

k ∈ {1, 2, 3} or if βij(q0) = 0 for more than one j ∈ Ni, then

∇_iGi(q0) = 0 ⇒ ∂ϕi ∂qi (q0) = 0 However, ϕi(q0) = γi(q0)/ γik(q0) + Gi(q0) 1/k = 1 and since ϕi : R2N →

[0, 1] this means that ϕi reaches its maximum at q0. Following the negated

gradient of ϕi, the maxima can not be reached from a set of open initial

conditions [4]. The conclusion is then that if the set of initial conditions is open, Q is invariant also for this case.

5.2.2 Formation convergence

Like in previous sections, the Lyapunov candidate is defined as

V =X

i

ϕi (5.4)

Theorem 5.2. The control law (5.3) drives the system (3.7) to converge to the desired configuration.

(31)

Proof. ϕi(qi) > 0 ∀ qi 6= qdi and ϕi(qdi) = 0 which means that the first

Lyapunov criteria hold for this function.

For the case where the constraints and destination point for a finger are defined by the relative position to the hand, the time derivative of the Lya-punov candidate becomes

˙ V = X i ˙ ϕi = X i ∂ϕi ∂qi0 ˙ qi0= X i −K ∂ϕi ∂qi0 2 = = X i −K ∂ϕi ∂qi 2 < 0 ∀ qi6= qdi (5.5)

with qi0 the relative position that was defined in section 5.1. It also follows

from the definition of the controller (5.2) that the only invariant set where ˙

V = 0 is the desired destination qdi.

LaSalle’s invariance principle then gives that the system will converge to the desired destination [13].

Theorem 5.3. The control law (5.2) drives the system (3.7) to converge to a region around the desired configuration defined by an arbitrarily small > 0.

Proof. The second case, when constraints and destination point of an agent i are defined by the relative position to other agents j ∈ Ni, 0 /∈ Ni, the

proof will be a bit more complicated. The time derivative of the Lyapunov candidate will be ˙ V = X i ˙ ϕi = X i " ∂ϕi ∂qi ˙ qi+ X j∈Ni ∂ϕi ∂qj ˙ qj # = = X i " −K ∂ϕi ∂qi 2 − X j∈Ni K∂ϕi ∂qj ∂ϕj ∂qj # (5.6)

Like in previous sections it can be shown that all agents i converge to regions where

∂ϕi ∂qi

2

< and > 0 is arbitrarily small. To do this, the notation Pij = ∂ϕ_∂q_ji is used and (5.6) can then be written as

˙ V = −K   X i:P2 ii≥ P_ii2+ X j∈Ni PijPjj + X i:P2 ii< P_ii2+ X j∈Ni PijPjj  ≤ ≤ −K   X i:P2 ii≥ + X j∈Ni PijPjj + X i:P2 ii< X j∈Ni PijPjj  

It can be shown that given a bounded workspace, −π < θ ≤ π, γi > γmin> 0

(32)

5. Connectivity constrained formation control 5.2. Analysis |P_ijPjj| ≤ σ1 if |Pij| ≤ σ2 ∧ |Pjj| ≤ σ3 ∧ σ2σ3 ≤ σ1 |P_ij| = γ_ik+ Gi −1/k−1 Gi∇jγi− γi k∇jGi ≤ ≤ βij γ2 min βij|qi− qj| + γi k βij2βij3|qi− qj| +

+βij1βij3|qi− qj| + βij1βij2|(θi− θj)(−∇jθj)|

≤ ≤ σ2 if k ≥ max j∈Ni γi

βij2βij3+ βij1βij3 |qi− qj| + βij1βij2

θ_i− θ_j −∇_jθj min j∈Ni σ2γmin2 βij − β_ij|q_i− q_j| (5.7) Since k > 0, this also defines a lower bound on γmin which is

γmin > v u u t max l∈Nj β_jlβjl|qj− ql| σ3 = v u u t max l∈Nj Gj|qj− ql| σ3 (5.10)

The limitations on the workspace ensures that the bounds presented above are finite and greater than zero.

(ii) X j∈Ni PijPjj ≥ −ρ2 < 0 if max j∈Ni |PijPjj| ≤ ρ2 kNik ≡ σ4

This will lead to lower bounds on k and γmin that are equivalent to

the ones given by (5.7)-(5.10) in (i).

(33)

5. Connectivity constrained formation control 5.3. Simulations

Assuming that the bounds derived in (i) and (ii) are fulfilled and that there exists at least one agent i such that P_ii2 ≥ the resulting bound on ˙V becomes

˙

V ≤ −K ρ1− (N − 1)2ρ2 < 0

if 0 < (N − 1)2ρ2< ρ1 (5.11)

˙

V = 0 only for Pii= Pij = Pjj = 0 ∀i, j

(5.11) then implies that P_ii2 converges to arbitrarily small for all agents i according to LaSalle’s invariance principle [13]. With going to zero this means that the system will converge to the desired destinations, thus defines the arbitrarily small region around the desired destinations.

5.3 Simulations

The results of the simulations of the controller (5.3) for the fingers will be shown in both two and three dimensions. The case that is considered is the one where the allowed area of an agent is only defined by its relation to the hand.

5.3.1 Two dimensions

In two dimensions, the allowed area for each finger is a segment of a circle sector that is defined by its minimum and maximum radius and by the max-imum angle span. It will be illustrated by blue lines in all of the simulation plots. The agents will be denoted by colored circles, and the hand is repre-sented by a blue arrow with the tip in the hand’s position and pointing in the direction of the orientation of the hand.

Since there are three different constraints, three simulation results will be presented that show connectedness in the limits corresponding to each con-straint.

To begin with, Figure 8 shows that each finger will stay a distance dmin =

0.01 from the hand. The starting positions, denoted by blue circles and the desired destinations, denoted by blue x’s, have been 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 8. As can be seen in the figure, the agents do

not enter this forbidden area but their trajectories instead follow the arch shaped boundary.

To show that the other two constraints will hold, it is necessary to define the desired destination outside the allowed area, thus violating the assumption that was made in section 3.3. The fingers will then stop when they have reached the limit of the active constraint and not converge to their destina-tions.

For the constraint on the maximum distance, this is shown in Figure 9 with dmax = 0.02. The corresponding result for the constraint on the angle is

(34)

5. Connectivity constrained formation control 5.3. Simulations −0.02 −0.015 −0.01 −0.005 0 0.005 0.01 0.015 0.02 −0.01 −0.005 0 0.005 0.01 0.015 0.02 x y

Figure 8: Simulation showing that the constraint on the minimum distance is fulfilled for all fingers

−0.02 −0.01 0 0.01 0.02 −0.01 −0.005 0 0.005 0.01 0.015 0.02 0.025 x y

Figure 9: Simulation showing that the constraint on the maximum distance is fulfilled for all fingers

5.3.2 Three dimensions

In three dimensions, the allowed area for each finger is a segment of a cone that is defined by its minimum and maximum radius and by the maximum aperture. It will be illustrated by blue grids in all of the simulation plots. In three dimensions only one agent will be shown for simplicity. This agent will be denoted by red circles and the hand is represented by a blue cone with the tip in the hand’s position and pointing in the direction of the hand’s

(35)

5. Connectivity constrained formation control 5.3. Simulations −0.02 −0.015 −0.01 −0.005 0 0.005 0.01 0.015 0.02 −0.01 −0.005 0 0.005 0.01 0.015 0.02 x y

Figure 10: Simulation showing that the constraint on the maximum angle is fulfilled for all fingers

orientation. The desired destination of the agent is denoted by a blue x. Like in the previous section there are three constraints to verify. Figure 11 shows the constraint on the minimum distance, Figure 12 shows the constraint on the maximum distance and Figure 13 shows the constraint on the maximum angle in the same way as for the two-dimensional case. Note that in Figure 13 the agent first goes towards the goal on a straight line, but when it reaches the maximum angle it changes direction and moves along the boundary to minimize the distance to the goal.

(36)

5. Connectivity constrained formation control 5.3. Simulations

Figure 12: Simulation showing that the constraint on the maximum distance is fulfilled in three dimensions

Figure 13: Simulation showing that the constraint on the maximum angle is fulfilled in three dimensions

(37)

6 Summary

In the following sections the thesis will be summarized and some possible future research interests in this area will be discussed. There will also be some comments on the planned demonstration.

6.1 Demonstration

The strategy that was chosen for demonstration was the decentralized con-troller of the non-holonomic two-agent system presented in Section 4. The task was the same as the one shown in the simulations in Section 4.3, i.e. enclosing of an object in a plane so the two-dimensional controller could be used without modification.

However, the dual arm on which the controller was going to be implemented had a sudden malfunction that could not be fixed before the deadline of the thesis. Therefore, no demonstration of the results in the thesis could be made before the writing of this report.

6.2 Conclusions

The first task of this thesis was to investigate wether it would be possible to extend already existing centralized controllers for non-holonomic multi-agent formation navigation with collision avoidance to the decentralized case. Two different centralized control strategies were used and they were both ex-tended to the decentralized case. For simplicity the modification was made for the two agent case, but while deriving the proof it was shown that the new strategies were valid also for multiple agents. It was shown that these control strategies both made the system converge to an arbitrarily small area around the desired destination, including position and orientation. However, the result could only be confirmed in simulations for one of the controllers. This was probably due to numerical issues that could not be resolved in the scope of this thesis.

The second task was to investigate wether connectivity constraints on a multi-agent system could be extended from only distance connectivity to also include angle connectivity. This was shown to be possible for a system with one central unit from which the angles associated with all the agents could be defined. For the case studied here, with the agents representing fingers, this could easily be done by regarding the hand as a central unit and define the angles from the position and orientation of 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.

6.3 Future work

(38)

6. Summary 6.3. Future work

this problem to see if these numerical issues that arose in implementation can somehow be overcome.

Another interesting problem related to the first part of this thesis is the way to construct the time-varying formation that the agents are controlled to keep. The formation used in the simulations in this thesis was adapted for that specific case. This may be able to be generalized and optimized with some further investigation. Also, the choosing of the tuning parameter for the controller could be investigated to find an optimal trajectory for the agents.

One problem with the second part of this thesis is the central unit that is needed to define the angles. Since one of the goals was to make all the con-trol strategies as decentralized as possible this is not an optimal solution. Therefore, this part could be extended by looking at general vector connec-tivity constraints using only relative positions.

For the specific finger case, improvement could be made by further work on the definition of the allowed area for each finger to make the resemblance to a real human hand more accurate.

It could also be interesting to modify this controller to a non-holonomic agent approach. Depending on the construction of the robot the movement of the fingers could be constrained in different ways and it is possible that a non-holonomic controller would be more suitable in some cases.

(39)

References

[1] M. Egerstedt and X. Hu. Formation constrained multi-agent control. Robotics and Automation, IEEE Transactions on, 17(6):947–951, 2002. ISSN 1042-296X.

[2] R. Olfati-Saber, J.A. Fax, and R.M. Murray. Consensus and coopera-tion in networked multi-agent systems. Proceedings of the IEEE, 95(1): 215–233, 2007. ISSN 0018-9219.

[3] D.V. Dimarogonas and K.J. Kyriakopoulos. A connection between for-mation infeasibility and velocity alignment in kinematic multi-agent systems. Automatica, 44(10):2648–2654, 2008. ISSN 0005-1098.

[4] D.V. Dimarogonas and K.H. Johansson. Bounded control of network connectivity in multi-agent systems. Control Theory and Applications, IET, 4(8):1330–1338, 2010. ISSN 1751-8644.

[5] H.G. Tanner, S.G. Loizou, and K.J. Kyriakopoulos. Nonholonomic stabilization with collision avoidance for mobile robots. In Intelli-gent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ Inter-national Conference on, volume 3, pages 1220–1225. IEEE, 2002. ISBN 0780366123.

[6] S.G. Loizou and K.J. Kyriakopoulos. Closed Loop Navigation for Multi-ple Non-Holonomic Vehicles. In Robotics and Automation, 2003. IEEE International Conference on, pages 420–425. IEEE, 2003.

[7] H.G. Tanner, S.G. Loizou, and K.J. Kyriakopoulos. Nonholonomic navigation and control of cooperating mobile manipulators. Robotics and Automation, IEEE Transactions on, 19(1):53–64, 2003. ISSN 1042-296X.

[8] S.G. Loizou, D.V. Dimarogonas, and K.J. Kyriakopoulos. Decentral-ized feedback stabilization of multiple nonholonomic agents. In Robotics and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE Interna-tional Conference on, volume 3, pages 3012–3017. IEEE, 2005. ISBN 0780382323.

[9] D.V. Dimarogonas, S.G. Loizou, K.J. Kyriakopoulos, and M.M. Za-vlanos. A feedback stabilization and collision avoidance scheme for mul-tiple independent non-point agents. Automatica, 42(2):229–243, 2006. ISSN 0005-1098.

[10] Z. Kan, AP Dani, JM Shea, and WE Dixon. Ensuring network connec-tivity during formation control using a decentralized navigation func-tion. 2010.

(40)

REFERENCES REFERENCES

[12] H.G. Tanner and K.J. Kyriakopoulos. Nonholonomic motion planning for mobile manipulators. In Robotics and Automation, 2000. Proceed-ings. ICRA’00. IEEE International Conference on, volume 2, pages 1233–1238. IEEE, 2002. ISBN 0780358864.

[13] H.K. Khalil. Nonlinear systems. Prentice hall New Jersey, 3rd edition, 2002. ISBN 0130673897.

[14] Dimos V. Dimarogonas and Emilio Frazzoli. Analysis of decentralized potential field based multi-agent navigation via primal-dual lyapunov theory. In IEEE Conference on Decision Control, pages 1215–1220, 2010.

[15] D.V. Dimarogonas and K.J. Kyriakopoulos. On the rendezvous problem for multiple nonholonomic agents. Automatic Control, IEEE Transac-tions on, 52(5):916–922, 2007.

Modeling and Control of Dual Arm Robotic Manipulators