Tracking control for underactuated non-minimum phase multibody systems

We consider tracking control for multibody systems which are modelled using holonomic and nonholonomic constraints. Furthermore, the systems may be underactuated and contain kinematic loops and are thus described by a set of differential-algebraic equations that cannot be reformulated as ordinary differential equations in general. We propose a control strategy which combines a feedforward controller based on the servo-constraints approach with a feedback controller based on a recent funnel control design. As an important tool for both approaches we present a new procedure to derive the internal dynamics of a multibody system. Furthermore, we present a feasible set of coordinates for the internal dynamics avoiding the effort involved with the computation of the Byrnes-Isidori form. The control design is demonstrated by a simulation for a nonlinear non-minimum phase multi-input, multi-output robotic manipulator with kinematic loop.


Introduction
In the present paper, we propose a combined feedforward and feedback tracking control strategy for underactuated non-minimum phase multibody systems. We follow a popular approach to two degree of freedom controller design as proposed e.g. in [48]. The feedforward control input design is based on a reference model of the system such that, if the model truthfully captures reality, exact tracking of a given reference signal by the output is achieved. We utilize the so-called servo-constraints approach for feedforward control design.
In order to compensate (inevitable) modeling errors, uncertainties, disturbances, noise, etc. an additional feedback loop is used to stabilize the system around the given reference signal. Since a robust feedback controller is desired and the output must respect prescribed error margins around the reference signal we use the funnel controller first proposed in [37]. Since the funnel controller presented in [37] is not feasible for non-minimum phase systems we use an extension recently developed in [8].
An important tool both in feedforward and feedback control design is the Byrnes-Isidori form, which allows a decoupling of the internal dynamics of the system. However, a calculation of the Byrnes-Isidori form and the accompanying nonlinear transformation requires a lot of computational effort in general. The approach presented in the present paper avoids this computation. In the feedforward control design, the servo-constraints constitute an approach which does not require the Byrnes-Isidori form for the solution of the inverse model. For the feedback control design, we present a new approach to choose a set of variables for the internal dynamics of the system directly in terms of the system parameters -circumventing the Byrnes-Isidori form. The feedback controller is then based on this representation of the internal dynamics.
More details of the considered system class and the proposed control methodology are given in the following. Furthermore, we recall the concept of vector relative degree and the Byrnes-Isidori form.

R ≥0
= [0, ∞) Gl n the group of invertible matrices in R n×n L ∞ loc (I → R n ) the set of locally essentially bounded functions f : I → R n , I ⊆ R an interval L ∞ (I → R n ) the set of essentially bounded functions f : I → R n f ∞ = ess sup t∈I f (t) W k,∞ (I → R n ) the set of k-times weakly differentiable functions f : the set of k-times continuously differentiable functions f : restriction of the function f : V → R n to W ⊆ V

System class
We consider multibody systems which possibly contain kinematic loops as well as holonomic and nonholonomic constraints. The equations of motion are given bẏ M(q(t))v(t) = f q(t), v(t) + J(q(t)) µ(t) + G(q(t)) λ (t) + B(q(t)) u(t), 0 = J(q(t))v(t) + j(q(t)), 0 = g(q(t)), with the generalized coordinates q : I → R n and generalized velocities v : I → R n (in the case of no constraints), where I ⊆ R ≥0 is some interval, the generalized mass matrix M : R n → R n×n , the generalized forces f : R n × R n → R n , the holonomic constraints g : R n → R and G : R n → R ×n , the nonholonomic constraints J : R n → R p×n and j : R n → R p (which may incorporate a change of physical units), the input distribution matrix B : R n → R n×m , the output measurement function h : R n × R n → R m .
The functions u : R → R m are the inputs that influence the multibody system (1) in affine form. We explicitly allow for m < n − , which means that, if no nonholonomic constraints are present, underactuated multibody systems are encompassed by this formulation. The affine input has the interpretation of a force or torque, however standard industrial actuators are typically velocity controlled and not force controlled, which may lead to different system properties [42,Sec. 4.4]. In the present paper, we assume that the actuators are force/torque controlled and that the multibody system is given in the form (1).
The functions y : R → R m are the outputs associated with the multibody system (1) and typically represent appropriate measurements. The generalized forces f usually encompass several terms, including applied forces, Coriolis, centrifugal and gyroscopic forces. The functions λ : R → R and µ : R → R p are the Lagrange multipliers corresponding to the holonomic and nonholonomic constraints, resp. The functions in (1) are assumed to be sufficiently smooth and to satisfy the natural assumptions + p + m ≤ n and ∀ q ∈ R n : M(q) = M(q) > 0 and g (q) = G(q). (2) Let us stress that invertibility of M(q) is not sufficient, we require that it is positive definite. This follows for example from deriving the equations of motion using the direct application of d'Alemberts principle in the form of Lagrange.
System (1) is a differential-algebraic system and its treatment needs special care. For an overview of important concepts for differential-algebraic control systems we refer to [6,40,47] and the series of survey articles [36].

Vector relative degree
An important property of the system (1) is its vector relative degree, which, roughly speaking, is the collection of numbers of derivatives of each output component needed so that the input appears explicitly. We briefly recall the required concepts from [39] and, to this end, consider a general nonlinear system affine in the control given bẏ where f : R d → R d , g : R d → R d×m and h : R d → R m are sufficiently smooth and x : I → R n , where I ⊆ R ≥0 is some interval. Denote the components of h by h 1 , . . . , h m , and recall the definition of the Lie derivative of h i , i ∈ {1, . . . , m}, along a vector field f at a point z ∈ U ⊆ R d , U open: where h i is the Jacobian of h i , i.e., the transpose of the gradient of h i . We may gradually define L k f h i = L f (L k−1 f h i ) with L 0 f h i = h i . Furthermore, denoting with g j (z) the columns of g(z) for j = 1, . . . , m, we define (L g h i ) (z) := [(L g 1 h i )(z), . . . , (L g m h i )(z)] ∈ R 1×m . Now, in virtue of [39], the system (3) is said to have vector relative degree (r 1 , . . . , r m ) ∈ N 1×m on U, if for all z ∈ U we have: where Γ : U → Gl m denotes the high-frequency gain matrix. If r 1 = . . . = r m =: r ∈ N, then system (3) is said to have strict relative degree r.
If system (3) has vector relative degree (r 1 , . . . , r m ) ∈ N 1×m on an open set U ⊆ R d , then there exists a (local) diffeomorphism Φ : The last equation in (4) represents the internal dynamics of system (3). The diffeomorphism Φ can be represented as . . .
where φ i : U → R, i =r + 1, . . . , d are such that Φ (z) ∈ Gl d for all z ∈ U. By a distribution, we mean a mapping from an open set U ⊆ R d to the set of all subspaces of R d . If the distribution x → G (x) := im(g(x)) in (3) is involutive, then the functions φ i in (5) can additionally be chosen such that by which p(·) = 0 in (4)

Control methodology
As in our preliminary work [13] we follow the popular controller design methodology for mechanical systems undergoing large motions, where a feedforward controller is combined with a feedback controller, see e.g. [48]. Both controllers are designed independently for tracking of a reference trajectory y ref : R ≥0 → R m . The feedforward control input u ff is designed using a reference (inverse) model of the system, while the feedback control input u fb is applied to the actual system. The latter may deviate from the reference model; the situation is depicted in Figure 1. As in [13] the feedforward control input u ff is computed using the method of servo-constraints introduced in [19], cf. also [20,21]. In this framework the equations of motion of the multibody system (1) are extended by constraints which enforce the output to coincide with the reference trajectory y ref . The resulting set of differential-algebraic equations (DAEs) is solved numerically for the inverse system, by which the feedforward control input u ff is obtained. The details of the method are presented in Section 4.
Servo-constraints have been successfully used for the control of rotary cranes [18], overhead gantry cranes [20,43], (infinitely long) mass-spring chains [2,32] and flexible robots [24]. The addition of the servo-constraints to the system may result in a higher differentiation index, as shown in [25]; see [22,33] and also [40] for a definition of the differentiation index. A higher index causes difficulties in the numerical solution and hence index reduction methods are frequently used, cf. [35]. Popular approaches are index reduction by projection onto the constrained and unconstrained directions [20] and index reduction by minimal extension [1,17].
The feedback control input u fb is generated by a dynamic state feedback of the forṁ We choose a feedback control design based on the funnel control methodology [11,37]. Recently, this method has been extended to linear non-minimum phase systems in [8].
Based on a linearization of the internal dynamics, the approach from [8] has been used for tracking control of a nonlinear non-minimum phase robotic manipulator in [10]. The objective of funnel control is to design a feedback control law such that in the closed-loop system the tracking error e(t) = y(t) − y ref (t) evolves within the boundaries of a prescribed performance funnel which is determined by a function ϕ belonging to where k = max i=1,...,m r i and (r 1 , . . . , r m ) is the vector relative degree of the considered system. Furthermore, all involved signals should remain bounded. The boundary of F ϕ is given by the reciprocal of ϕ, see Figure 2. The case ϕ(0) = 0 is explicitly allowed, which means that no restriction is put on the initial value and the funnel boundary 1/ϕ has a pole at t = 0 in this case.
Each performance funnel given by F ϕ is bounded away from zero, since boundedness of ϕ implies existence of λ > 0 such that 1/ϕ(t) ≥ λ for all t > 0. The funnel boundary is not necessarily monotonically decreasing and widening the funnel over some later time interval might be beneficial, e.g. in the presence of periodic disturbances.
The detailed design of a feedback controller of the form (7) yielding the feedback control input u fb is presented in Section 5 and based on the recent approach [8,10]. The funnel controller was already successfully applied e.g. in temperature control of chemical reactor models [38], DClink power flow control [45], voltage and current control of electrical circuits [16], control of peak inspiratory pressure [44], adaptive cruise control [14,15] and control of industrial servo-systems [34] and underactuated multibody systems [13].

Organization of the present paper
The present work is oranized as follows. In Section 2 we present a novel procedure to compute the internal dyamics of a multibody system. To this end, we introduce an auxiliary input and output to avoid the DAE formulation. In Section 3 we present a feasible set of coordinates of the internal dynamics. Section 4 provides a presentation of the open-loop control strategy based on servo-constraints and in particular details the case of unstable internal dynamics. In Section 5 we present a funnel-based feedback-controller design which is based on recently developed results for linear non-minimum phase systems. Finally, in Section 6 we apply the findings of the present paper to achieve output tracking of a nonlinear, non-minimum phase, multi-input, multi-output multibody system with kinematic loop, namely a robotic manipulator which is described by a DAE that cannot be conveniently reformulated as an ordinary differential equation (ODE).

Computing the internal dynamics
In this section we present a novel approach to decouple the internal dynamics of the multibody system (1) avoiding the DAE formulation. In principle, it would be possible to obtain a Byrnes-Isidori form for a DAE directly, see [7]. Here we define auxiliary inputs and outputs by removing the constraints and adding the Lagrange multipliers to the input functions. For this auxiliary ODE system we derive the Byrnes-Isidori form as in (4) and then add the constraints which have been removed before, thus obtaining the internal dynamics.

The general case
Consider (1) and define an auxiliary input and output as follows: With the state variables x 1 = q, x 2 = v and x = (x 1 , x 2 ) we may now consider the auxiliary ODE system (omitting the argument t for brevity) which is of the form (3), and decouple its internal dynamics using the Byrnes-Isidori form. After that we add the constraints y aux,1 = O 1 (x) = 0 and y aux,2 = O 2 (x) = 0 to derive the internal dynamics of (1). First we calculate the vector relative degree of (10). Let U ⊆ R 2n be open such that the following Lie-derivatives exist for x = (x 1 , x 2 ) ∈ U: as well as are invertible it follows that a vector relative degree of system (10), if it exists, is of the form r = (r 1 , r 2 , r 3 ) with r 1 = (1, . . . , 1) ∈ N 1×p , r 2 = (2, . . . , 2) ∈ N 1× and some r 3 ∈ N 1×m . In the following, we restrict ourselves to the case where the multi-index r 3 has equal entries, i.e., r 3 = (r, . . . ,r) ∈ N 1×m for somer ∈ N.
Then it is necessary that, for all x ∈ U, The first two block rows are given above, the last row strongly depends on the shape of the function h. In the special case that inputs and outputs are colocated we may explicitly calculate Γ 3 (x) and show that the relative degree is well defined, see Section 2.3. Now we can compute the Byrnes-Isidori form of (10) using the state space transformation as in (5), which in this case reads . . .
wherer = p + 2 +rm and φ i : U → R for i =r + 1, . . . , 2n. Then, in the new coordinates the system has the forṁ If the distribution x → K (x) = im K(x) is involutive, then the functions φ i can be chosen such that, for all x ∈ U, by which p(·) = 0, cf. Section 1.3. For the auxiliary ODE (10) we have the following result.
Proof First, recall that a distribution D(·) = span{d 1 (·), ..., d q (·)} is involutive if, and only if, for any 0 ≤ i, j ≤ q and x ∈ U, we have where [d i , d j ](x) denotes the Lie bracket of d i and d j at x. Observe that K in (10) has the following structure: for all x = (x 1 , x 2 ) ∈ U. Therefore, using (13) the distribution K (·) is involutive on U.
Lemma 2.1 yields that it is always possible to choose the diffeomorphism Φ such that p(·) = 0 in (12), which we assume is true henceforth.
We are now in the position to invoke the original constraints O 1 (x) = 0 and O 2 (x) = 0, which in the new coordinates simply read ξ 1 = 0 and ξ 2 = 0. Therefore, we obtain the system which is equivalent to the original multibody system (1).
Considering the first two equations in (14) we obtain that can be written as and is invertible, it follows that K(x) has full column rank for all x ∈ U. Therefore, J(x 1 ) G(x 1 ) B(x 1 ) has full column rank and the matrix J(x 1 ) G(x 1 ) has full column rank as well, hence, by using that M is pointwise positive definite, we have that A(x 1 ) := is positive definite for all x 1 ∈ [I n , 0]U. Therefore, we can solve for the Lagrange multipliers as follows: for some appropriately chosen function b 4 . Inserting this into the third equation in (14) gives Note that it is not clear in general whether the matrix S(x) is invertible for some or all x ∈ U. The multibody system (1) is then equivalent to where b 5 is some appropriate function,S(·) = S Φ −1 (0, ·) and the internal dynamics are completely decoupled in (16).

Remark 2.2
We note that an alternative to the above described approach would be to simply differentiate the holonomic and nonholonomic constraints in (1) and solve for the respective Lagrange multipliers. When these have been inserted in (1) (and the still present holonomic and nonholonomic constraints are neglected) an ODE of the form (3) is obtained, for which the Byrnes-Isidori form as in Section 1.3 can be computed. This approach has some disadvantages compared to our approach presented above: 1) Since differentiating the constraints does not remove them, it must be guaranteed that any solution of the resulting ODE is also a solution of the overall DAE, i.e., it satisfies the constraints for all times. Nevertheless, this can be ensured, provided that the initial values are chosen appropriately. 2) It cannot be avoided to solve for the Lagrange multipliers first, while in our approach the internal dynamics in (16) are the same as obtained for the auxiliary system in (12), since p(·) = 0 in the latter by Lemma 2.1.
The system (12) can be computed without solving for the Lagrange multipliers and hence requires much less computational effort. Nevertheless, it is still important to choose the functions φr +1 , . . . , φ 2n such that L K φ i = 0 for i =r + 1, . . . , 2n; this problem is discussed in Section 3.

Position-dependent output with relative degree two
In the special case h(q, v) = h(q), i.e., h : R n → R m and y(t) = h q(t) , we obtain some additional structure. First of all, we may compute that whencer ≥ 2. Assuming thatr = 2 (a typical situation) we obtain, for all x = (x 1 , x 2 ) ∈ U, which is the Schur complement of A(x 1 ) in Γ (x).

The colocated case
Now we consider the special case that the inputs and outputs of system (1) are colocated, which means that h(q, v) = h(q) as in Section 2.2 and We show that in this case the vector relative degree with respect to O 3 isr = 2. First we calculate, for all Then we obtain the high-gain matrix Therefore, we find that, for some i.e., the latter matrix has full column rank. Roughly speaking, this condition means that (the components of) the Lagrange multipliers µ and λ and the inputs u influence the system in a linearly independent (i.e., non-redundant) way, because in has full column rank, thus the auxiliary input u aux does not have any redundant components.
Then the transformation which puts (10) into Byrnes-Isidori form is given by is invertible for all x ∈ U. Using the new coordinates and invoking the original constraints O 1 (x) = 0 and O 2 (x) = 0 we may rewrite the original multibody system (1) in the form (14), which in the colocated case simplifies to As in the general case we may solve for the Lagrange multipliers µ and λ and insert this in (18), thus (15) becomes It is a well-known result that since Γ (x) is positive definite, then the Schur complement S(x 1 ) is positive definite as well. Therefore, the decoupled system (16) is of the form whereS is pointwise positive definite.

A feasible set of coordinates for the internal dynamics
In this section we derive a representation of the internal dynamics which depends on the auxiliary output y aux and its derivativeẏ aux . Consider a system (1) with holonomic and nonholonomic constraints and position-dependent output. Further set H(x 1 ) =: h (x 1 ) and assume that, for some open set (20) for all x 1 ∈ U 1 , which means that the high-gain matrix Γ (·) is invertible on U := U 1 × R n . Then, following Section 2.2, the auxiliary ODE (10) with (again omitting the argument t for brevity) u aux = (µ , λ , u ) and y aux = For the internal dynamics η = φr +1 (x), . . . , φ 2n (x) we make, with some abuse of notation, the structural ansatz where If we now rearrange (21), i.e., we set where P ∈ Rr ×r is some permutation matrix, we see that (ξ 1 , ξ 2 ) and (η 1 , η 2 ) are of similar structure. Since Φ is a diffeomorphism and system (10) has vector relative degree (1, . . . , 1, 2, . . . , 2) on U, its Jacobian is invertible on U: where * is of the form ∂ ∂ x 1 ζ (x 1 )x 2 with appropriate ζ : U 1 → R q×n and q ∈ N. Since we are interested in the case p(·) = 0 in (12) (which can be achieved by Lemma 2.1) we aim to find φ 2 : We summarize this in the following result.  (24) and (25) Now, we use this to show the existence of φ 1 . Since by (20) we have rk[G( . which is invertible on U 0 1 since by (20) the high-gain Γ (x 1 ) is invertible, and rk T 2 (x 1 ) = n − p − − m for all x 1 ∈ U 0 1 . Hence the second condition in (24) is satisfied on U 0 1 . Furthermore, equation (25) holds on U 0 1 by construction of φ 2 .
Lemma 3.1 justifies the structural ansatz for the internal state η in (22). Note that the construction of φ 1 in the proof of Lemma 3.1 is not unique. There is a lot of freedom in the choice of φ 1 , as e.g. E could be any matrix such that ET 2 (x 0 1 ) is invertible. Basically, φ 1 can be chosen freely up to (24). On the other hand, φ 2 is uniquely determined up to an invertible left transformation. To find all possible representations, assume that (24) and (25) hold. Then there exist P : Furthermore, P,V have pointwise full column rank, by which the pseudoinverse of V is given by (24) and (25) hold. Then φ 2 is uniquely determined up to an invertible left transformation. All possible functions are given by x 1 ∈ U 1 , for feasible choices of V satisfying (26).
Proof Assume that (24) and (25) hold, and hence we have (26) for some corresponding P and V . Observe that multiplying (26) from the left by V (x 1 ) † gives Invoking (25), we further obtain from (26) that and hence P is uniquely determined by M, J, G, H, B. Therefore, φ 2 is given as in (27). Furthermore, it follows from (26) that . Therefore, the representation of φ 2 in (27) only depends on the choice of the basis of ker[J( .
Then a short calculation shows the claimφ 2 ( Now, let φ 1 ∈ C 1 (U 1 → R n− −m ) and φ 2 ∈ C (U 1 → R (n−p− −m)×n ) be such that (24) and (25) are satisfied and V : U 1 → R n×(n−p− −m) as in (26). We continue deriving a representation of the internal dynamics. We defineȳ aux := [0, I +m ]y aux and observe that, using the first equation in (10), Then, with y aux, Thus, using (22) and (28) the dynamics of η 1 are given bẏ Now, since for a continuously differentiable ϕ : U 1 → R n , the Jacobian of which is invertible on U 1 by (24). In order to ensure that ϕ is a diffeomorphism on U 1 we need to additionally require that there exist a diffeomorphism ψ : U 1 → R n and ω : [0, ∞) → (0, ∞) which is continuous, nondecreasing and Then [9, Thm. 2.1] yields that ϕ : U 1 → W 1 := ϕ(U 1 ) is a diffeomorphism. Then we have To combine (30) with (29) we define the following functions as concatenations on W 1 : Therefore, we obtain the following representation of (29): (10), (22) and (25) we obtaiṅ Then the dynamics of η 2 are given bẏ Finally, we invoke the original constraints, which means that y aux,1 = 0 andȳ aux = 0 y . With this, and omitting the argument (ȳ aux , η 1 ) = (0, y , η 1 ) where it is obvious, the internal dynamics reaḋ Remark 3.3 If no nonholonomic constraints are present we may obtain further structure for the internal dynamics in (31). Consider the functions φ 1 and φ 2 as above and recall the concept of a conservative vector field. For U ⊆ R n open, a vector field ζ : U → R n is said to be conservative, if it is the gradient of a scalar field. More precisely, if there exists a scalar field σ : U → R such that σ (x) = ζ (x) . Now, if every row φ 2,i (·) of φ 2 (·) defines a conservative vector field, then the components of φ 1 (·) can be chosen to be multiples of the associated scalar fields. More precisely, if there exist . . . , λ n− −m ) and, likewise, φ 1,ϕ = Λ φ 2,ϕ . Hence via (25) and Lemma 3.2 the dynamics of η 1 in (31) simplify tȯ where the diagonal entries of Λ can be chosen as desired.
Hence, in this case the variables η 1 = φ 1 (q) can be interpreted as (transformed) positions and η 2 = Λ −1η 1 as velocities. Therefore, the states η = (η 1 , η 2 ) admit a partition which is structurally similar to that of the states (q , v ) of the multi-body system (1). In the presence of nonholonomic constraints this simplification is not possible, because of the different dimensions of φ 1 and φ 2 .

Remark 3.4
The computation of the Byrnes-Isidori form leading to the decoupling of the internal dynamics as in (16) often requires a lot of effort. The choice of variables for the internal dynamics presented in this section offers an alternative, leading to the internal dynamics given in (31) directly in terms of the original system parameters; a computation of the Byrnes-Isidori form is not necessary.

Servo-constraints approach
We seek to find a feedforward control u ff regarding the control methodology described in Section 1.4 by inverting the system model. For minimum phase systems, the transformed system (16) can be used as an inverse model for feedforward control. Solving the respective first equation for the input u with y = y ref yields the feedforward control signal u ff , provided the internal dynamics are integrated simultaneously. However, a direct integration of the internal dynamics is not possible for non-minimum phase systems, due to unstable dynamics. Thus, stable inversion is introduced in [26,28] for finding a bounded solution based on a boundary value problem (BVP). This yields a non-causal solution, in the sense that a system input u ff (t) = 0 is present in a preactuation phase t < t 0 before the start of the trajectory at time t 0 . Thus, the BVP is solved on a time interval T 0 , T f with T 0 < t 0 and T f > t f , with t f denoting the end of the desired trajectory y ref . Provided that the equilibrium of the internal dynamics is hyperbolic, there are n s eigenvalues in the left half plane and n u eigenvalues in the right half plane, such that n s + n u = 2n −r. The boundary conditions of the BVP are chosen such that the states η of the internal dynamics start on the unstable manifold W u 0 ∈ R n u of the equilibrium point η eq,0 at time T 0 . At the end T f , the boundary conditions constrain the states η to lie on the stable manifold W s f ∈ R n s of the equilibrium point η eq,f . This is summarized as The case of non-hyperbolic equilibria is discussed in [27]. The BVP consisting of the dynamics (16) subject to the constraints (32) yields a bounded solution to the internal dynamics. It was recently demonstrated in [29] that the boundary conditions (32) can be simplified as where the matrices L 0 ∈ R n 0 ×(2n−r) and L f ∈ R n f ×(2n−r) select in total n 0 + n f = 2n −r conditions to constrain n 0 states of the internal dynamics to the equilibrium point at time T 0 and n f states of the internal dynamics to the equilibrium point at time T f . Thus, the number of constraints equals the number of unknowns. For higher relative degree and multi-input, multi-output systems, the symbolic derivation of the internal dynamics, and especially of the stable and unstable manifolds, becomes tedious and complex. Thus, the method of servo-constraints introduced in [19] is applied to demonstrate an alternative approach. Motivated from modelling of classical holonomic mechanical constraints, such as bearings, the equations of motion (1) are appended by m servo-constraints which enforce the output to stay on the prescribed trajectory y ref ∈ W r,∞ (R ≥0 → R m ). This yields a set of DAEṡ to be solved numerically for the coordinates q and v, the Lagrange multipliers λ and µ and the input u. We stress that the initial values q 0 , v 0 , λ 0 , µ 0 , u 0 must be chosen so that they are consistent and the desired trajectory must be compatible with the possible motion of the system, i.e., it is required that a solution of (35) exists on R ≥0 . Note that (35) might have multiple solutions in the case that the desired output trajectory can be achieved by choosing different state trajectories. This is for example already the case for a 2-arm manipulator and most serial manipulators. However, from a control perspective, any solution that generates the desired output trajectory can be chosen. The freedom of choosing any solution can be utilized to optimize e.g. input energy. The chosen solution (q, v, λ , µ, u) : R ≥0 → R n ×R n ×R ×R p ×R m of (35) is used to define the feedforward control signal u ff := u. As a consequence, if all the system parameters and initial values of the multibody system (1) are known exactly and the control u = u ff is applied to it, then the closed-loop system has a solution which satisfies y = y ref . However, this result may not be true in the presence of disturbances, uncertainties and/or modelling errors. Then, a feedback controller can be added to reduce the tracking errors. This is demonstrated in the application example. While classical mechanical constraints are enforced by reaction forces orthogonal to the tangent of the constraint manifold, the servo-constraints (34) are enforced by the generalized input B q(t) u(t) which is not necessarily perpendicular to the tangent of the constraint manifold. Different configurations are distinguished depending on the vector relative degree of the associated auxiliary ODE (10), see [20]. For example, if inputs and outputs are colocated as discussed in Section 2.3, then the system input can directly actuate the output and the inverse model is in so-called ideal orthogonal realization. Recall that this corresponds to a relative degree two framework. If less than m components of the system input directly influence the system output, then the inverse model is in so-called mixed tangential-orthogonal or purely tangential realization.
Remark 4.1 It is interesting to note that in the colocated case, i.e., h (q) = B(q) for all q ∈ R n , and for y ref = 0 the servo-constraints act like holonomic constraints in (35), where the corresponding Lagrange multipliers are given by the inputs u(t) of the system. Therefore, the system (35) can again be interpreted as a multibody system (without any inputs or outputs) in this case.
While DAEs describing mechanical systems dynamics with classical constraints are of differentiation index 3, the set of DAEs (35) might have a larger differentiation index. As a rule of thumb for single-input single-output systems, the differentiation index is larger by one than the relative degree of the respective system, if the internal dynamics is modeled as an ODE and not affected by a constraint [25].
It is shown in [23] that the original stable inversion problem, solving the boundary value problem of (16) and (32) can be formulated and solved directly for the inverse model described by the DAEs (35). However, the derivation of the boundary conditions (32) is still tedious. In order to avoid the boundary conditions, it is proposed in [4] to reformulate the stable inversion problem as an optimization problem. Alternatively, it was recently shown in [29] that the simplified boundary conditions (33) can also be formulated for the inverse model described by the DAEs (35). They then read = L 0 q eq,0 v eq,0 λ eq,0 µ eq,0 u eq,0 and with the matrices L 0 ∈ R n 0 ×(2n+ +p+m) and L f ∈ R n f ×(2n+ +p+m) describing in total n 0 + n f = 2n + + p + m conditions, which constrain n 0 states to the initial equilibrium and n f states to the final equilibrium of the internal dynamics. Compared to the Byrnes-Isidori form, the resulting boundary value problem of the dynamics (35) subject to the constraints (36) greatly simplifies the problem setup, since less analytical derivations are required. The boundary value problem needs to be solved numerically. Since high index DAEs are difficult to solve numerically, see e.g. [35], different index reduction strategies can be applied. In the context of servo-constraints, index reduction by projection is proposed in [20], minimal extension is proposed in [1,17] and Baumgarte stabilization is applied in [5]. Available methods for solving BVPs are for example single shooting, multiple shooting or finite differences [3]. In the present paper, we utilize finite differences with Simpson discretization [46].

Funnel-based feedback controller
In this section we present a feedback-controller design for the multibody system (1), which is based on the funnel controller for systems with arbitrary vector relative degree recently developed in [12]. Furthermore, the design invokes a recently developed methodology for non-minimum phase systems from [8], which has been successfully applied to a nonlinear non-minimum phase robotic manipulator in [10] however, here we extend this methodology to the case of vector relative degree. The method is based on the introduction of a new output, which is differentially flat for the unstable part of the internal dynamics, cf. [30] for differential flatness. With respect to the new output, the overall system has a higher vector relative degree (in some of the components), but the unstable part of the internal dynamics is eliminated. This allows to apply the controller from [12] to the system (1) with new output and appropriate new reference signal. Since the required differentially flat output does often not exist for real-world nonlinear multibody systems (cf. [10]), we first linearize the internal dynamics, i.e., the second equation in (16), around an equilibrium (η 0 we obtain the linearized internal dynamics (with some abuse of notation, again using η as state variable) Note thatq andp may also be given in the special form (31), obtained using the approach presented in Section 3. In the next step, as in [10], we need to transform (37) so that the derivatives of y are removed from the right hand side. To this end, first definē Proceeding in this way we obtain (with some abuse of notation, again using η as state variable) where P = ∑r i=1 Q i−1 P i . In the following we assume that σ (Q) ∩ C + = / 0, so that the internal dynamics are not minimum phase.
In order to derive the controller design, we first require some assumptions which are extensions of those stated in [8]. Compared to the latter, here we increase each component of the vector relative degree separately (and possibly differently), instead of uniformly increasing the strict relative degree. To this end, choose T ∈ Gl 2n−r and k ∈ N such that We assume: (A1) There exist q ∈ {1, . . . , m} and k i ∈ N, i = 1, . . . , q such that k 1 + . . . + k q = k. Furthermore, there exist σ ∈ {−1, 1}, K 1 , . . . , K q ∈ R 1×k and K q+1 , . . . , K m ∈ R 1×m such that, for the matrix-valued functionS : W → R m×m , W = [0, I 2n−p−2 ]Φ(U), that appears in (16) (which is basically the high-gain matrix of this equation), we have that is positive definite for all w ∈ W . (A2) Let y ref ∈ Wr ,∞ (R ≥0 → R m ) be a given reference signal and W ∈ Gl k be such that where Q i ∈ R l i ×l i , i = 1, 2, with σ (Q 1 ) ⊆ C + and σ (Q 2 ) ⊆ iR. Then the equatioṅ Note that in the colocated case discussed in Section 2.3, the matrix S(·) (and hence alsoS(·)) is always pointwise positive definite, independent of the specific system parameters. Hence, the last condition in (A1) is satisfied, if, for instance, it is possible to choose K 1 , . . . K m such that We may now define the new output. Let, for η as in (38) and T as in (39), (η 1 , η 2 ) = T η (not to be confused with the variables introduced in Section 3). Theṅ With this we define y new : R ≥0 → R m by Similar to [8] it can be computed that the vector relative degree (of the auxiliary ODE (10)) increases when the output y is replaced by y new . More precisely, in the context of Section 2 we find that r 3 changes to (r +k 1 , . . . ,r +k q ,r, . . . ,r) ∈ N 1×m . In order to track the original reference signal y ref with the original output y, we need to introduce a new reference signal for system (1) with new output (41). The new reference signal is generated by the subsystem (40) corresponding to the unstable part η 2 when the output y is substituted by the reference y ref , i.e., Equation (42) will be part of the controller design and constitutes a dynamic part of the overall controller. In order for the controller from [12] to be applicable we require that As shown in [8] this is the case, if, using the notation from (A2), Furthermore, if the original reference y ref is generated by a linear exosystem (as in linear regulator problems, cf. [31]) of the forṁ where the parameters A e ∈ R n e ×n e ,C e ∈ R m×n e and w 0 ∈ R n e are known, and σ (A e ) ⊆ C − so that any eigenvalue λ ∈ σ (A e ) ∩ iR is semisimple (note that this guarantees y ref ∈ Wr ,∞ (R ≥0 → R m )), then η 0 2,ref can be calculated via the solution X ∈ R l 1 ×n e of the Sylvester equation Now, using the new output (41) and the new reference signal (42), the auxiliary tracking error is defined by The application of the funnel control law from [12] requires the derivatives of y new , and for implementation we need to express them in terms of the original variables q and v of the multibody system (1). We use the linearization (40) to obtain these derivatives. We replace η 2 in y new and its derivatives by the original coordinates from (1). To this end, we pretend that the coordinates of the linearization of the internal dynamics in (37) coincide with the original coordinates in the second equation of (16), which can be obtained via Φ(q, v) = (0, y,ẏ, . . . , y (r−1) , η).
Assume that the transformation from (37) to (38) is given by then we consider and its derivatives for i = 1, . . . , q. To this end, observe that, similar as in [8], we may calculate that Hence, we obtain the replacement rule , v(t)) + K iQ k iP y(t) for i = 1, . . . , q, where the derivatives y ( j) (t) are to be expressed via Lie derivatives as in (11) and read Similarly, we obtain the replacement rule for i = q + 1, . . . , m and j = 0, . . . ,r − 1.
With this, the application of the controller from [12] to a multibody system (1) with new output (41) and reference signal as in (42) leads to the following overall control law: . . .
We note that in (48) the variables e i, j are only short-hand notations and they can be expressed in terms of y new ,ŷ ref , ϕ i, j and the derivatives of these. Therefore, the expressions in (46) must be used for the replacement of the derivatives of y new in terms of the original coordinates q and v. Further note that the controller (48) implicitly assumes that the physical dimensions of all input and output variables coincide. For systems in which these variables have different physical dimensions a straightforward renormalization approach as presented in [13] can be used.

A robotic manipulator with kinematic loop
In this section, we illustrate our findings by a robotic manipulator which is described by a DAE that cannot be reformulated as an ODE and, at the same time, has unstable internal dynamics. The model is shown in Figure 3. This example is motivated by a similar flexible manipulator in [41]. It consists of three bodies with mass m i , length L i and moments of inertia I i . The first body is fixed to a translationally moving actuator on one end. The other end is connected to the end of body 2. The center of gravity of body 2 is mounted on a translationally moving actuator. Body 3 is attached to the end of body 2 by a linear spring-damper combination with coefficients c and D. Due to the described configuration, there exists a kinematic loop in the model and the resulting equations of motion in DAE-form cannot be easily reformulated as an ODE.
With the variables q = (s 1 , s 2 , α, β , γ) , λ = (λ 1 , λ 2 ) and the system input u = (u 1 , u 2 ) the equations of motion are given bẏ which are of the form (1) with f , g, M, G, B defined in (49) in a brief way. The model parameters are listed in Table 1. Note that a homogeneous mass distribution is assumed for bodies 1 and 2, while the center of gravity of body 3 can be varied by X 3 . The parameters of the reference model differ from the parameters for the forward simulation of the actual model in order to show robustness of the presented approach. For this example, the mass m 3 of the third arm is chosen 20% larger compared to the reference model. This might be the case if the robot is hoisting an unknown mass with its third arm.
The reference model is inverted according to Section 4 to obtain the feedforward control input u ff . Using identical parameters for both models would yield exact tracking of the feedforward controller. Therefore, the parameters for the simulated model differ from the ones of the inverted model. The funnel controller introduced in Section 5 decreases the tracking errors due to parameter missmatch of the pure feedforward control. However, note that the funnel controller (48) also requires some system parameters, for instance to compute the matricesQ andP, and hence relies on the reference model. Nevertheless, we will demonstrate that it is able to compensate the tracking error induced by feedforward control.
The output is chosen as where the first component is the horizontal position of the second body and the second component corresponds to an auxiliary angle describing the end-effector position for small angles γ. Both components may be used to approximate the end effector position by .
To demonstrate the tracking capability of the proposed controller design we choose the reference trajectory r app,ref in end-effector coordinates as the path In the following we aim to calculate the relative degree in a preferably large open set around the equilibrium point To this end, we choose (and this will be justified later) , which accordingly restricts the operating range of the system. Henceforth, we identify M, G, g and h with their restrictions to U q and f with its restriction to U. It is straightforward to check that the conditions in (2) are satisfied on U q . However, the inputs and outputs are not colocated here, i.e., (17) does not hold. It is then easy to see that for all and a Matlab calculation yields that detΓ (x) = −L 2 1 L 2 2 sin(α) sin(α +β ) .
Let us assume in the following that the third body has a homogeneous mass distribution, and hence Now, for x = (q , v ) ∈ U we find that sin(α) > 0, sin(α + β ) > 0 and cos γ > 2 3 , where the latter gives thus detΓ (x) < 0 and hence Γ (x) is invertible and the vector relative degree is well defined on U withr = 2. As a consequence we find thatr = 4 + 4 = 8 < 10 = 2n, so the system has nontrivial internal dynamics.
Furthermore, the Jacobian of Φ is given by (51). Invertibility of the matrix in (51) is equivalent to invertibility of the submatrix Since x ∈ U implies that sin α > 0 this matrix is invertible if, and only if, the determinant of the lower right 2 × 2 submatrix is nonzero, which is given by since cos γ > 2 3 . Therefore, Φ (x) is invertible everywhere for all x ∈ U. In the next step we aim to obtain the internal dynamics, i.e., the second equation in the decoupled system (16). First we calculate thaṫ In order to resolve the right hand sides in the above equation, we calculate the inverse of the diffeomorphism Φ. First observe that the new coordinates admit the representation in (52). Upon solving, and invoking the original constraints φ 1 = . . . = φ 4 = 0, with δ := 2L 3 L 2 +2L 3 we obtain for the re-quired coordinates s 2 , β , γ andṡ 2 ,β andγ that With this and φ 5 = y 1 , φ 6 = y 2 , φ 7 =ẏ 1 , φ 8 =ẏ 2 , φ 9 = η 1 and φ 10 = η 2 the internal dynamics are given by (53). Denote with J F ,x (x 0 , y 0 ) ∈ R p×q the Jacobian of a function F ∈ C 1 (R q × R k → R p ) with respect to x at a point (x 0 , y 0 ) ∈ R q × R k . Instead of linearizing (53) around the equilibrium point, in order to increase performance we linearize it around the starting point of the reference trajectory at t 0 and the end point at t f given by and combine both points linearly to obtain Q, P 1 and P 2 as in Section 5 from the function F = (F 1 , F 2 ) defined in (53) by With the coefficients where β 0 , β f are the values of β at t 0 and t f , resp., induced by the reference trajectory, these matrices are given by and we consider the corresponding system in the form as in (37), that is (52) We may observe that Q has a positive and a negative eigenvalue, hence the system is not minimum phase, not even locally. After the transformationη(t) = η(t) − P 2 y(t) we obtain where P = QP 2 . Now, we calculate that Q has eigenvalues 2 +C 0 c. Note, that for c > 0 we have µ 1 < 0 < µ 2 , thus (54) has a hyperbolic equilibrium, whence the linearized internal dynamics have an unstable part. Next we seek a transformation which diagonalizes Q and hence separates the stable and the unstable part of the internal dynamics: Using the transformationη(t) = T −1η (t) we obtain the linearized internal dynamics d dt According to (A1) we setP = [0, 1]T −1 P, then this has exactly the form of (40) in Section 5: whereQ,P satisfy (A1) with q = 1, k 1 = 1, however, strictly speaking the last condition in (A1) is only satisfied in a neighborhood of the equilibrium point. (A2) is satisfed with W = 1. Hereinafterη 2 plays the role of η 2 in Section 5 which is not to be confused with the expressions in (53). We have y new,1 (t) := K 1η2 (t), y new,2 (t) = K 2 y(t).
In order to implement the controller (48) we calculate the required derivatives of y new,1 in terms of the system's original variables. Recall y(t) = h(q(t)) andẏ(t) = h (q(t))v(t).
The simulation parameters are given in The feedforward control u ff is obtained from solving the boundary value problem described in Section 4, based on the inverse model in the DAE formulation (35). The boundary conditions are e.g. chosen as . The BVP solution includes a pre-actuation phase before the start of the trajectory at time t 0 = 0 s. However, here only the causal part for t ≥ t 0 is considered, such that where u bvp denotes the solution of the BVP. Note that the clipping of the input signal will also yield small tracking errors, which have to be reduced by the feedback controller. Furthermore, the solution of the BVP does not only yield the feedforward control u ff , but also the desired state trajectories q ref , v ref , λ ref , which may be used for reference in a feedback control law.
In the simulations, three controller configurations are compared. The controller C 1 is the combination u(t) = u fb (t) + u ff (t) of the feedback and feedforward strategy discussed above. The controller C 2 is pure feedback control u(t) = u fb (t) and the controller C 3 is pure feedforward control u(t) = u ff (t). The simulations over the interval 0 − 2 s are performed on MATLAB with the solver ode15s (AbsTol and RelTol at default values).
Snapshots of an animation of the simulation with controller C 1 are shown in Figure 4 for different time instances. The motion of the complete robot is visualized and the motion of the actuators attached to the moving bases with s 1 (t) and s 2 (t) can be seen. Moreover, the deflection γ(t) of the third, passive arm is visible at time t = 0.4 s.  Figure 5 shows the associated states s 1 (t), s 2 (t), α(t), β (t) and γ(t) during the motion, applying the combined controller C 1 . The oscillation of the third, passive body due to the motion is visible as an oscillation of γ(t). This motion cannot be actuated directly and is therefore responsible for the involved controller design.  Figure 6 shows the end-effector position in the twodimensional space under all three controller configurations C 1 -C 3 . While at the beginning a good tracking is achieved, the pure feedforward controller C 3 does not reach the desired final position at (e 1 , e 2 ) = (0.9, −0.9) m. While both controllers C 1 and C 2 reach the correct desired final position, the controller C 1 shows smaller tracking errors over the course of the trajectory. The tracking errors are evaluated in the following.   Figure 7(b) the errors in end-effector coordinates r app −r ref under the controllers C 1 , C 2 and C 3 , resp. The pure feedforward controller C 3 cannot compensate the increasing tracking errors due to the mass m 3 , which is larger in the simulated model compared to the reference model used for controller design. In contrast, both controllers C 1 and C 2 , which include a feedback channel, reduce the tracking error after an initial growth period. Applying the combined controller C 1 results in the smallest tracking error at the end time t f = 2 s. Moreover, the cumulative error of the controller C 1 is considerably smaller compared to the controller C 2 . The controller C 1 reduces the cumulative error of C 2 by 40% and 50%, resp., for the coordinates of y and r app .   In Figure 8 the input functions u 1 and u 2 are depicted. The controller C 3 shows the smooth feedforward control signal from model inversion of the reference model. The funnel controller C 2 varies strongly, especially in the first component u 1 . These large derivatives result in large accelerations of the physical system and should be avoided in order to reduce the loads on the mechanical parts. This can be achieved by the controller C 1 . Since it includes the smooth feedforward signal, which roughly moves the system on the desired path, the work load of the funnel feedback controller is reduced and the peaks and strong variations of the input signal can be completely avoided here. The input signals of C 1 are smooth and therefore reduce the loads on the mechanical parts of the system.   In Figure 9 the error norm ē(·) and the funnel boundary ϕ(·) −1 under the controllers C 1 and C 2 are depicted. In both controller configurations the error lies within the funnel boundary. However, in the scenario C 2 when only the feedback controller is applied, the error is closer to the funnel boundary which results in a larger input, cf. Figure 8.

Summary and Conclusion
In the present work we successfully combined a feedforward control strategy based on servo-constraints with a novel high-gain feedback controller design in order to achieve output tracking for a multibody system with kinematic loop and unstable internal dynamics. First, we introduced auxiliary inputs and outputs in order to avoid the DAE formulation and decouple the internal dynamics. Then we derived a feasible set of coordinates for the internal dynamics which allows a decoupling in terms of the original system parameters and avoids an explicit calculation of the Byrnes-Isidori form.
In order to apply the recent result from [8] on funnel control of linear non-minimum phase systems, we considered a linearization of the internal dynamics of the multibody system. Furthermore, a system inversion based on servo-constraints is applied as a feedforward control. Due to the unstable internal dynamics, this includes solving a boundary value problem beforehand. As a proof of concept we applied the combination of both control strategies to a nonlinear multi-input, multioutput multibody system with unstable internal dynamics in a simulation and compared the results. We found the combination of both controllers to have the best tracking performance in terms of the error between the system output and the reference signal. Moreover, the combination reduces peaks and strong variations in the input signal compared to the pure feedback strategy. The results motivate further research on the combination of open-loop and closed-loop control strategies for non-minimum phase systems.