Augmented Invariance Control forImpedance-controlled Robots with Safety

Margins

Christoph Jahne ∗, Sandra Hirche ∗

∗ Both authors are with the Chair of Information-oriented Control(ITR), Technical University of Munich, 80333 Germany

(e-mail: c.jaehne@tum.de, hirche@lsr.ei.tum.de).

Abstract: Various robotic applications require enforcing constraints, to achieve task perfor-mance or to hinder the robot from causing danger. Especially in human-robot-interaction,collision avoidance and velocity limits are crucial for safety. A promising approach to enforceadherence to safety margins is invariance control. Considering the system dynamics, it correctsa nominal control based on a switching policy. To overcome the resulting lack of smoothness interms of control inputs and states, a control scheme is developed that adds dynamics to the inputand thereby augments the system. In previous work it is shown that the closed loop system isstable in combination with an exponentially stabilizing nominal control. However, this could notbe shown for impedance controlled robots. Therefore, in this paper, an augmented invariancecontrol for robotic applications is developed and proven to be uniformly asymptotically stablewith an impedance-type nominal control. The proposed control law is validated in experimentson a KUKA LWR4+ in a collision avoidance scenario demonstrating stable behaviour with im-proved smoothness and chattering characteristics while enforcing the imposed hard constraints.

Keywords: invariance control, hard constraints, smoothness, continuous actuation, safetymargins, robot, impedance control, asymptotic stability.

1. INTRODUCTION

Robots are gradually expanding their field of applicationfrom purely industrial settings to e.g. service roboticsand enter domestic domains. This involves facing dynamicenvironments that are hard to predict or plan. Especiallyin human-robot interaction this raises the need for controlfeatures that ensure safety for both robot and humanduring operation. A safe solution is reactive collision avoid-ance that prevents unintended contacts between robotand environment. To achieve this, various methods exist.Model predictive control (MPC) (Mayne et al., 2000) andthe command governor approach (Angeli and Mosca, 1999)are based on optimization and allow to consider hardconstraints to states, inputs or outputs but may be compu-tationally too expensive in real-time application. Virtualpotential fields enable the derivation of repelling forcesthat are added to the control input of the robot (Rimonand Koditschek, 1992) but without considering a dynamicmodel of the system, adherence to constraints cannot beguaranteed though. Another alternative to enforce con-straints is the inclusion of control barrier functions as in(Rauscher et al., 2016) that serve to impose constraints toa convex optimization problem for supervising a nominalcontrol signal w.r.t. an arbitrary number of boundaries.However, barrier functions tend to infinity approaching thebarriers and are therefore only defined inside the constraintadmissible area. In real applications, minor boundary vio-lations cannot be excluded due to sensor noise. A closelyrelated approach is invariance control. It reactively super-

vises a nominal control scheme and corrects it whenevernecessary in order to keep boundaries that are defined ashard constraints to the state space. Instead of barrier func-tions, invariance functions are derived from the system dy-namics which are defined in the constraint admissible areaas well as outside. This enables the system to recover fromconstraint violations. As shown in (Kimmel and Hirche,2015) this also allows for dynamic boundaries which makesit appealing in human-robot-interaction. When facing dy-namic sceneries, the set of considered boundaries needsto be updated online as a priori unknown or untrackedobjects/persons might enter the workspace of the robot.In addition limited capabilities of perception might notallow complete tracking of all obstacles at all time. Thus,jumps in the set of constraints cause abrupt and unfore-seen reactions of the robot. In general, with the approachto invariance control described in (Kimmel and Hirche,2015) and adapted from (Wolff and Buss, 2004), a smoothrobot motion at the boundaries is hindered by switchingbetween nominal and corrective control. This induces re-peated jumps in the robots control input, especially whenchattering occurs due to discrete implementation. On thelong run, this potentially damages the actuation systemand is highly undesired for smooth and intuitive human-robot interaction. In (Kimmel et al., 2016) we proposean approach to invariance control that allows to achievesmoothness requirements on states and control inputs byartificially augmenting the controlled system. Stability isproven, assuming an exponentially stable nominal con-trol. However, most common control schemes for physical

human-robot-interaction such as impedance control couldnot be shown to be exponentially stable.The contribution of this paper is the development of anaugmented invariance control for impedance controlledrobots. We show asymptotic stability for the augmentedclosed loop system. The functionality and stability ofthe approach are validated in experiments on a KUKALWR4+. Chattering and smoothness characteristics incomparison with a non-augmented invariance controlscheme indicate the advantages of the proposed controlscheme.The remainder of the this paper is organized as follows:Section 2 introduces system formulations and augmentedinvariance control. In Section 3, the theory is adaptedto impedance-controlled robots. Experiments in Section 4illustrate stability and demonstrate the advantages of ourapproach.

Notation: Vectors are bold lower case letters a and ma-trices bold capital letters A. The Moore-Penrose pseudoinverse is written A+. Derivatives w.r.t. time are repre-sented by dots above the quantity x := dx

dt . The v-th

time derivative of x is x(v) := dvxdtv . The set of continuous

functions that are v times continuously differentiable isdenoted by Cv. A set K in the subscript AK indicatesthe reduction of the quantity to the rows of the indicescontained in the set. The symbols 0 and I denote zeroand identity matrices. Dimensions may be specified insubscripts Im where a single dimension denotes a squarematrix.

2. PRELIMINARIES

2.1 Robotic System

We consider a force controlled robotic system representedby the non-linear dynamics

M(q)q +C(q, q)q + g(q) = τ c (1)

with generalized coordinates q ∈ Rm, and τ c ∈ Rm beingthe joint torques. The terms M(q) ∈ Rm×m, C(q, q) ∈Rm×m and g(q) ∈ Rm denote the positive definite massmatrix, the Coriolis matrix, and the gravitational torquesrespectively. This system has the states xT = [qT , qT ]T

with x ∈ R2m=n and is reformulated to

x =

[qq

]=

[q

M−1(−g −Cq)

]︸ ︷︷ ︸

f(x)

+

[0

M−1

]︸ ︷︷ ︸G(x)

τ c . (2)

In the following we omit time and non-linear dependenciesof the parameters for notational simplicity. Without thesupervising invariance control, the robot is directly con-trolled by a nominal control τ c = τ no of impedance-type

τ no = −K(q − qd)−Dq + g(q) , (3)

where K ∈ Rm×m and D ∈ Rm×m are symmetricand positive definite stiffness and damping matrices. Thedesired configuration of the robot is denoted by qd ∈ Rm.

2.2 Invariance Control and Augmentation

The idea of invariance control as presented in (Kimmel andHirche, 2015) is the supervision of a nominal control w.r.t.a set of time varying hard constraints hi(x(t),η(t)) ∈

R, i ∈ {1, 2..., l} with relative degrees ri ∈ N to switchbetween nominal and corrective control, whenever neces-sary to enforce the constraints. The functions hi(x(t),η(t))parametrized by η(t) ∈ Rnη define the set of constraintslimiting the state space of the system to the admissible set

H(t) = {x ∈ Rn |hi(x(t),η(t)) ≤ 0 ∀ i ∈ {1, 2, ...l}} . (4)

The switching policy is based on so called invariance func-tions Φi(x(t),η(t), ...,η(ri−1)(t), γi) ∈ R which are worst-case predictions for hi given the current states x andassuming a maximum constant reaction of the robot in

direction of the constraint h(ri)i = γi. Here, the param-

eter γi is equivalent to a maximum relative acceleration.The derivation of Φi is based on an I/O-linearisation of hi

zi = h(ri)i = aTi (x,ηi)τ c + bi(x,ηi, ηi, ...,η

(ri)i ) . (5)

with vector ai ∈ Rm describing the influence of the controlinput τ c on constraint i and bi ∈ R containing all residualterms which includes e.g. motion of the constraint surfacesexpressed by the time-varying parameters η(t). In casecorrective control is applied, constraints in the vector spaceof τ c as

h(ri)i

!≤ zc,i (6)

are set such that the system stays inside the admissibleset (4). A simple choice to achieve this goal is zc,i = γifor an active constraint indicated by Φi ≥ 0. To reducechattering in areas where Φi ' 0 holds, more involvedstrategies are possible (Kimmel and Hirche, 2014). In orderto achieve a minimum difference in terms of the Euclideannorm between corrective and nominal control, the systeminput τ c is determined by solving the minimization prob-lem

argminτ c

||τ c − τ no||2subject to (6) .

(7)

As long as no constraint is active, the solution of (7) isexactly τ no. For a set of linearly independent constraints Kwith |K| < m and a non-empty admissible set H, prob-lem (7) has an analytic solution given by

τ c = τ no +A+K(zK,c − zK,no) (8)

according to (Kimmel and Hirche, 2015). The matrix Aconsists of the rows aTi from (5). An overall control struc-ture of an invariance controlled robot is depicted in Figure1. Between robot and nominal control, invariance controlis set for supervision. Constraint tracking is required withdynamic boundaries to determine the parameters η(t)online.This paper addresses the problem of jumps caused by in-variance control in terms of system inputs τ c by switchingbetween nominal and corrective control. These disconti-nuities can be solved applying an augmented version ofinvariance control as proposed in (Kimmel et al., 2016).

robotinvariancecontrol

nominalcontrol

x

η

xd τ no τ c

constrainttracking

Fig. 1. Control scheme of a robot supervised by a non-augmented invariance control (blue).

The key idea is to move the corrective switching to ahigher relative degree by artificially adding dynamics onthe control input of the system, introducing additionalstates χ ∈ Rm·v where v ∈ N denotes the degree of aug-mentation. The augmented dynamics of a generic controlaffine system is[

xχ

]=

[f(x) +G(x)τ c

fa(χ)

]+

[0

Ga(χ)

]τ ac . (9)

where the smooth vector field fa ∈ Rm·v and the ma-trix Ga ∈ Rm·v×m represent the added control affinedynamics. The augmented control input to system (9)is τ ac ∈ Rm which can be considered a higher relativedegree version of the τ c. We use it to design the augmentedinvariance control scheme with higher relative degrees ofthe contraints ra,i = ri + v and using (5)-(8). Correctivecontrol actions to enforce the constraints are now per-formed using the higher order pseudo input chosen e.g.as a derivative of τ c. It is obvious that corrective andnominal control have to act on the system at the samerelative degree to achieve smoother behaviour and for theprinciples of invariance control to be applicable. For v > 0,this is not the case with control (3). A solution to thisproblem is given in (Kimmel et al., 2016) by

τ ano = A−1a (χ)

(τ

(v)no − ba(χ)−

v−1∑j=0

kj(τ(j)c − τ (j)

no )

)(10)

as augmented nominal control that may be interpretedas a filtered version of the original control with timeconstants defined through the gains kj > 0. For non-active constraints, τ ac = τ ano is applied as input to theaugmented system. The matrix Aa ∈ Rm×m and thevector ba ∈ Rm result from the I/O-linearisation of τ c

w.r.t. τ ac

τ (v)c = Aa(χ)τ ac + ba(χ) (11)

where Aa must be invertible which is the case if τ c hasa vector relative degree of (v, v, ..., v) w.r.t. τ ac. Thismotivates the following assumption:

Assumption 1. The additional dynamics fa, Ga from (9)are chosen such that τ c has a vector relative degreeof (v, v, ..., v) w.r.t. τ ac.

As an example, the choice Aa = Im, ba = 0 fulfilsAssumption 1 and implements the added dynamics asa simple integrator chain. Equation (10) achieves thatfor non-active constraints, the control input τ c convergesto τ no according to (Kimmel et al., 2016) and under theassumption that τ no is v times differentiable.

Assumption 2. The nominal control input τ no is at least vtimes continuously differentiable w.r.t. time, i.e. τ no ∈Cv+ , v+ ∈ N, v+ ≥ v.

The depicted approach to augmented invariance control isapplied in this paper to perform collision avoidance withan impedance controlled robot, achieving continuous con-trol inputs. Therefore the following section serves to adaptthe theory of augmented invariance control to a robotsetting proving asymptotic stability based on Lyapunovtheory.

3. AUGMENTED INVARIANCE CONTROL FORIMPEDANCE-CONTROLLED ROBOTS

3.1 Nominal Control and Dynamics

The generic augmented dynamics (9) allow for any fa, Ga

that fulfil assumption 1. For reasons of simplicity andinterpretability we define the added dynamics to be pureintegrator chains of higher order time derivatives of thecontrol input τ c.

Assumption 3. The dynamics fa, Ga from (9) are chosento be integrator chains, meaning Aa = Im and ba = 0.

With this assumption, (11) simplifies to τ(v)c = τ ac

which we insert into (10), yielding the augmented nominalcontrol

τ ano = τ (v)no −

v−1∑j=0

kj(τ(j)c − τ (j)

no ) . (12)

Introducing the error ∆ ∈ Rm·v

∆ =

∆0

∆1

...∆v−1

=

τ no − τ c

τ no − τ c

...

τ (v−1)no − τ (v−1)

c

(13)

we reformulate the augmented dynamics (9) under aug-mented nominal control (12) with τ no from (3) to qq

∆

=

qM−1(−K(q − qd)−Dq −Cq − S0∆)

[ST0 , −KTa ]T∆

. (14)

The gain matrix Ka ∈ Rm×m·v can be written as Ka =[k0Im, k1Im, ..., kv−1Im] and the selection matrices S0 ∈Rm×m·v and S0 ∈ Rm·(v−1)×m·v are defined by S0 =[Im, 0m×m·(v−1)] and S0 = [0m·(v−1)×m, Im·(v−1)] respec-tively. As only ∆0 = τ no− τ c directly influences the jointaccelerations q, we select it with S0∆ = ∆0. All remainingelements in ∆ besides ∆0 are selected by S0.

3.2 Stability

The proof of stability from (Kimmel et al., 2016) foraugmented nominal control is formulated for exponentiallystabilizing nominal controls. To our best knowledge, ex-ponential stability could not be proven for impedance-controlled robots, motivating the following theorem.

Theorem 1. Let Assumptions 1-3 hold. Then the sys-tem (1) under control (12) with τ no from (3) is uniformlyasymptotically stable in the sense of Lyapunov.

Proof:We assume that the desired joint configuration is in theorigin qd = 0. This is no mayor restriction as for qd 6= 0we can always transform (2) to meet this assumption. TheLyapunov function

V (q, q,∆) =1

2qTKq +

1

2qTMq +

1

2∆TP∆ (15)

contains the virtual energy stored in the virtual spring ofthe impedance control qTKq, the kinetic energy of thesystem qTMq, and the pseudo energy term ∆TP∆ thatserves as a positive definite storage function for the statesadded due to augmentation. The matrix P ∈ Rm·v×m·v issymmetric and positive definite. Also V (0,0,0) = 0 holds,

making V a suitable Lyapunov candidate. Differentiationof (15) gives

V = qTKq + qT (−Kq −Dq −Cq − S0∆)

+1

2qTMq + ∆T

[ST0 , −K

Ta

]TP∆

(16)

where we insert the skew-symmetry property of mass andCoriolis matrices qT (M−2C)q = 0, (Murray et al., 1994)and simplify to

V = −qTDq + ∆T[ST0 , −K

Ta

]TP∆− qTS0∆ (17)

which is zero at the origin x = 0, ∆ = 0. A symmetricpositive definite matrix P always exists that satisfies theinequality

V (q,∆) ≤ 0 (18)

and thus renders V (q,∆) negative semi-definite, consider-

ing that the states q are not contained in V . To illustratethis claim, consider the exemplary choice for P

P =

[p1Im·(v−1) 0

0 p2Im

](19)

with positive scalar parameters p1 > 0 and p2 > 0.Let p1 << p2 and p1 << 1 hold. We rearrange theterms in (17) into negative definite and indefinite termsby splitting the quadratic form of ∆.

V = −qTDq −∆T[0, p2K

Ta Im

]T∆︸ ︷︷ ︸

negative definite

−qTS0∆ + ∆T[p1S

T0 Im(v−1), 0

]T∆︸ ︷︷ ︸

finite, indefinite

(20)

It is obvious that the term ∆T [p1ST0 Im(v−1), 0]T∆ tends

to 0 for p1 → 0 and that for large enough values p2 andfinite states ∆ and q, we render V negative semi-definite.At this point we have proven semi-global uniform stabilityof the system according to Theorem 4.1 of (Khalil, 1996).Applying LaSalle’s invariance principle proves asymptoticstability, which is omitted at this point for the sake ofbrevity. �Given that the invariance control scheme ensures that thesystem stays inside the admissible set, we argue that thesystem will always eventually come back to a state withoutactive constraints, where the augmented nominal controlasymptotically stabilizes the system. In case of active con-straints, the corrections will only influence τ ac in directionof the constraints. Perpendicular to the constraints, stillnominal control is applied. A phenomenon that mightoccur due to the I/O-linearisation of the constrains hi areinternal dynamics. It is shown in (Kimmel et al., 2016)that under Assumption 1, no additional internal dynamicsarise due to the performed augmentation. This motivatesintroducing the following assumption:

Assumption 4. The I/O-linearisation of hi ∀ i ∈ {1, 2, .., l}given system (2) yields stable internal dynamics.

3.3 Exemplary Control Design

For the experiments of this work, an invariance controlis implemented that achieves continuous actuation in anobstacle avoidance scenario. A non-augmented invariancecontrol in combination with a torque controlled robotswitches between nominal and corrective control on the

level of torques, resulting in discontinuous control in-puts τ c and state trajectories that are x(t) ∈ C0 whichdescribes a vectorial function that is continuous but notcontinuously differentiable. Our goal is to achieve

τ c(t)!∈ C0 , x(t)

!∈ C1 . (21)

Thus, we need to augment the system by v = 1. Insertinginto (9) and with Assumption 3 we obtain

xa =

[xτ c

]=

[f(x) +G(x)τ c

τ ac

](22)

as augmented dynamics with xa ∈ Rn+m denoting thestates of the augmented system. In collision avoidancescenarios as considered in this paper, constraints are seton position level and thus have a relative degree of ri = 2.For v = 1 this means that ra,i = 3 holds. From (Wolffand Buss, 2004) we know that we obtain an upper boundfor future values of a constraint hi of relative degree 3

assuming a constant h(3)i = γi by

hi(t+ tf ) ≤t3f6γi + hi(t) + tf hi(t) +

1

2t2f hi(t)︸ ︷︷ ︸

pi(t,tf ,γi)

(23)

with tf ∈ R, tf > t being a time in the future andthe upper bound polynomial pi(t, tf , γi). The predictedmaximum value of hi, given the relative degree of 3, isreached at tmax

tmax = − hiγi

+

√h2i

γ2i

− 2hiγi. (24)

Defining that the maximum reaction of the robot to the

constraint h(ra,i)i = γi we thus obtain a precise worst case

prediction and define the invariance function by

Φi =

{h(t) , (I) ∨ (II)

pi(t, tmax, γi) , else(25)

with the sets(I) = {x,η(t) | hi ≤ 0 ∧ hi ≤ 0}(II) = {x,η(t) | h2

i − 2hiγi < 0}.This gives us all we need to determine whether constraintsare active and when to use (5)-(7) to calculate the correc-tive control. Therefore, a valid choice of zc,i is necessary.To stay inside the admissible set, one needs to assure thatthe invariance functions of active constraints are locallydecreasing at the boundaries ∂Φ of the set {Φi ≤ 0,∀i ∈{1, 2, .., l}} which is thereby rendered positively invariant.Once the system entered, it will always stay inside thisinvariant set. Two further goals that influence our choiceof zc,i is that we want to apply nominal control wheneverpossible, and that unnecessary chattering at the bound-aries should be avoided. We therefore propose to deviatefrom the simple choice of setting zc,i = γi, ∀i ∈ {i|Φi ≥ 0}and to apply

zc,i =

{zno,i , (III)min(zno,i, γi) , (IV)min(zno,i, 0) , else

(26)

with the sets (III) and (IV) defined as:(III) = {x,η, η, η|Φi < 0 }(IV) = {x,η, η, η| [Φi ≥ 0 ∧ (hi > 0 ∨ hi > 0)]∨(Φi > 0 ∧ hi = 0 ∧ hi = 0)}. Finally we insert v = 1and τ c = τ ac into (12) yielding

τ ano = τ no − k0(τ c − τ no) (27)

robotimpedancecontrol

invariancecontrol

∫qd x

+ +

-τ no

τ c

k0

k0

ddt

Augmentation

constrainttrackingη

Fig. 2. Closed loop system with a (v = 1)-augmentedinvariance control scheme. The red area marks theaugmented invariance control implemented for theexperiments of this paper.

as the augmented nominal control. The resulting closedloop system is illustrated in Figure 2. The area indicatedin red marks the augmented invariance control. Comparedto the original scheme in Figure 1, nominal control is differ-entiated prior to supervision. Integration of the correctedsignal yields the input to the robotic system.Remark : Unlike e.g. potential field methods, augmentedinvariance control guarantees constraint adherence undersignificant dynamics. Additionally, it can be combinedwith a broad variety of nominal controls allows high flexi-bility and modularity in the control design in contrast toe.g. MPC. The optimal invariance control can be foundanalytically despite non-linear dynamics and boundariesmaking the approach computationally efficient and there-fore highly applicable when real-time matters as in robotforce control schemes.

4. EXPERIMENTAL EVALUATION

4.1 Setup and Procedure

To compare the approach of augmented invariance controlfrom Section 3 with a non-augmented version, both ap-proaches were implemented based on the robot operatingsystem ROS to perform obstacle avoidance in task spacewith a KUKA-LWR4+. Therefore the end-effector of theKUKA arm is here geometrically modelled as a sphere,centred at the last link of the arm. A spherical obstacleis simulated to pass the end-effector, forcing the arm todeviate in order to keep the hard constraint formulated onthe Euclidean distance of the surfaces of end-effector andobstacle geometry. This scenario is illustrated in Figure3. The simulation of the obstacle allows for identical con-ditions for both approaches such that the results yield avalid comparison of performance. Parameters appearing inboth approaches where therefore chosen equally as listedin Table 1. A Cartesian formulation of impedance controlis applied as nominal control in the shape of

τ c = JT (q)(−KC(p− pd)−DCp) + g(q) (28)

where KC ∈ R and DC ∈ R are positive stiffness anddamping gains and the Jacobian J(q) serves to map fromCartesian task space to joint space. The test scenarioand the resulting avoidance motion of the robot aredepicted in Figure 3. The spherical obstacle passes witha velocity vobs = 0.1m/s by the end-effector link from leftto right. A mainly vertical deviation of the robot is causedby the constrained Euclidean distance. After the obstaclepassed, nominal control brings the arm back to the desiredposition. Relevant data is published as topics using thepublish/subscribe framework of ROS. The experimental

Fig. 3. Test scenario for the comparison augmented vs.non-augmented invariance control for obstacle avoid-ance. Hard constraints are defined on the Euclideandistance between end-effector link (yellow) and thedynamic obstacle (cyan).

augmentednon-augmentednominal

3 4 5 6 7 8 9

0

−20

−40

contr

olac

tion

[N]

time [s]

Fig. 4. Vertical forces applied during obstacle avoidancewith augmented (red) and non-augmented invari-ance control (blue). The augmented version shows asmooth behaviour with lower absolute forces and lesschattering. The depicted forces do not contain gravity.

data was recorded in ROS-bags and is analysed in thefollowing section.

4.2 Results

The experiments successfully demonstrated the stabilityof the approach as well as the benefits of augmentinginvariance control. A major difference is observed w.r.t.the actuation τ c which is illustrated in Figure 4. It showsvertical forces, as this is the main direction of both cor-rective control and motion in the experiments. The non-augmented version executes jumps between nominal andcorrective control due to chattering, while the augmentedcontrol achieves the desired smooth behaviour in termsof a continuous actuation. Besides avoiding jumps, theaugmented implementation manages to perform the obsta-cle avoidance with far lower absolute forces. On average,the integrated absolute actuation during the experimentswas reduced by about 89.7% in average with the aug-mented approach. The maximum forces are even reducedby about 40.1N which represents a reduction of circa92.6% compared to the non-augmented approach. Theseresults indicate a drastically reduced load for the robot

Table 1. Key parameters for the experiment comparingaugmented and non-augmented invariance control inan obstacle avoidance scenario.

parameter non-augmented augmentedKC 300.0N/m 300.0N/mDC 100.0Ns/m 100.0Ns/mreff 0.1m 0.1mrobs 0.2m 0.2mγ −1.0 −1.0k0 − 1.0s−1

vobs 0.1m/s 0.1m/sfcontrol,Hz 500Hz 500Hz

time [s]

con

stra

int

h[m

]

augmentednon-augmented

3 4 5 6 7 8 9

0

−0.04

−0.08

Fig. 5. Constraint h during obstacle avoidance. Bothapproaches achieve adherence to the constraint h ≤ 0.

time[s]

act

ive

[1]

non-augmented, ' 13.2Hzaugmented, ' 4Hz1

0

3 4 5 6 7 8 9

Fig. 6. Chattering during obstacle avoidance. The averagechattering frequency is reduced by about 70% withaugmented invariance control.

actuation with the proposed control. Also both approachesmanage to keep the constraint h which is plotted in Figure5. While the augmented approach shows minor initialoscillation at the boundary h = 0, the non-augmentedcontrol sticks close to the boundary from the beginning ofthe avoiding motion at about 3.5s. This oscillation comesfrom the type of augmentation integral filter and can bereduced by adjusting filter parameters. This is beyondthe scope of this paper and may be regarded as higherrelative degree chattering as is observable in Figure 6,which shows when the constraint is active using Booleanvariables (0 for non-active, 1 for active). At the boundariesthe augmented approach outperforms the non-augmentedversion. Some chattering occurs for both approaches dueto the discrete time implementation. Both control schemesare evaluated at a frequency of 500Hz, still the exact timewhen Φ = 0 is reached is slightly missed causing chatteringat the boundaries of the admissible set. These violationsdue to chattering need to be separately addressed (Kim-mel and Hirche, 2014) and are not in the scope of thispaper. A considerable difference between augmented andnon-augmented approach is obvious regarding the averagefrequencies of chattering when the system slides alongthe h = 0 boundary (' 3.95Hz for augmented, ' 13.20Hz for non-augmented). In the conducted experimentsthe chattering frequency was reduced by about 70% inaverage by the augmentation showing another benefit ofthe proposed approach. The boundary violations due tochattering are at a sub-millimetre level and are considerednegligible. Figure 6 shows that the augmented versionreacts sooner to the approaching obstacle as the constraintis active by about 0.2s earlier. This is a result of the higherrelative degree of corrective control and can be influencedin terms of the parameter γ. The experiments also showedthat the augmented approach takes longer to return tothe desired position after the obstacle avoidance motion.This is because the augmented version does not instan-taneously jump back to nominal control but smoothlytracks it according to (27), causing a less direct trackingbehaviour. This can be improved by introducing more

complex dynamics fa and Ga, but is beyond the scopeof this work.

5. CONCLUSION

This paper addresses the problem of augmented invariancecontrol for impedance controlled robots. We show thatstability characteristics are maintained when augmentingan impedance-type nominal control for invariance control.These results are validated in a collision avoidance exper-iment. The comparison to non-augmented invariance con-trol shows promising results, especially regarding smooth-ness and control action. Lower control inputs to the systemcould be achieved by means of augmentation. As a positiveside effect the influence of chattering was reduced. In turn,the workspace of the robot may be slightly reduced, ascorrective actions at higher relative degrees may requireearlier corrective reaction for preventing boundary viola-tions. Future work will address the performance orienteddesign of the control scheme.

ACKNOWLEDGEMENTS

This work was supported by the EU Horizon2020 projectRAMCIP, under grant agreement no. 643433.

REFERENCES

Angeli, D. and Mosca, E. (1999). Command gover-nors for constrained nonlinear systems. IEEE Trans-actions on Automatic Control, 44(4), 816–820. doi:10.1109/9.754825.

Khalil, H.K. (1996). Nonlinear systems. Prentice Hall,Upper Saddle River, (N.J.).

Kimmel, M., Jahne, C., and Hirche, S. (2016). Aug-mented invariance control for systems with smoothnessconstraints. In Proceedings of the 55th Conference onDecision and Control (CDC). Las Vegas, USA.

Kimmel, M. and Hirche, S. (2014). Invariance Control withChattering Reduction. IEEE Conference on Decisionand Control, 68–74. doi:10.1109/CDC.2014.7039361.

Kimmel, M. and Hirche, S. (2015). Active Safety Controlfor Dynamic Human-Robot Interaction. 4685–4691. doi:10.1109/IROS.2015.7354044.

Mayne, D.Q., Rawlings, J.B., Rao, C.V., and Scokaert,P.O. (2000). Constrained model predictive control:Stability and optimality. Automatica, 36(6), 789–814.doi:10.1016/S0005-1098(99)00214-9.

Murray, R.M., Sastry, S.S., and Zexiang, L. (1994). AMathematical Introduction to Robotic Manipulation.CRC Press, Inc., Boca Raton, FL, USA, 1st edition.

Rauscher, M., Kimmel, M., and Hirche, S. (2016). Con-strained robot control using control barrier functions. InProceedings of the IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 7. Daejeon,Korea.

Rimon, E. and Koditschek, D. (1992). ExactRobot Navigation Using Artificial Potential Functions.Robotics and Automation, IEEE, 8(5), 501–518. doi:10.1109/70.163777.

Wolff, J. and Buss, M. (2004). Invariance control designfor nonlinear control affine systems under hard stateconstraints. Proceedings of the NOLCOS 2004, (2), 711–716.