Shared invariance control for constraint satisfaction in multi-robot systems

Abstract

In systems involving multiple intelligent agents, e.g. multi-robot systems, the satisfaction of environmental, inter-agent, and task constraints is essential to ensure safe and successful task execution. This requires a constraint enforcing control scheme, which is able to allocate and distribute the required evasive control actions adequately among the agents, ideally according to the role of the agents or the importance of the executed tasks. In this work, we propose a shared invariance control scheme in combination with a suitable agent prioritization to control multiple agents safely and reliably. Based on the projection of the constraints into the input spaces of the individual agents using input–output linearization, shared invariance control determines constraint enforcing control inputs and facilitates implementation in a distributed manner. In order to allow for shared evasive actions, the control approach introduces weighting factors derived from a two-stage prioritization scheme, which allots the weights according to a variety of factors such as a fixed task priority, the number of constraints affecting each agent or a manipulability measure. The proposed control scheme is proven to guarantee constraint satisfaction. The approach is illustrated in simulations and an experimental evaluation on a dual-arm robotic platform.

Keywords

Invariance control human–robot interaction multi-robot systems collision avoidance agent prioritization safety real-time systems motion control

1. Introduction

The simultaneous employment of multiple robotic systems to relieve and aid humans proves challenging as each robot need to execute its assigned task while limiting their affect on the actions of others and ensuring the safe interaction with other robots, humans, and the environment (Lasota et al., 2017). Exemplary applications are found in manufacturing and logistics, where cooperating manipulators take over strenuous tasks. Other application domains include search and rescue, where mobile robotic systems are deployed to keep humans safe from dangerous environments, elderly assistance, where robotic systems are introduced to household environments, and autonomously driving cars (Saleh et al., 2013). In order to ensure successful task execution, the satisfaction of environmental constraints and of constraints between multiple agents is essential. Furthermore, careful role allocation as introduced by Mörtl et al. (2012) and an assessment of the risk related to certain actions as investigated by Medina Hernández et al. (2013) is required to allocate suitable actions to agents. Naturally, the tasks carried out by the agents may be of different importance. Rescuing an injured person, for example, should be rated more important than scouting the environment, and getting to the docking station if the energy is running low should be prior to a cleaning task. Hence, to allow the execution of the more important task, other agents should clear the path and take over the main role in avoiding constraint violations.

This creates the necessity for a prioritization scheme for multiple agents, which are constrained by the environment and mutually disturb each other’s task execution. In addition, a priority-based constraint satisfaction algorithm is required to maintain the prioritization of the agents. It should enable agents with high priorities to carry on undisturbed from other agents. At the same time, agents with low priority should never be in a situation where they are trapped by other agents and unable to avoid constraint violations. In this article, we propose a novel invariance control approach for shared constraint satisfaction in multi-robot systems. The proposed scheme guarantees the satisfaction of environmental and inter-agent constraints while distributing the control effort between the agents according to their priorities and avoiding trap situations.

1.1. Related work

Constraint satisfaction is achieved by various control schemes. Methods from collision-free navigation such as virtual wall rendering by Gillespie and Cutkosky (1996), potential fields by Rimon and Koditschek (1992), dynamic windows by Fox et al. (1997), and virtual fixtures by Rosenberg (1993) are widely used in robotic applications, but are unable to provide constraint satisfaction guarantees for high inertias. Approaches from constrained control such as reference governors by Gilbert and Kolmanovsky (2002), model predictive control (MPC) by Mayne et al. (2000), methods based on invariant and reachable sets by Blanchini (1999) and Akametalu et al. (2014), control barrier functions (CBFs) by Ames et al. (2014), Rauscher et al. (2016), and Xu (2018), and invariance control by Wolff and Buss (2004) and Kimmel and Hirche (2016) generally guarantee constraint satisfaction. These approaches do, however, not consider the option of sharing evasive actions between agents in multi-agent systems according to given priorities. This means that implementing them on each of the agents to enforce the constraints may lead to overly conservative behavior, which is disadvantageous especially for high numbers of constraints leading to narrow regions of admissible states as it may lead to infeasibility.

Rossmann (1996), Freund et al. (2001), and Freund and Rossman (2003) introduced Collision Avoidance in Real-time Environments (CARE) as a method for multi-agent collision avoidance, which, especially for complex shapes or structures, only achieves an approximate reaction in real-time and is thus not able to provide guarantees for constraint satisfaction. Cai et al. (2007) suggested another approach for multi-robot collision avoidance, which deals with multi-agent collision avoidance by stopping the lower-priority agents. As a consequence, the lower-priority agents may block the path. Hence, an approach in which these agents actively retreat is preferable. Alternatively, evasive actions may be distributed using task allocation models as discussed by Lianghong et al. (2015) and Maoudj et al. (2015). These models allow the scheduling of task on the basis of existing priorities. These approaches, however, do not allow the re-scheduling of tasks if priorities change over time.

Priority-based constraint satisfaction relies on a suitable prioritization scheme. Barthès et al. (2016) introduced a heuristic approach, which changes the priorities according to the circumstances and aims at mimicking human behavior. Static effects on the priority are distinguished from time-dependent ones and dependencies between tasks are considered. In traffic management for autonomous vehicles the order in which the vehicles cross an intersection is also determined via prioritization as discussed by Alonso et al. (2011). Hong et al. (2009), on the other hand, employ priorities in a message scheduling scheme for multi-agent systems. The priorities are derived from their importance and the message throughput. If the goal is to grant the agents access to a shared goal, the possibilities range from deterministic weighted round-robin scheduling as proposed by Katevenis et al. (1991) to probabilistic approaches such as the lottery approach presented by Tiwari et al. (2016). However, these methods do not ensure a prioritization that guarantees constraint satisfaction and avoids trapping agents with low priorities.

1.2. Contribution

In this work, we propose shared invariance control as a method to guarantee the satisfaction of environmental and inter-agent constraints while using weightings in the control derivation to divide the control effort between the agents. Shared invariance control allows for a distributed implementation on the agents by generating independent constrained optimization problems, which are derived using input/output (I/O) linearization with respect to the constraint functions. It is further formally proven to guarantee satisfaction of non-contradicting constraints. In order to allow for shared evasive actions, the control approach introduces weighting factors based on agent priorities, which are determined by a two-stage prioritization scheme. This scheme allots the priorities such that trap situations are resolved and a variety of factors such as a fixed task priority, the number of constraints affecting each agent or a manipulability measure may be included in the prioritization. The approach is illustrated in simulations and an experimental evaluation on two robotic manipulators.

1.3. Notation

By convention, vectors are denoted by bold small and matrices by bold capital characters. The Euclidean vector norm (2-norm) of a vector $x \in R^{n}$ is $| | x | |_{2} = \sqrt{x^{⊤} x}$ . The expression $x_{1} ⪯ x_{2}$ indicates the element-wise inequality of two vectors $x_{1}, x_{2} \in R^{n}$ . Low-order time derivatives are indicated by dots $\overset{\cdot}{x} = \frac{d x}{d t}$ , higher-order time derivatives by superscripts $x^{(k)} = \frac{d^{k} x}{d t^{k}}$ . First-order Lie derivatives, i.e., the directional derivative in direction of a vector $f$ or a matrix $G = [g_{1} \dots g_{m}]$ of a scalar function $h (x, z)$ with respect to the variables $x$ , are given by

L_{x f} h (x, z) = \frac{\partial h}{\partial x} f, L_{x G} h (x, z) = [\frac{\partial h}{\partial x} g_{1} \dots \frac{\partial h}{\partial x} g_{m}]

Lie derivatives of higher order $L_{x f}^{i} h (x)$ , $i > 1$ , are determined recursively. The set of $k$ times continuously differentiable functions $h : R^{n} \to R$ is denoted by $C^{k}$ .

2. Problem setting

We consider $n_{ag} \in N$ nonlinear control affine systems

{\overset{\cdot}{x}}_{j} = f_{j} (x_{j}) + G_{j} (x_{j}) u_{j}

(1)

Each system $j \in N_{ag}$ with $N_{ag} = {1, \dots, n_{ag}}$ has an input $u_{j} \in R^{m_{j}}$ and a state $x_{j} \in X_{j} \subseteq R^{n_{j}}$ . The functions $G_{j} = [g_{j, 1} \dots g_{j, m_{j}}]$ with $G_{j} : R^{n_{j}} \to R^{n_{j}} \times R^{m_{j}}$ and $f_{j} : R^{n_{j}} \to R^{n_{j}}$ are sufficiently smooth (matrix) vector fields.

Remark 1. The generalized dynamics of robotic systems are determined by

M_{q} (q) \overset{\cdot\cdot}{q} + C_{q} (q, \overset{\cdot}{q}) \overset{\cdot}{q} + g_{q} (q) = τ

(2)

with the generalized coordinates $q \in R^{n_{q}}$ , the mass matrix $M_{q} (q) \in R^{n_{q} \times n_{q}}$ , the Coriolis and centripetal forces $C_{q} (q, \overset{\cdot}{q}) \overset{\cdot}{q} \in R^{n_{q}}$ , gravitational torques $g_{q} (q) \in R^{n_{q}}$ , and torque input $τ \in R^{n_{q}}$ . By defining $x = [q^{⊤}, {\overset{\cdot}{q}}^{⊤}]^{⊤}$ as the state, these may be transformed into the control affine form (1). As the addition of various forms of low-level control preserves the control affine form, which is discussed by, e.g., Kimmel and Hirche (2017), we use the general formulation (1) for generality in the following.

Based on the constraint parameters $η \in N_{η} \subseteq R^{n_{η}}$ (their dynamic properties are discussed at end of this section), and the concatenated state $x = [x_{1}^{⊤} x_{n_{ag}}^{⊤}]^{⊤} \in X \subseteq R^{\sum n_{j}}$ of the multi-agent system, constraints on states or outputs are defined.

Definition 1. Each constraint function $y_{c, i}$ with

y_{c, i} (t) = h_{c, i} (x, η)

(3)

encodes a single constraint $i$ and is defined to equal zero for states on the constraint, is negative for admissible states, and positive if the constraint is violated.

All constraints are gathered in the set of constraints

K = {i \in N | 1 \leq i \leq l}

(4)

where $l \in N$ may be an arbitrarily high number. Furthermore, they define the admissible set $H (η)$

H (η) = {x \in R^{Σ n_{j}} | h_{c, i} (x, η) \leq 0, \forall 1 \leq i \leq l}

(5)

As the control derivation introduced in the following section relies on I/O linearization, we make the following smoothness assumptions on the output functions and the parameters.

Assumption 1. For all $i \in K$ , there exist constants $r_{i} \in N$ such that the following conditions are fulfilled:

for any continuous input, $y_{c, i} (t)$ is $r_{i}$ times continuously differentiable with respect to time, i.e., $y_{c, i} \in C^{r_{i}}$ , $\forall i \in K$ ;

each $h_{c, i} (x, η)$ fulfills $\forall η \in N_{η}, x \in X$

L_{x_{j}} G_{j} L_{x_{j} f_{j}}^{j} h_{c, i} (x, η) = 0 \forall 0 \leq j < r_{i} - 1

L_{x_{j} G_{j}} L_{x_{j} f_{j}}^{r_{i} - 1} h_{c, i} (x, η) \neq 0

$η_{j} \in C^{r_{\max}}$ , $1 \leq j \leq n_{η}$ with

r_{\max} = max_{i \in K} (r_{i})

$H (η)$ is non-empty, i.e., $H (η) \neq \emptyset$ .

The assumption, if not already fulfilled by appropriate constraint design, may be achieved by a sufficiently smooth approximation of the constraints and the parameter variation. In robotic systems, for example, the assumptions on the constraint functions are fulfilled if the constraints are non-contradicting and the robotic system has full manipulability within the admissible set, which may, for example, be achieved by introducing constraints that enforce limits on the manipulability measure.

Finally, in order to fully describe the constraints and their derivatives, it is necessary to know the parameter dynamics. Hence, we introduce the vector $x_{η} \in R^{(r_{\max} + 1) n_{η}}$

x_{η} = {[\begin{matrix} η^{⊤} & {\overset{\cdot}{η}}^{⊤} & {η^{(r_{\max} - 1)}}^{⊤} & {η^{(r_{\max})}}^{⊤} \end{matrix}]}^{⊤}

which concatenates the dynamic parameters $η$ and their time derivatives up to the $r_{\max}$ th order. Using $x_{η}$ as the state vector, the dynamics of the parameters may then be modeled as an integrator chain with $r_{\max}$ integrators and the input $u_{η} \in R^{n_{η}}$ .

{\overset{\cdot}{x}}_{η} = \underset{f_{η} (x_{η})}{\underset{︸}{[\begin{matrix} 0_{n_{η}} & I_{n_{η}} & 0_{n_{η}} \\ ⋮ & ⋱ & ⋱ & ⋮ \\ 0_{n_{η}} & 0_{n_{η}} & I_{n_{η}} \\ 0_{n_{η}} & 0_{n_{η}} & 0_{n_{η}} \end{matrix}] x_{η}}} + [\begin{matrix} 0_{n_{η}} \\ ⋮ \\ 0_{n_{η}} \\ I_{n_{η}} \end{matrix}] u_{η}

(6)

Note that, applying I/O-linearization to any vector of dynamic parameters $η$ fulfilling Assumption 1 results in (6). Hence, the proposed parameter model does not impose any further restrictions.

Remark 2. Constant parameters $η$ represent static constraints. This is achieved by $x_{η} (t_{0}) = [η^{⊤}, 0^{⊤}, \dots, 0^{⊤}]^{⊤}$ and $u_{η} = 0$ in (6).

3. Invariance control

Invariance control for single agents, i.e., $x = x_{j}$ , and guaranteed constraint satisfaction is introduced by Wolff and Buss (2005) and the application to robotic systems is investigated by Kimmel and Hirche (2017). This section introduces the most important results. For a single agent, the dynamics are given by

\overset{\cdot}{x} = f (x) + G (x) u

(7)

As discussed in Remark 1, in general, the dynamics of robotic systems may be transformed into the given form. Figure 1 illustrates how invariance control is added to existing control loops with system and nominal control. The nominal control law is designed to fulfill task specifications such as tracking a desired trajectory, stability, and performance without taking any constraints into account. Note that nominal control design is not within the scope of this work. Constraints are enforced by the invariance control law by switching from nominal $u_{no} \in R^{m}$ to a corrective $u_{c} \in R^{m}$ control input whenever necessary.

Fig. 1.

Control structure with nominal control system and the addition of invariance control based on the constraint definition and the related dynamics.

The determination of corrective control relies on I/O linearization. It determines the effect of the control input $u$ on the change of the constraint functions (3) and yields

z_{i} = h_{c, i}^{(r_{i})} (x, x_{η}) = a_{i}^{⊤} (x, η) u + b_{i} (x, x_{η})

(8)

where

a_{i}^{⊤} (x, η) = L_{x G} L_{x f}^{r_{i} - 1} h_{c, i} (x, η)

b_{i} (x, x_{η}) = {(L_{x f} + L_{x_{η} f_{η}})}^{r_{i}} h_{c, i} (x, η),

where $z_{i}$ and $r_{i}$ are the pseudo-input of the resulting integrator chain and the relative degree, respectively. In robotic systems, relative degrees of one or two are most common as they correspond to position and velocity constraints. Kimmel and Hirche (2017) discussed the explicit expressions of $a_{i}^{⊤}$ and $b_{i}$ for such constraints. Note that the relative degree is well-defined by Assumption 1.

Remark 3. While Isidori (1995) argued that a well-defined vector relative degree is required for I/O-linearization, a well-defined relative degree suffices for invariance control. The system is not I/O linearized but instead I/O linearization is used to derive conditions on the control input that are then used in a constrained optimization.

Setting $z_{i} \leq γ_{i}$ with a constant design parameter $γ_{i} \in R^{-}$ , an upper bound on the future output values is given by

y_{c, i} (t + Δ t) \leq \underset{= p_{i} (x, x_{η}, γ_{i}, Δ t)}{\underset{︸}{\frac{Δ t^{r_{i}}}{r_{i}!} γ_{i} + \sum_{j = 0}^{r_{i} - 1} \frac{Δ t^{j}}{j!} h_{c, i}^{(j)} (x, x_{η})}}

(9)

where $Δ t \geq 0$ . The derived maximum output value is defined by the invariance function (Wolff and Buss, 2005)

Φ_{i} (x, x_{η}, γ_{i}) = max_{Δ t \geq 0} p_{i} (x, x_{η}, γ_{i}, Δ t)

(10)

which, in turn, yields the invariant set

G (x_{η}, γ) = {x \in R^{n} | Φ_{i} (x, x_{η}, γ_{i}) \leq 0 \forall i \in K}

(11)

with $γ = [γ_{1} γ_{l}]^{⊤}$ . Note that $G (x_{η}, γ) \subseteq H (η)$ holds for all $γ_{i} \leq 0$ as shown by Kimmel and Hirche (2017) and, thus, the invariant set solely contains admissible states. Furthermore, it extends the admissible set by taking system and constraint dynamics into account. Hence, it explicitly considers the inertia in mechanical systems such as robots.

The constraints, which are in danger of being violated are indicated by a non-negative value of the invariance function and are collected in the set of active constraints

K_{act} (x, x_{η}, γ) = {i \in K | Φ_{i} (x, x_{η}, γ_{i}) \geq 0}

(12)

Combining (8) and $z_{i} \leq γ_{i}$ yields a set of constraint admissible inputs.

M = {u | a_{i}^{⊤} (x, η) u + b_{i} (x, x_{η}) \leq γ_{i} \forall i \in K_{act}}

(13)

Corrective control $u_{c} \in R^{m}$ is then determined by the following theorem.

Theorem 1. (Kimmel and Hirche, 2017). Let the system and constraints be given by (7) and (3), respectively. Let Assumption 1 hold and let $M$ from (13) be non-empty. Then, the control input $u = u_{c}$ derived from

u_{c} = \underset{u}{argmin} | | u - u_{no} | |_{2}^{2}

(14)

s . t . a_{i}^{⊤} (x, η) u + b_{i} (x, x_{η}) \leq γ_{i} \forall i \in K_{act} (x, x_{η}, γ)

with $γ_{i} < 0$ renders the system state controlled positively invariant with respect to the invariant set $G (x_{η}, γ)$ . Furthermore, the optimization problem is convex.

This theorem rephrases the content of Theorem 1 in Kimmel and Hirche (2017) to fit the problem setting at hand.

As the new system input renders the state controlled positively invariant by Kimmel and Hirche (2017) with respect to the invariant set (11), which is a subset of the admissible set, any constraint violation is avoided and the optimization ensures that $u = u_{no}$ holds if no constraints are active.

Remark 4. Kimmel and Hirche (2017) stated that boundedness of the tracking error is guaranteed for all combinations of stabilizing nominal control laws and constraints, if an additional boundedness condition is derived using Lyapunov methods and included in the optimization (14). As this work focuses on constraint enforcement, the boundedness condition is omitted for briefness. It may, however, be included without any effect on the constraint enforcement.

4. Shared invariance control

Naturally, invariance control as introduced in the previous section may be implemented on multiple agents simultaneously to enforce environmental and inter-agent constraints by using a dynamic model of the other agents in the constraint parameterization. The approach leads, however, to a rather restrictive behavior of the agents as each one tries to avoid collisions at any cost which is especially disadvantageous for robots in narrow environments. Alternatively, the constraint may be enforced via invariance control only by a subset of the involved agents, which may lead to the optimization being infeasible if the evading agent is unable to act, e.g., due to being trapped between constraints.

These drawbacks are resolved in this section by introducing an invariance control approach for multi-agent systems based on the following assumption.

Assumption 2. The I/O linearization of each constraint $y_{c, i}$ , $i \in K$ , with respect to the input $u_{j}$ of each agent $j \in N_{ag}$ yields the same relative degree $r_{i}$ for all agents $j$ .

This assumption is imposed for convenience of notation and poses no additional restrictions on the systems, because the relative degree may be increased, if necessary, by augmented invariance control as introduced for general systems by Kimmel et al. (2016) and for robotic systems by Jähne and Hirche (2017). Note that for robotic systems with position constraints, for example, the assumption is naturally fulfilled as the relative degree equals two for all agents.

Building on the naturally shared actions achieved by a centralized implementation, shared invariance control allows for a distributed implementation based on given priorities.

4.1. Centralized implementation

Concatenating states and inputs of the $n_{ag}$ systems (1) yields the centralized system description (7) with

x = [\begin{matrix} x_{1} \\ ⋮ \\ x_{n_{ag}} \end{matrix}], u = [\begin{matrix} u_{1} \\ ⋮ \\ u_{n_{ag}} \end{matrix}], f (x) = [\begin{matrix} f_{1} (x_{1}) \\ ⋮ \\ f_{n_{ag}} (x_{n_{ag}}) \end{matrix}]

G (x) = [\begin{matrix} G_{1} (x_{1}) & 0 & 0 \\ 0 & ⋱ & 0 \\ 0 & 0 & G_{n_{ag}} (x_{n_{ag}}) \end{matrix}]

(15)

Thus, the centralized system may be considered as a single agent, allowing invariance control to be derived analog to the previous section. This requires the involved agents to be controlled by a single, centralized controller, which has access to the agent states, dynamic information and constraint parameters. The structure for two agents is illustrated in Figure 2.

Fig. 2.

Central control architecture for inter-agent constraints of two agents.

Using Theorem 1 and the centralized system description, a constraint enforcing control input is determined. The control action is automatically shared between the agents as the left side of the optimization condition (13)

a_{i}^{⊤} (x, η) u + b_{i} (x, x_{η}) = {[\begin{matrix} {α_{i, 1}}^{⊤} \\ ⋮ \\ {α_{i, n_{ag}}}^{⊤} \end{matrix}]}^{⊤} [\begin{matrix} u_{1} \\ ⋮ \\ u_{n_{ag}} \end{matrix}] + b_{i} (x, x_{η})

= \sum_{j \in N_{ag}} {α_{i, j}}^{⊤} u_{j} + b_{i} (x, x_{η})

(16)

with

{α_{i, j}}^{⊤} = L_{x_{j} G_{j}} L_{x_{j} f_{j}}^{r_{i} - 1} h_{c, i} (x, η)

(17)

b_{i} (x, x_{η}) = {(\sum_{j \in N_{ag}} L_{x_{j} f_{j}} + L_{x_{η} f_{η}})}^{r_{i}} h_{c, i} (x, η)

(18)

includes the input $u_{j}$ of each agent system affected by the constraint due to Assumption 2. Hence, all agents react to the constraint, the evasive effort is partitioned based on the agent dynamics and constraint satisfaction is guaranteed by Theorem 1.

In this rather straightforward approach, the agents have to be controlled by a common central controller even if no inter-agent constraints are active. For more independence of the agents, a distributed implementation may, however, be preferred. Furthermore, any prioritization of the agents is lost, because the priorities have no effect on the allocation of the evasive effort.

4.2. Distributed implementation

Introducing shared invariance control, we aim at a distributed implementation as depicted for two agents in Figure 3, where each agent has its own control loop of system dynamics and control law. Each agent is assumed to have knowledge of the constraint parameters $x_{η}$ and the dynamic information of the other agents either through explicit communication or via observation. In addition each agent is assigned a priority.

Fig. 3.

Distributed control architecture of shared invariance control for inter-agent constraints of two agents.

Definition 2. The priority $c_{i}$ of an agent $i \in N_{ag}$ fulfills

c_{i} \in] 0, c_{\max}]

with a maximum priority value $c_{\max} \in R^{+}$ .

A framework for how to assign such priorities is introduced in the following section. For now, the priorities are assumed to be given.

The goal of shared invariance control is to share the evasive control action for constraint satisfaction between the agents according to their priorities, i.e., high effort for low-priority agents and low or no effort for high-priority agents. This is achieved by partitioning the agents into different groups based on their priority.

Definition 3. An agent priority community $S_{j}$ consists of the agents, whose priorities $c_{i}$ fulfill $d_{j} < c_{i} \leq d_{j + 1}$ with positive constants $d_{j}, d_{j + 1} > 0$ and is defined as

S_{j} = {i \in N_{ag} | d_{j} < c_{i} \leq d_{j + 1}, d_{j}, d_{j + 1} > 0}

Note that the agent priority communities have to be chosen such that each agent belongs to exactly one community. As only the agents with the lowest priority should actively enforce the constraints and share the effort, it is necessary to find these agents and the related priority community.

Definition 4. The minimal active priority community $S_{j_{\min}}$ for constraint $i$ denotes the lowest-priority community, which contains agents that are affected by the constraint. The community $S_{j_{\min}}$ fulfills the following two properties:

\exists j \in S_{j_{\min}} : {α_{i, j}}^{⊤} \neq 0

\forall k < j_{\min}, j \in S_{k} : {α_{i, j}}^{⊤} = 0

Using the minimal agent priority community, it is possible to find those agents, which should actively pursue constraint enforcement, i.e., those agents with the lowest priorities, which are affected by a constraint.

Definition 5. The set of active agents $A_{i}$ for constraint $i$ denotes those agents in the agent priority community with the lowest priorities, which are affected by the constraint

A_{i} = {j \in S_{j_{\min}} | {α_{i, j}}^{⊤} \neq 0}

The remaining agents, which are not in the set of active agents, are either not affected by the constraint at all, independent from their priority, or have high priorities and should therefore not carry out any evasive action. These agents are collected in the set of inactive agents.

Definition 6. The set of inactive agents $J_{i}$ for constraint $i$ denotes the complementary set to the set of active agents

J_{i} = N_{ag} \ A_{i}

Using these considerations, it is possible to introduce shared invariance control for multi-agent systems.

Theorem 2. Let the agent dynamics and constraints be given by (1) and (3), respectively. Let Assumptions 1–2 hold. Then, if the set of admissible control values

M_{j} = {u_{j} | {α_{i, j}}^{⊤} u_{j} + w_{i, j} (c) (\sum_{k \in J_{i}} {α_{i, k}}^{⊤} u_{k} + b_{i} (x, x_{η}))

\leq w_{i, j} (c) γ_{i} \forall i \in K_{act} (x, x_{η}, γ), j \in A_{i}}

(19)

with $γ_{i} < 0$ , $c = [c_{1} \dots c_{n_{ag}}]^{⊤}$ , ${α_{i, j}}^{⊤}$ , $b_{i} (x, x_{η})$ according to (17), (18), and weights $w_{i, j} (c) : [0, c_{\max}]^{n_{ag}} \to [0, 1]$ fulfilling

\sum_{j \in A_{i}} w_{i, j} (c) = 1

(20)

is non-empty for each agent $j \in N_{ag}$ and all $t \geq t_{0}$ , the optimization

u_{c, j} = \underset{u_{j}}{argmin} | | u_{j} - u_{no, j} | |_{2}^{2}

(21)

s . t . u_{j} \in M_{j}

(22)

yields control inputs $u_{j} = u_{c, j}$ avoiding any constraint violation.

Proof. See the appendix. □

A solution exists if the constraints on each agent are non-contradicting, i.e., if admissible control values in $M_{j}$ exist for each agent or, more generally, if the set $M$ from (13) is non-empty for the centralized system (16). Furthermore, full controllability, i.e., manipulability of robotic systems, is essential for the system to react to the constraints. Naturally, especially in systems with many agents, it is not immediately clear whether constraints are always non-contradicting. However, both controllability and the existence of admissible control inputs may be ensured by including these criteria in the determination of priorities, as discussed in the following section. Note that the theorem provides only one way to share the evasive control action between the lowest-priority agents.

Remark 5. The way of dividing the optimization condition between the agents is not unique. In general, the optimization conditions (13) may be arbitrarily shared among the agents as long as the conditions on the input (19) sum up to the centralized invariance condition (16). This is especially useful to reduce the communication between agents as parts of the dynamics do not need to be known by the other agents.

As the control actions of the agents just suffice to fulfill the invariance condition, overly restrictive reactions to the constraints, which would occur if the constraints are fully enforced by all agents, are avoided. This means that the approach lends itself to the implementation on robotic systems acting in narrow environments, where each robot has to satisfy the constraints imposed by the environment as well as by other agents.

Corollary 1. The control input determined by Theorem 2 enforces environmental constraints $h_{i} (x_{j}, η)$ on agent $j$ independently from the other agents.

Proof. See the appendix. □

Theorem 2 does not give any indication as to how the weighting factors $w_{i, j} (c)$ are determined as a function of the priority vector $c = [c_{1} \dots c_{n_{ag}}]^{⊤}$ . The most straightforward definition of the weight function is given by

w_{i, j} (c) = \frac{\frac{1}{c_{j}}}{\sum_{j \in A_{i}} \frac{1}{c_{j}}}

(23)

By inverting the priorities the agents with the lowest priority in the set of active agents are allocated the highest weighting factor, which means that they are affected most by the constraint. Normalizing the inverted priority ensures that condition (20) is fulfilled. Naturally, other weighting functions, possibly with a saturation may be used as well. Note that shared invariance control exhibits some useful properties.

Corollary 2. The optimization problem (21) is convex.

Proof. See the appendix. □

Corollary 3. Let the state values of all agents and the inputs of the agents in the set of inactive agents $j \in J_{i}$ be known to the active agents $j \in A_{i}$ if any of these agents share a constraint with the active agent $j$ . Then, the constrained optimization problems (14) for shared invariance control may be solved in a distributed manner.

Proof. See the appendix. □

As the optimization may be solved in a distributed fashion, this allows for the desired control structure illustrated in Figure 3, where each agent determines its own control input based on its priority. Hence, the cost of solving the optimization solely depends on the number of constraints and not on the number of agents and due to the convexity of the optimization problem by Corollary 2, efficient solvers may be used thus allowing the application on real-time systems with fast sampling times.

Remark 6. As solely active constraints affect the optimization, the agents only need the know the states (and inputs) of those agents, with whom they share an active constraint. In order to determine which constraints are active, the agents do, nevertheless, need to know the inputs/states of all agents, with whom they share any constraint leading to a large communication cost. However, if the agents are able to conservatively estimate the required knowledge, e.g., from known state limits or from sensor data, the communication cost may be reduced by estimating which constraints most definitely will not be active.

5. Agent prioritization

Note that Theorem 2 does not give information on when the optimization (21) is feasible for all agents. Instead, the prioritization scheme is designed to account for the feasibility, because there might occur situations in which the agents trap each other thus leading to infeasibility. Figure 4 shows an exemplary situation with four spherical agents, which may represent mobile robotic systems, with fixed priorities in two-dimensional space. The innermost agent holds a static position, while the outer agents move in the direction indicated by the solid arrows. In order to avoid collisions, each pair of agents defines one inter-agent constraint. As they are in different agent priority communities, the lower-prioritized agent takes the entire evasive action as indicated by the dashed arrows. The inner agent is in the lowest-priority community and has to avoid the collision with all agents. Eventually, the outer agents crash into the inner one as they leave no space to escape this trap situation.

Fig. 4.

Four-agent scenario with different fixed priorities.

5.1. Two-stage prioritization scheme

In order to resolve such trap situations, we propose a prioritization scheme composed of two stages as depicted in Figure 5. The first stage assigns a priority community according to Definition 3. The priority community decides which agents actively enforce constraints. This stage ensures that the optimization problems remain feasible. Note that as the situation may change over time, the priority assignment needs to be monitored constantly. The second stage takes care of the priority fine-tuning. Taking individual factors into account the priorities of agents within one priority community are assigned to share the effort accordingly.

Fig. 5.

Two-stage prioritization scheme.

5.2. Priority community assignment

The goal of assigning agents to a priority community is to ensure the feasibility of the optimization in Theorem 2. This requires a detection of trap situations leading to infeasibility and a trap handling scheme to adjust the priorities.

5.2.1. Trap detection

Before introducing trap detection, we define when an agent is considered trapped.

Definition 7. An agent is trapped if the optimization in Theorem 2 is infeasible and free otherwise.

More formally, if the set

M_{j} = {u_{j} | A_{j} u_{j} ⪯ β_{j}}

(24)

β_{j} = [\begin{matrix} w_{i_{1}, j} (c) (γ_{i_{1}} - \sum_{j \in J_{i_{1}}} {α_{i_{1}, j}}^{⊤} u_{j} + b_{i_{1}}) \\ ⋮ \\ w_{i_{j_{act}}, j} (c) (γ_{i_{j_{act}}} - \sum_{j \in J_{i_{j_{act}}}} {α_{i_{j_{act}}, j}}^{⊤} u_{j} + b_{i_{j_{act}}}) \end{matrix}]

A_{j} = [\begin{matrix} {α_{i_{1}, j}}^{⊤} \\ ⋮ \\ α_{i_{j_{act}}, j} \end{matrix}], i_{1}, \dots, i_{j_{act}} \in {i | i \in K_{act}, j \in A_{i}}

which considers all relevant constraints for agent $j$ , is empty, the optimization problem is infeasible and the agent is trapped. In order to determine whether $M_{j} = \emptyset$ , Farkas’ lemma is used.

Theorem 3. (Farkas’ lemma (Vanderbei, 2001: p.167)). The element-wise inequality $A_{j} u_{j} ⪯ β_{j}$ has no solution if and only if there exists a $z \in R^{j_{act}}$ fulfilling $z ⪰ 0$ element-wise such that $A_{j}^{⊤} z = 0$ and $β_{j}^{⊤} z < 0$ .

The conditions introduced in Farkas’ lemma may be evaluated by the minimization

min_{z} β_{j}^{⊤} z

s . t . A_{j}^{⊤} z = 0, z ⪰ 0

This minimization has a solution as $z = 0$ fulfills both minimization conditions. By applying computationally efficient algorithms from convex linear programming the minimization is solved. If a $z ⪰ 0$ exists for which the found minimum is negative, the conditions from Farkas’ lemma are fulfilled and agent $j$ is trapped. Otherwise, the minimum is $z = 0$ and the agent is free.

Naturally, trap situations may only be resolved if reducing the number of constraints is possible, i.e., if they are caused by inter-agent constraints. Therefore, we assume that the uncontrollable environmental constraints are such that an agent is at no time trapped solely by the environment.

5.2.2. Trap handling

Once all trapped agents are determined, the agents taking part in or being close to a trap situation are determined. For robotic systems with position constraints, this corresponds to finding all robots that are physically close or moving fast towards the trapped robot. More generally speaking, evaluating the invariance functions shows which agents may be considered close.

Definition 8. A nearly active constraint has a non-positive invariance function value fulfilling

0 > Φ_{i} (x, x_{η}, γ_{i}) \geq Φ_{th}

with a constant negative threshold $Φ_{th} \in R^{-}$ .

Using the nearly active constraints, an agent graph is established. The agents are nodes and those affected by the same nearly active constraints are connected via edges.

Definition 9. An emergency community $E$ is a set of agents connected by common nearly active inter-agent constraints, in which at least one of the agents is trapped.

Emergency communities are determined via graph search starting from trapped agents. Within an emergency community, priorities are reassigned to reinstate feasibility of the optimization (21).

Naturally, there is not a single solution to reassigning priorities to resolve trap situations. We suggest a priority community $S_{j}$ reassignment based on two criteria:

the severity of the trapping situation depending on the involved constraints;

the proximity to free agents.

The severity criterion is determined by the number of involved environmental constraints. In Figure 6, the severity increases from left (solely agents) to right (multiple environmentally constrained dimensions). The more severely an agent is trapped, the higher should be its priority community.

Fig. 6.

Trap situations of different severity with the most trapped agent restricted by (a) solely free agents , (b) free agents and a single wall , and (c) two walls and other trapped agents , which are in turn trapped by free agents and a single wall.

The proximity criterion is determined by the distance to free agents. Using the agent graph of the emergency community and starting at the free agents, which are sorted into the lowest-priority community, the priority increases for each step along an edge until all agents are assigned.

The trap situation is then resolved by Algorithm 1. By assigning free agents to low-priority communities, it removes constraints from the other agents, thus freeing more agents. These are in turn assigned to the higher-priority communities, which is repeated until all agents are free.

Algorithm 1 Priority Community (PC) Assignment

1: Determine all trapped agents

2: Determine emergency community E

3: while ∃ trapped agent do

4: Determine

S_{m}

: lowest PC containing trapped agent

5: Set agent

i \in S_{m} \forall i \in E

i \in S_{p}

p > m

6: Determine all trapped agents

7: for trapped agents

i \in S_{m}

8: Determine severity

se (i)

, i.e.,

i \in S_{se (i)}

9: Determine proximity

pr (i)

, i.e.,

i \in S_{pr (i)}

10: Reassign agents to new PC:

11: if Single trapped agent

i

then

12:

i \in S_{p}

with

p > m

13: else

14: for all pairs of trapped agents

i, j

15: if

se (i) = se (j)

pr (i) = pr (j)

then

16:

i, j \in S_{p}

with

p > m

17: else if

se (i) > se (j)

then

18:

i \in S_{p_{i}}, j \in S_{p_{j}}

with

p_{i} > p_{j} > m

19: else if

se (i) = se (j)

pr (i) > pr (j)

then

20:

i \in S_{p_{i}}, j \in S_{p_{j}}

with

p_{i} > p_{j} > m

21: else

22:

i \in S_{p_{i}}, j \in S_{p_{j}}

with

m < p_{i} < p_{j}

23: Determine all trapped agents

Theorem 4. Let the agent dynamics and constraints be given by (1) and (3), respectively. Let Assumptions 1–2 hold. Then, if the set

\begin{matrix} M_{j} = {u_{j} | α_{i, j} u_{j} + b_{i} \leq γ_{i} \forall i \in K_{act}, \\ where α_{i, k} \neq 0 for k = j, α_{i, k} = 0 \forall k \neq j} \end{matrix}

is non-empty for each agent $j$ and there is no set of agents $N \subseteq N_{ag}$ , such that

\begin{matrix} M_{N, j} = {u_{j} | α_{i, j} u_{j} + w_{i, j} b_{i} \leq w_{i, j} γ_{i} \forall i \in K_{act}, \\ where \exists k \in N such that α_{i, k} \neq 0 \\ and α_{i, k} = 0 \forall k \notin N} \end{matrix}

is empty for all agents $j \in N$ , Algorithm 1 converges.

Proof. See the appendix. □

In other words, the algorithm converges, if there is no set of constraints that mutually traps two or more agents, i.e., that would require removing one or more constraints to free the agents, and if an agent is never trapped solely by environmental constraints. Note that the algorithm will never need more steps than there are agents in the emergency community, but usually it will need considerably less.

The process is illustrated in the following example.

Example 1. Consider the exemplary trap situation with eight spherical agents with double integrator dynamics and one environmental constraint in two-dimensional space depicted in Figure 7(a). Initially, there are two trapped agents as the surrounding agents are in higher-priority communities. Applying Algorithm 7 (lines 1–5), the lowest-priority community containing a trapped agent is $S_{1}$ , meaning that all agents except the one in $S_{0}$ , are reassigned to $S_{1}$ . As a result, there are now three trapped agents as depicted in Figure 7(b). After finishing the first run of the while loop, the free agents remain in $S_{1}$ , while the previously trapped agents are assigned to $S_{2}$ as all are restricted by one environmental constraint and are one step from a free agent. Consequently there is still one trapped agent as illustrated in Figure 7(c) as it is not able to evade the outer two agents moving in. After conducting a second run of the while loop, the outer two agents, which are now free, remain in $S_{2}$ , while the middle agent is reassigned to $S_{3}$ . In this final configuration, shown in Figure 7(d), all agents are free and the priority community assignment is completed.

Fig. 7.

Trap situation with free and trapped agents with respective priority communities in different stages of Algorithm 1. All depicted agents belong to the same emergency community as their constraint outlines touch, meaning they share (nearly) active constraints.

All agents that are not part of an emergency community may be assigned either to arbitrary priority communities or preferably based on their priority shares.

5.3. Priority share assignment

In situations, which are not critical for the feasibility of the optimization problem, the priorities may be freely assigned. In this case, each agent $j$ has a priority $c_{j}$ that is calculated using $n_{c}$ priority criteria. The criteria coefficients $c_{j, i}$ , where $i \in {1, \dots, n_{c}}$ should be determined such that they take high values if the criterion is highly restrictive, such as for example low manipulability of the robotic system, or important and low values otherwise. The individual coefficients $c_{j, i}$ are then weighted depending on which criteria should be the most influential and combined to the agent priority

c_{j} = \sum_{i = 1}^{n_{c}} w_{j, i} \cdot c_{j, i}

(25)

with the number of criteria $n_{c} \in N$ and the related non-negative scalar weights $w_{j, i} \in R^{+}$ .

The proposed priority share calculation framework offers several advantages. It is flexible, scalable, and allows the calculation of the agent priorities in a distributed manner. Furthermore, adding new criteria for priority calculation is trivial.

Remark 7. While the priority share only depends on the individual agent characteristics and may therefore be computed in a distributed manner, the priority community assignment is preferably determined by a central computing unit as otherwise extensive communication between the agents would be required.

In the following, we illustrate the capabilities of the proposed prioritization framework in numerical examples and experiments.

6. Evaluation

The proposed trap handling and prioritization scheme using shared invariance control is tested in different scenarios in simulation as well as in experiments on robotic manipulators. For both the numerical examples and the experiments, control is implemented in Matlab/Simulink.

6.1. Agents in different priority communities

Starting with a rather straightforward example, we examine the case of two spherical agents in different priority communities with radius $r_{ag} \in R^{+}$ and double-integrator dynamics. Initially, the agents are arranged as depicted in Figure 8(a).

Fig. 8.

Setup and trajectories for two shared invariance controlled agents in different priority communities.

Both agents are nominally controlled by a proportional-derivative (PD) control law with respect to a desired position and the inter-agent constraint is given by

y_{c} = 2 r_{ag} - | | p_{ag 1} - p_{ag 2} | |_{2}

(26)

where $p_{ag 1}, p_{ag 2} \in R^{2}$ are the positions of the agents. The model parameters are provided in Table 1.

Table 1.

Parameters for evaluation of shared invariance control.

Agent radii	$r_{ag}$	0.2 m
Sampling time	$T_{A}$	0.001 s
Initial positions	$p_{ag, 0}$	$[\begin{matrix} - 1 \\ 0 \end{matrix}]$ , $[\begin{matrix} 1 \\ 0.1 \end{matrix}]$ m
Desired positions	$p_{ag, des}$	$[\begin{matrix} 1 \\ 0 \end{matrix}]$ , $[\begin{matrix} - 1 \\ 0.1 \end{matrix}]$
PD control	$K_{P}, K_{P}$	$10 \cdot I_{2} 1 / s^{2}$ , $7 \cdot I_{2} 1 / s^{2}$
Corrective control	$γ$	$- 20 m / s^{2}$

Without the constraint, both agents would follow a straight line towards their desired position. With the constraint and shared invariance control derived using Theorem 2, the agents adjust their actions to avoid constraint violations. Figure 8(b) and (c) depict the trajectories of the agents if the second agent is in a higher priority community. As expected, the second agent follows a straight trajectory towards its desired position whereas the first agent carries out an evasive motion. If the priority communities are reversed, the first agent goes straight towards the goal while the second agent makes the evasive movement as shown in Figure 8(d) and (e).

The successful assignment of evasive actions to agents in lower-priority communities is the basis for resolving trap situations.

6.2. Evaluation of trap handling

For the evaluation of the proposed approach for trap detection and handling, we consider the two setups illustrated in Figure 9.

Fig. 9.

Initialization of different test scenarios with free and trapped agents as well as static environmental constraints . The arrows indicate the movement directions of the agents.

Each setup contains multiple spherical agents with radius $r_{ag} \in R^{+}$ and double-integrator dynamics. The agents are pairwise constrained by inter-agent constraints according to (26). Nominal control inputs are constant accelerations with the directions according to the arrows in Figure 9. Shared invariance control is implemented according to Theorem 2. The parameters used in the simulations are provided in Table 2.

Table 2.

Simulation parameters for evaluation of trap handling

General
Agent radii	$r_{ag}$	0.2 m
Threshold	$Φ_{th}$	0.05 m
Setup Figure 9(a)
Initial positions	$p_{ag, 0}$	$[\begin{matrix} 0 \\ 0 \end{matrix}]$ , $[\begin{matrix} 0 \\ 1 \end{matrix}]$ , $[\begin{matrix} - \frac{\sqrt{3}}{2} \\ - 0.5 \end{matrix}]$ , $[\begin{matrix} \frac{\sqrt{3}}{2} \\ - 0.5 \end{matrix}]$ m
Accelerations	$u_{no}$	$[\begin{matrix} 0 \\ 0 \end{matrix}]$ , $[\begin{matrix} 0 \\ - 2 \end{matrix}]$ , $[\begin{matrix} \sqrt{3} \\ 1 \end{matrix}]$ , $[\begin{matrix} - \sqrt{3} \\ 1 \end{matrix}] m / s^{2}$
Corrective control	$γ$	$- 20 m / s^{2}$
Sampling time	$T_{A}$	0.001 s
Setup Figure 9(b)
Initial positions	$p_{ag, 0}$	$[\begin{matrix} 1.3 \\ 1.3 \end{matrix}]$ , $[\begin{matrix} 1.3 \\ 0.9 \end{matrix}]$ , $[\begin{matrix} 0.9 \\ 1.3 \end{matrix}]$ , $[\begin{matrix} - 1.3 \\ - 1.3 \end{matrix}]$ , $[\begin{matrix} - 0.9 \\ - 0.9 \end{matrix}]$ , $[\begin{matrix} 0 \\ 0 \end{matrix}]$ m
Accelerations	$u_{no}$	$[\begin{matrix} - 1 \\ - 1 \end{matrix}]$ , $[\begin{matrix} 0 \\ 2 \end{matrix}]$ , $[\begin{matrix} 2 \\ 0 \end{matrix}]$ , $[\begin{matrix} 2 \\ 2 \end{matrix}]$ , $[\begin{matrix} - 1 \\ - 1 \end{matrix}]$ , $[\begin{matrix} 3 \\ 3 \end{matrix}]$ $m / s^{2}$
Corrective control	$γ$	$- 10 \cdot \sqrt{18} m / s^{2}$
Sampling time	$T_{A}$	0.0001 s

Figure 10(a) shows what happens in the first setup without trap handling. With the inner agent being stationary and the outer agents moving towards it, at one point, the inner agent is trapped. As it is in the lowest-priority community, it has to take over all evasive actions, which is not possible. Being in higher-priority communities, the outer agents do not respect the constraint and therefore, the constraints are violated.

Fig. 10.

Behavior of controlled agents (a) without trap detection and (b) with trap detection in the scenario of Figure 9(a).

If, on the other hand, trap detection is used, the agents form an emergency community and redistribute their priorities. As a result, the outer agents have to respect the constraints and constraint violation is avoided as depicted in Figure 10(b).

The results of trap handling are further illustrated by the second setup introduced in Figure 9(b). In this case, the agents move such that two emergency communities form around the agents in the opposing corners. Figure 11 depicts the final configuration of the agents, when all agents become stationary as movement in the desired directions is no longer possible. It may be observed that no constraint violations occur. Furthermore, the agent, which starts in the middle and moves towards the upper right corner pushes its way in between the two agents trapping the agent in the corner. This is owed to the fact that this agent is in a higher-priority community, leading to evasive movements of the other two.

Fig. 11.

Two emergency communities are formed for the two agents trapped in different corners in the scenario shown in Figure 9(b).

With reliable trap detection and handling, we now turn to the experimental evaluation of shared invariance control.

6.3. Experimental evaluation

The experimental evaluation is executed on two redundant position-controlled manipulators $j \in {l, r}$ with seven degrees of freedom ( $m_{j} = 7$ ) each. Detailed information on the manipulators is provided by Stańczyk (2006). The end effectors are controlled to follow a desired trajectory while complying to external forces and adhering inter-agent constraints for collision avoidance between the end effectors.

6.3.1. Nominal control

The trajectory for the position-controlled manipulators is generated by an admittance-type control law derived from a model of the robotic manipulators and an impedance control law. The gravity and Coriolis effects are assumed to be compensated leading to the general joint-dynamics model of the manipulators

M_{q, j} {\overset{\cdot\cdot}{q}}_{j} = u_{j}

(27)

with the joint positions $q_{j} \in R^{7}$ , a positive definite mass matrix $M_{q, j} \in R^{7 \times 7}$ and the input torque $u_{j} \in R^{7}$ . The Cartesian impedance control law as introduced by Albu-Schaffer et al. (2003) is

\begin{matrix} τ_{j, imp} = τ_{j, ext} + J_{j}^{⊤} \\ (M_{j, p, des} {\overset{\cdot\cdot}{p}}_{j, des} + D_{j, p, des} ({\overset{\cdot}{p}}_{j, des} - {\overset{\cdot}{p}}_{j}) + K_{j, p, des} (p_{j, des} - p_{j})) \end{matrix}

with the external torques $τ_{j, ext} = J_{j}^{⊤} f_{j, ext} \in R^{7}$ , derived from the Cartesian forces $f_{j, ext} \in R^{3}$ using the Jacobian $J_{j} \in R^{3 \times 7}$ , the Cartesian position $p_{j} \in R^{3}$ , the desired trajectory $p_{j, des} \in R^{3}$ , the Cartesian mass matrix $M_{j, p, des} \in R^{3 \times 3}$ , positive definite stiffness $K_{j, p, des} \in R^{3 \times 3}$ , and damping $D_{j, p, des} \in R^{3 \times 3}$ matrices. The Cartesian impedance is complemented by the desired null space behavior

τ_{j, N} = - K_{j, N} (q_{j} - q_{j, N}) - D_{j, N} {\overset{\cdot}{q}}_{j, N}

with the desired joint trajectory $q_{j, N}$ and the positive-definite matrices $K_{j, N}, D_{j, N} \in R^{7}$ . This leads to the nominal control law

u_{j, no} = τ_{j, imp} + (I_{7} - J_{j}^{⊤} (J_{j}^{#})) τ_{j, N}

with the generalized inverse $J_{j}^{#} \in R^{3 \times 3}$ , which achieves the desired force behavior in Cartesian task space. Invariance control is included in between nominal control and the dynamics model (27), thus generating a constraint admissible trajectory for the actual robotic system.

6.3.2. Constraints

An inter-agent constraint is chosen to model both end effectors being enclosed by spheres with constant radii, which should never overlap. The constraint function is given by

y_{c} = - ‖ p_{l} - p_{r} ‖_{2} + r_{sph}

(28)

with a constant radius $r_{sph} = r_{l} + r_{r} \in R^{+}$ , which is the sum of the radii of the spheres around both end effectors. In addition, static box constraints in accordance with Kimmel et al. (2012) are implemented. Naturally, more inter-agent constraints may be added for full-body collision avoidance, but are omitted for improved clarity of the results.

6.3.3. Corrective control

Differentiation of (28)

{\overset{\cdot}{y}}_{c} = \frac{\partial y_{c}}{\partial p_{l}} \frac{\partial p_{l}}{\partial q_{l}} {\overset{\cdot}{q}}_{l} + \frac{\partial y_{c}}{\partial p_{r}} \frac{\partial p_{r}}{\partial q_{r}} {\overset{\cdot}{q}}_{r} = \frac{\partial y_{c}}{\partial p_{l}} J_{l} {\overset{\cdot}{q}}_{l} + \frac{\partial y_{c}}{\partial p_{r}} J_{r} {\overset{\cdot}{q}}_{r}

{\overset{\cdot\cdot}{y}}_{c} = \frac{\partial {\overset{\cdot}{y}}_{c}}{\partial p_{l}} J_{l} {\overset{\cdot}{q}}_{l} + \frac{\partial {\overset{\cdot}{y}}_{c}}{\partial p_{r}} J_{r} {\overset{\cdot}{q}}_{r} + \frac{\partial y_{c}}{\partial p_{l}} ({\overset{\cdot}{J}}_{l} {\overset{\cdot}{q}}_{l} + J_{l} {\overset{\cdot\cdot}{q}}_{l})

+ \frac{\partial y_{c}}{\partial p_{r}} ({\overset{\cdot}{J}}_{r} {\overset{\cdot}{q}}_{r} + J_{r} {\overset{\cdot\cdot}{q}}_{r})

and substitution of ${\overset{\cdot\cdot}{q}}_{j}$ from (27) yields $r = 2$ as the relative degree. The conditions on the input are given by (19) with

α_{j} = \frac{\partial y_{c}}{\partial p_{j}} J_{j} M_{j}^{- 1}, j \in {l, r}

b = \sum_{j \in {l, r}} (\frac{\partial {\overset{\cdot}{y}}_{c}}{\partial p_{j}} J_{j} {\overset{\cdot}{q}}_{j} + \frac{\partial y_{c}}{\partial p_{j}} {\overset{\cdot}{J}}_{j} {\overset{\cdot}{q}}_{j})

Corrective control is derived according to Theorem 2 with the optimization conditions corresponding to the static box constraints discussed by Kimmel et al. (2012) included in the optimization of each agent according to Corollary 1.

6.3.4. Priority assignment

In the experiment, the agent priorities are assigned according to multiple criteria.

Each agent has a static task priority $c_{tsk}$ .

The number of nearly active constraints criterion uses Definition 8 with

c_{Φ} = | D |, D = {i \in K | Φ_{i} \geq Φ_{th}}

to assign a higher priority to agents with a more constrained environment.

The external force criterion assigns higher priorities to agents with high external forces:

c_{f_{ext}} = ‖ f_{ext} ‖_{2}

The joint limit criterion

c_{jl} = \frac{1}{min (q_{\max} - q, q - q_{\min})}

assigns higher priorities if agents are close to a joint limit.

The manipulability measure as introduced by Yoshikawa (1985) is based on the Jacobian $J$ and is used to give a higher priority to agents with lower manipulability in task space

c_{J} = \frac{1}{\sqrt{det J J^{⊤}}}

A weighted sum with positive weights over the desired criteria derives the final agent priorities.

6.3.5. Setup

The derived invariance control law including admittance control and the constraints is implemented in the Real-Time Workshop of Matlab/Simulink. The used solver is a discrete time Euler solver with the sampling frequency of $1 kHz$ . The desired Cartesian trajectories are given by

p_{des, l} = \int_{t_{0}}^{t} 0.1 [\begin{matrix} 0.2 \sin (2 \frac{π}{25} ψ + \frac{π}{2}) \\ - 0.1 \sin (\frac{π}{25} ψ) \\ - 0.1 \sin (\frac{π}{25} ψ + \frac{π}{2}) \end{matrix}] d ψ

p_{des, r} = \int_{t_{0}}^{t} 0.1 [\begin{matrix} 0.2 \sin (2 \frac{π}{25} ψ + \frac{π}{2}) \\ 0.05 \sin (\frac{π}{25} ψ) \\ - 0.05 \sin (\frac{π}{25} ψ + \frac{π}{2}) \end{matrix}] d ψ

The parameters for the experiments are provided in Table 3. If not denoted differently, the parameters are the same for both manipulators.

Table 3.

Experimental parameters.

Initial joint	$q_{0, l}$	${[0.27, 2.22, 0, - 1.49, 1.44, 1.32, - 0.73]}^{⊤} rad$
position	$q_{0, r}$	${[0.61, 0.39, 0, 1.5, 0.79, 0.93, - 1.35]}^{⊤} rad$

Initial Cartesian	$p_{0, l}$	${[0.5, 0.15, - 0.441]}^{⊤}$ m
position	$p_{0, r}$	${[\begin{matrix} 0.5 & - 0.15 & - 0.341 \end{matrix}]}^{⊤}$ m

Desired	$M_{p, des}$	$12 I_{3}$ kg
compliance	$K_{p, des}$	$200 I_{3} N / m$
behavior	$D_{p, des}$	$80 I_{3} Ns / m$

Desired null-	$K_{N}$	$diag (360 I_{4}, 180 I_{3}) Nm / rad$
space behavior	$D_{N}$	$diag (25 I_{4}, 10 I_{3}) Nms / rad$

Invariance control	$γ$	$- 1.8 m / s^{2}$

Box	$p_{\min}$	$[0.3, - 0.3, - 0.48]$ m
constraints	$p_{\max}$	$[0.57, 0.3, - 0.28]$ m
Inter-agent constraint	$r_{sph}$	$0.2$ m

Static priority	$c_{tsk, l}$	$2$
	$c_{tsk, r}$	$1$
Dynamic	$Φ_{th}$	$- 0.025$
priority	$λ_{Φ}$	$0.5$
assignment	$λ_{f_{ext}}$	$0.1$
	$λ_{jl}$	$0.05$
	$λ_{J}$	$0.4$

Shared invariance control is validated in two experiments. First, solely static priorities are used to illustrate the capabilities of shared invariance control in the presence of external forces and environmental box constraints. Then the static priorities $c_{tsk}$ are replaced by dynamic priorities $c$ , which are derived from a weighted sum of the dynamic priority criteria

c = λ_{Φ} c_{Φ} + λ_{f_{ext}} c_{f_{ext}} + λ_{jl} c_{jl} + λ_{J} c_{J}

with the weights $λ_{Φ}, λ_{f_{ext}}, λ_{jl}, λ_{J} \in R^{+}$ as provided in Table 3. Furthermore, solely the inter-agent constraint is considered in the second experiment. Hence, this experiment evaluates the shared invariance control approach for multi-agent systems with dynamic priority assignment.

6.3.6. Results

In the first experiment, the end effectors follow the desired trajectories while enforcing the inter-agent and box constraints. In addition, external forces as depicted in Figure 12 are applied and generate a compliant reaction due to the used admittance control.

Fig. 12.

External forces $f_{p_{1}, ext}$ , $f_{p_{2}, ext}$ , and $f_{p_{3}, ext}$ in the experiment with static priorities for the left and right manipulator.

Despite the forces, the box constraints are enforced, as Figure 13 shows no violations of the bounds. In addition, the inter-agent constraint holds as depicted by Figure 14(a), which shows that the distance between the end-effector positions is at least $r_{sph}$ .

Fig. 13.

The Cartesian position $p_{l / r}$ in the experiment with static priorities and box constraints for the left and the right manipulator with the respective the reference $p_{ref, l / r}$ , without invariance control.

Fig. 14.

Inter-agent constraint, invariance functions, and relative position deviation for the experiment with static priorities with highlighting the time, during which solely the inter-agent constraint is active. (a) Distance between the end effectors. (b) Invariance functions of the Cartesian box constraints for the left manipulator, the right manipulator and the invariance function of the inter-agent constraint . (c) Relative position deviation for the left and the right manipulator with the respective desired values .

The invariance functions depicted in Figure 14(b) remain at non-positive values, thus emphasizing the constraint satisfaction. In addition, the relative position deviation from the trajectory without invariance control

e_{rel, l / r} = \frac{e_{p, l / r}}{e_{p, l} + e_{p, r}}

(29)

with $e_{p, l / r} = p_{ref, l / r} - p_{l / r}$ is shown in Figure 14(c). Note that the high oscillations in case no constraint is active are due to the small position deviations in this case and therefore tiny changes have a major effect on the relative position deviation. More importantly, however, if solely the inter-agent constraint is active, e.g., at $t \in [129, 132]$ , the relative position deviation is partitioned according to the weights $w_{l} = \frac{1}{3}$ and $w_{r} = \frac{2}{3}$ resulting from the static priorities and (23). Hence, the experimental results validate shared invariance control for multi-agent systems as both manipulators adhere to the static box constraints and share the evasive effort for the inter-agent constraint according to their priority values.

In the second experiment the priorities for shared invariance control are determined using the dynamic criteria. In this experiment solely the inter-agent constraint is considered.

As the applied external forces, depicted in Figure 15, are used in the force-based priority assignment criterion $c_{f_{ext}}$ to determine the agent priorities, they have a direct effect on the priorities, shown in Figure 16(b). Instead of being static, the priority values of the agents now change over time according to the dynamic criteria. Nevertheless, the manipulators generally adhere to the inter-agent constraint keeping at least a distance of $r_{sph}$ as depicted in Figure 16(a). The minor violations are caused by the sampled time implementation and a small magnitude of $γ$ . By increasing $γ$ and applying methods for chattering reduction by Kimmel and Hirche (2014), this effect may be overcome.

Fig. 15.

External forces $f_{p_{1}, ext}$ , $f_{p_{2}, ext}$ , and $f_{p_{3}, ext}$ in the experiment with dynamic priorities for the left and right manipulator.

Fig. 16.

Inter-agent constraint, dynamic priorities and weights for shared invariance control in the experiment with dynamic priorities with highlighting the time, during which the inter-agent constraint is active. (a) Distance between the end effectors. (b) Priorities of the left ; and the right ; manipulator. (c) Relative position deviation to the unconstrained position for the left ; and the right ; manipulator with the respective desired values , .

As the force sensors are prone to measurement noise, the priorities of the agents as depicted in Figure 16(b) and the desired partition in Figure 16(c) are subject to noise as well. It may, however, be observed that the relative position deviation of the manipulators follows the trend of the desired weights while the constraint is active and the robotic system acts like a low-pass filter, smoothing the desired signal. Naturally, if the constraint is inactive, the weights differ from the desired weight as no evasive action and therefore no weighting is necessary. Hence, this experiment encourages the use of shared invariance control for multi-agent systems with weighted inter-agent constraints and dynamic priorities.

7. Conclusion

In this work, we have proposed shared invariance control as a method to share and distribute the constraint enforcing control effort in multi-agent systems according to agent priorities. The control law allows for a distributed implementation and has been formally shown to guarantee constraint satisfaction. In addition, a two-stage prioritization scheme has been developed that ensures that no constraint violation occurs due to low-priority trapped agents and that allows the determination of agent priorities according to multiple static and dynamic conditions. The approach has been successfully evaluated in simulation and experiments on robotic manipulators.

Footnotes

Appendix. Proofs

Proof of Theorem 2. For the proof, we consider the agents as a single system described by (7) and (15). The minimization (21) is then re-written as

u_{c} = \underset{u = [u_{1}^{⊤}, \dots, u_{n_{ag}}^{⊤}]^{⊤}}{argmin} \sum_{j \in N_{ag}} | | u_{j} - u_{no, j} | |_{2}^{2}

subject to the same input constraints (19), which yields the same result as the decoupled optimization problems (21). Any solution to the optimization $u$ fulfills all optimization conditions, i.e., the input $u_{j}$ of each agent $j \in A_{i}$ is in the set of admissible inputs (19). Therefore, the inequality also holds for the sum of the individual inequalities

\begin{matrix} \sum_{j \in A_{i}} ({α_{i, j}}^{⊤} u_{j} + w_{i, j} (c) (\sum_{k \in J_{i}} {α_{i, k}}^{⊤} u_{k} + b_{i} (x, x_{η}))) \\ \leq \sum_{j \in A_{i}} w_{i, j} (c) γ_{i} \end{matrix}

Using (20) yields

\sum_{j \in A_{i}} {α_{i, j}}^{⊤} u_{j} + \sum_{k \in J_{i}} {α_{i, k}}^{⊤} u_{k} + b_{i} (x, x_{η}) \leq γ_{i}

and with $A_{i} \cup J_{i} = N_{ag}$

\sum_{j \in N_{ag}} {α_{i, j}}^{u_{j}} + b_{i} (x, x_{η}) \leq γ_{i}

which corresponds to the invariance condition (16) of the centralized system. Hence, by Theorem 1 adherence to the constraints is achieved as the control input fulfills the condition for invariance encoded in the optimization condition and, thus, avoids constraint violation. □

Proof of Corollary 1. Theorem 2 states that the control input derived from the optimization (21) enforces inter-agent constraints $h_{i} (x, η)$ . Environmental constraints $h_{i} (x_{j}, η)$ as introduced by Kimmel and Hirche (2017) represent a special case of inter-agent constraints. As this constraint solely depends on the states $x_{j}$ of agent $j$ and not on the states of the other agents, the conditions on the input (19) reduce to

{α_{i, j}}^{⊤} u_{j} + w_{i, j} (c) b_{i} (x_{j}, x_{η}) \leq w_{i, j} (c) γ_{i}

Furthermore, as agent $j$ is the sole agent affecting the constraint $i$ , it is the only agent in $A_{i}$ , i.e., (20) yields $w_{i, j} (c) = 1$ resulting in the condition

{α_{i, j}}^{⊤} u_{j} + b_{i} (x_{j}, x_{η}) \leq γ_{i}

which corresponds to the optimization condition for invariance control introduced by Kimmel and Hirche (2017). As this condition is independent of the states of the other agents, each agent enforces environmental constraints independently from other agents. □

Proof of Corollary 2. The cost function of the optimization is convex as it is a quadratic function with the Hessian being the identity matrix. Furthermore, the constraints are linear in the optimization variables $u_{j}$ and therefore also convex. Thus, the optimization is convex. □

Proof of Corollary 3. Both the cost function (21) and the conditions on the input (19) solely depend on one optimization variable $u_{j}$ , meaning that the solution of the optimization problems for the other agents do not affect the solution. Furthermore, as the constraints affect the solution, only the states and inputs of the agents affecting these constraints need to be known to the active agent. Hence, the optimizations may be solved independently of each other, which allows the implementation in a distributed manner. □

Proof of Theorem 4. Algorithm 1 converges if it is able to free at least one agent at each iteration step until all agents in the emergency community are free. Therefore, it is necessary to investigate whether the priority community $S_{m}$ , which all agents with a higher priority are reassigned to, contracts in each step, i.e., contains free agents that are not considered in the next iteration of the algorithm. The proof is conducted by induction.

Let there be a single agent in $S_{m}$ . As agents with a higher priority are generally reassigned to $S_{m}$ and this means that the other agents have a lower priority, the single agent in $S_{m}$ has the highest priority in the emergency community. Therefore, it is not constrained by inter-agent constraints but only by environmental constraints $h_{i} (x_{j}, η)$ . As, by assumption, the set $M_{j}$ is non-empty for each agent $j$ , an agent is never trapped solely by environmental constraints, this agent is free. This holds for all single agents in the emergency community if they have the highest priority.

Now consider the case of the priority community $S_{m}$ containing $n$ agents. We assume that there is at least one free agent. If adding another agent to $S_{m}$ results in all agents in $S_{m}$ being trapped, this means that the new agents shares inter-agent constraints with previously free agents, which now block those agents and the new agent and, hence, there is a set of agents $N$ for which $M_{N, j}$ is empty. As this situation of agents mutually trapping each other is excluded by assumption, this means that after adding a new agent, either the previously free agent still has to be free or the newly added agent has to be free.

Hence, by induction, if the number of agents in $S_{m}$ grows, there is always a free agent. As this is independent from the order, in which the agents are added, the existence of free agents is also guaranteed in the reversed case if agents are removed from $S_{m}$ , i.e., if free agents are put in lower-priority communities by the algorithm. This means that $S_{m}$ contracts with each iteration until it is empty, i.e., the algorithm converges. □

Acknowledgements

M. Kimmel, J. Pfort, and J. Wöhlke contributed equally to this work.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the EU Seventh Framework Programme FP7/2007-2013 within the ERC Starting Grant Control based on Human Models (con-humo; grant agreement number 337654) and the joint Sino–German project “Control and Optimization for Event-triggered Networked Autonomous Multi-agent Systems” funded by the German Research Foundation (DFG) and the National Science Foundation of China (NSFC).

ORCID iDs

Melanie Kimmel

Jannick Pfort

Jan Günter Wöhlke

References

Akametalu

Fisac

Gillula

Kaynama

Zeilinger

Tomlin

(2014) Reachability-based safe learning with Gaussian processes. In: IEEE 53rd Annual Conference on Decision and Control (CDC), pp. 1424–1431.

Albu-Schaffer

Ott

Frese

Hirzinger

(2003) Cartesian impedance control of redundant robots: Recent results with the DLR-light-weight-arms. In: 2003 IEEE International Conference on Robotics and Automation, Vol. 3, pp. 3704–3709.

Alonso

Milanés

Pérez

Onieva

González

De Pedro

(2011) Autonomous vehicle control systems for safe crossroads. Transportation Research Part C: Emerging Technologies 19(6): 1095–1110.

Ames

Grizzle

Tabuada

(2014) Control barrier function based quadratic programs with application to adaptive cruise control. In: IEEE 53rd Annual Conference on Decision and Control (CDC), pp. 6271–6278.

Barthès

Callebert

Lourdeaux

(2016) Priority-based contextual local decision making in multi-agent systems. In: IEEE 20th International Conference on Computer Supported Cooperative Work in Design (CSCWD), pp. 186–191.

Blanchini

(1999) Set invariance in control. Automatica 35(11): 1747–1767.

Cai

Yang

Zhu

Liang

(2007) Collision avoidance in multi-robot systems. In: 2007 International Conference on Mechatronics and Automation, pp. 2795–2800.

Fox

Burgard

Thrun

(1997) The dynamic window approach to collision avoidance. IEEE Robotics and Automation Magazine 4(1): 23–33.

Freund

Rossman

(2003) The basic ideas of a proven dynamic collision avoidance approach for multi-robot manipulator systems. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. 2, pp. 1173–1177.

10.

Freund

Schluse

Rossmann

(2001) Dynamic collision avoidance for redundant multi-robot systems. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium, Vol. 3, pp. 1201–1206.

11.

Gilbert

Kolmanovsky

(2002) Nonlinear tracking control in the presence of state and control constraints: A generalized reference governor. Automatica 38(12): 2063–2073.

12.

Gillespie

Cutkosky

(1996) Stable user-specific haptic rendering of the virtual wall. In: ASME International Mechanical Engineering Congress and Exposition, Vol. 58, pp. 397–406.

13.

Hong

Park

Song

Youn

(2009) A priority-based message scheduling scheme for multi-agent system dynamically, adapting to the environment change. In: 2009 International Conference on Cyber-Enabled Distributed Computing and Knowledge Discovery, pp. 191–196.

14.

Isidori

(1995) Nonlinear Control Systems. 3rd Ed. Berlin: Springer-Verlag.

15.

Jähne

Hirche

(2017) Augmented invariance control for impedance-controlled robots with safety margins. IFAC-PapersOnLine 50: 12053–12058.

16.

Katevenis

Sidiropoulos

Courcoubetis

(1991) Weighted round-robin cell multiplexing in a general-purpose ATM switch chip. IEEE Journal on Selected Areas in Communications 9(8): 1265–1279.

17.

Kimmel

Hirche

(2014) Invariance control with chattering reduction. In: IEEE 53rd Annual Conference on Decision and Control (CDC), pp. 68–74.

18.

Kimmel

Hirche

(2016) Invariance control with time-varying constraints. In: European Control Conference (ECC), pp. 867–872.

19.

Kimmel

Hirche

(2017) Invariance control for safe human–robot interaction in dynamic environments. IEEE Transactions on Robotics 33(6): 1327–1342.

20.

Kimmel

Jähne

Hirche

(2016) Augmented invariance control for systems with smoothness constraints. In: IEEE 55th Annual Conference on Decision and Control (CDC), pp. 4867–4873.

21.

Kimmel

Lawitzky

Hirche

(2012) 6D workspace constraints for physical human–robot interaction using invariance control with chattering reduction. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3377–3383.

22.

Lasota

Fong

Shah

(2017) A survey of methods for safe human–robot interaction. Foundations and Trends in Robotics 5(4): 261–349.

23.

Lianghong

Jian

Xiang

(2015) A research on priority-based sequential task allocation model in multi-agent system. In: Chinese Automation Congress (CAC), pp. 1058–1063.

24.

Maoudj

Bouzouia

Hentout

Toumi

(2015) Multi-agent approach for task allocation and scheduling in cooperative heterogeneous multi-robot team: Simulation results. In: IEEE 13th International Conference on Industrial Informatics (INDIN), pp. 179–184.

25.

Mayne

Rawlings

Rao

Scokaert

(2000) Constrained model predictive control: Stability and optimality. Automatica 36(6): 789–814.

26.

Medina Hernández

Sieber

Hirche

(2013) Risk-sensitive interaction control in uncertain manipulation tasks. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 502–507.

27.

Mörtl

Lawitzky

Kucukyilmaz

Sezgin

Basdogan

Hirche

(2012) The role of roles: Physical cooperation between humans and robots. The International Journal of Robotics Research 31(13): 1656–1674.

28.

Rauscher

Kimmel

Hirche

(2016) Constrained robot control using control barrier functions. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, Korea, pp. 279–285.

29.

Rimon

Koditschek

(1992) Exact robot navigation using artificial potential functions. IEEE Transactions on Robotics and Automation 8(5): 501–518.

30.

Rosenberg

(1993) Virtual fixtures: Perceptual tools for telerobotic manipulation. In: IEEE Virtual Reality Annual International Symposium pp. 76–82.

31.

Rossmann

(1996) On-line collision avoidance for multi-robot systems: A new solution considering the robots’ dynamics. In: 1996 IEEE/SICE/RSJ International Conference on Multisensor Fusion and Integration for Intelligent Systems, pp. 249–256.

32.

Saleh

Chevrel

Claveau

Lafay

Mars

(2013) Shared steering control between a driver and an automation: Stability in the presence of driver behavior uncertainty. IEEE Transactions on Intelligent Transport Systems 14(2): 974–983.

33.

Stańczyk

(2006) Development and Control of an Anthropomorphic Telerobotic System. PhD Thesis, Lehrstuhl f ür Steuerungs- und Regelungstechnik Technische Universität München.

34.

Tiwari

Chandraker

Goel

(2016) Comparative analysis of different lottery bus arbitration techniques for SOC communication. In: International Conference on Computational Techniques in Information and Communication Technologies (ICCTICT), pp. 495–499.

35.

Vanderbei

(2001) Linear Programming: Foundations and Extensions (International Series in Operations Research and Management Science, Vol. 37). 2nd Ed. New York: Springer.

36.

Wolff

Buss

(2004) Invariance control design for nonlinear control affine systems under hard state constraints. In: 6th IFAC Symposium on Nonlinear Control Systems (NOLCOS), Stuttgart, Germany, pp. 711–716.

37.

Wolff

Buss

(2005) Invariance control design for constrained nonlinear systems. In: 16th IFAC World Congress, Prague, Czech Republic.

38.

(2018) Constrained control of input–output linearizable systems using control sharing barrier functions. Automatica 87(Suppl. C): 195–201.

39.

Yoshikawa

(1985) Dynamic manipulability of robot manipulators. In: Proceedings 1985 IEEE International Conference on Robotics and Automation, Vol. 2, pp. 1033–1038.