Robust IDA-PBC for under-actuated systems with inertia matrix dependent of the unactuated coordinates: application to a UAV carrying a load

Abstract

This paper presents a methodology to solve the problem of robustification of Interconnection and Damping Assignment-Passivity Based Control (IDA-PBC) scheme for the case of under-actuated systems with inertia matrix dependent of the unactuated coordinates. Specifically, we analyze the robustness of the IDA-PBC strategy with respect to constant external disturbances. This algorithm requires adding an integral action with a particular change of coordinates in an outer-loop to the IDA-PBC strategy to reject constant external disturbances. The asymptotic stability of the proposed controller despite constant external disturbances is proved using the closed-loop Hamiltonian as a Lyapunov candidate function and LaSalle’s invariance principle. Finally, as a proof of concept, we have applied the proposed robust IDA-PBC strategy to an Unmanned Aerial vehicle that transports a payload suspended by a cable, which is a class of underactuated system with inertia matrix dependent of the unactuated coordinate. Satisfactory results in numerical experiments demonstrate the applicability of the method.

1 Introduction

The main objective of the Interconnection and Damping Assignment-Passivity Based Control (IDA-PBC) is to stabilize systems described by Port-controlled Hamiltonian (PCH) structures [1]. It is well-known that the IDA-PBC strategy is robust with respect to unmodeled dynamics and parametric uncertainty, in the sense that the stability is preserved [2,3,4]. However, its robustness with respect to external disturbances has not been fully investigated, particularly for the case when the dynamical system is under-actuated, which is very common in different physical systems. Thus, the IDA-PBC robustness with respect to external disturbances, for the case of under-actuated systems described by PCH models, is an open problem that has attracted the research interest in recent years.

For the case of fully actuated PCH systems, the literature reports several works, for example, [5] presents a method allowing to endow, to a PCH system with dissipation (PCHD), an integral action control depending on variables of relative degree higher than one. A drawback of this approach is that a set of algebraic equations must be solved to find a change of variables that allow building an extended PCHD preserving the form of the Hamiltonian function and the characteristic matrices of the original PCHD system. Also, a robust integral control to unmatched disturbances is proposed in [6] for the regulation of non-passive outputs. However, this approach is limited to the case of fully actuated PCH systems. Moreover, the authors in [7] discuss the robustness of nonlinear feedback systems with unknown perturbations considering the robust right coprime factorization and the passivity property. Also, an adaptive and integral control scheme of Permanent Magnet Synchronous Motors (PMSM) in the PCH framework is designed in [8]. First, a speed tracking controller is developed for the nominal PMSM model with the IDA-PBC approach. Then, a second control term is designed to deal with errors caused by unknown or uncertain parameters. Finally, a third integral control term is provided to compensate the unknown disturbances. The robustness of energy shaping controllers was enhanced for fully actuated mechanical systems with external disturbances in [9], by embedding in the dynamic state feedback, an integral action, along with the gyroscopic and damping forces. These previous works provide alternative solutions to the IDA-PBC robustification problem only for fully actuated systems. Unfortunately, the extension of these approaches for the case of underactuated systems is not straightforward.

For the case of under-actuated PCH systems, on the one hand, there exist works dedicated to systems with a constant inertia matrix. For example, [10] presents experimental results of an IDA-PBC controller in an inertia wheel inverted pendulum platform. The results show the robustness of the controller with respect to external disturbances, however, it can also be appreciated that the controller cannot deal with static errors in the presence of unmatched disturbances. Ryalat et al. [11] proposes to solve the robustification of IDA-PBC with respect to constant external disturbances for an under-actuated mechanical system. Nevertheless, only a simplified case is considered, where the system has two degrees of freedom, constant inertia matrix, and under-actuation degree one. Moreover, in [12] the robustness improvement to vis-à-vis matched input disturbances is addressed for an IDA-PBC with application to a class of under-actuated mechanical system. This was achieved with the combination of the IDA-PBC algorithm with an adaptive control technique.

On the other hand, to the best of our knowledge, there exist only two works that provide a possible solution to the problem of robustification for under-actuated systems with variable inertia matrix: [13, 14], moreover, [15] presents a correction to the paper [14]. These works address the design of a robust IDA-PBC for under-actuated mechanical systems subject to vis-à-vis matched, constant, and unknown disturbances. However, the design is only applicable for under-actuated PCH systems where the closed-loop mass matrix is constant, and the mass matrix is independent of the unactuated coordinates. This disadvantage means that the design cannot be used for a wide range of applications, where the inertia matrix is dependent of the unactuated coordinates, for example: the cart-pole system, ball and beam, pendubot, overhead crane, Unmanned Aerial Vehicle (UAV) transporting a cable-suspended payload, among others.

In summary, methods for under-actuated systems, where the mathematical models have a constant inertia matrix or an inertia matrix independent of the unactuated coordinates, have been proposed recently in the literature. Nevertheless, the above is only a partial solution to the problem of robustification of IDA-PBC to reject constant external disturbances due to the fact that there exists a wide range of systems that do not satisfy these conditions, i.e., that the mathematical models have an inertia matrix dependent of the unactuated coordinates and the effect of disturbances may reduce the efficiency of the control algorithm. Such systems, to the best of our knowledge, have not been studied in previous works. The main contribution of this work is to propose a solution to the aforementioned problem.

Motivated by the previous considerations, in this paper, we offer a robust IDA-PBC approach to solve the problem of constant external disturbances rejection for under-actuated systems, where the mathematical models have either inertia matrices dependent of the unactuated coordinates or constant inertia matrices. Taking also into account that the IDA-PBC strategy has been successfully applied for Unmanned Aerial Vehicles such as quadrotors [16,17,18,19] and for quadrotors transporting a suspended payload [20,21,22], in this work, as a case of study, the proposed robust control algorithm is applied to a UAV transporting a cable-suspended payload, which is an under-actuated system with inertia matrix dependent of the unactuated coordinates. It is important to mention that previous works [20,21,22] have only applied the IDA-PBC control strategy in order to control a UAV with suspended load and have not solved any robustification problem. The purpose of our current manuscript is to provide a solution to the more general problem of robustification for under-actuated systems with variable inertia matrix and not only to control the UAV with suspended load, here this system is only used as a case of study.

In line with the works of [11, 13] and with a particular change of coordinates, in this paper, we prove using the Lyapunov second method and LaSalle’s invariance principle that an integral controller can be added in an outer-loop to the IDA-PBC strategy to reject constant external disturbances. The proposed control algorithm works for under-actuated Hamiltonian systems with constant or variable inertia matrices, with an arbitrary under-actuation degree and with n degrees of freedom. It should be highlighted that compared with the obtained results in [11, 13], our algorithm is applicable for under-actuated PCH systems with inertia matrices dependent of the unactuated coordinates. In other words, the contribution of our work is to add an integral action that preserves the port-Hamiltonian structure of the closed-loop system, and that also ensures asymptotic stability of the desired equilibrium despite the presence of constant disturbances for systems where the mass matrix is dependent of the unactuated coordinates.

The remainder of the paper is organized as follows: Section 2 presents a brief background of the main concepts of the IDA-PBC algorithm. Section 3 develops the proposed control strategy: Robust IDA-PBC. Section 4 presents numerical results of the robust control algorithm applied to a UAV transporting a cable-suspended payload. Finally, conclusions and future work are discussed in Sect. 5.

2 IDA-PBC background

The objective of the IDA-PBC is to control Hamiltonian systems of the form

$$\begin{aligned} \left[ {\begin{array}{*{20}{c}} {\varvec{\dot{q}}} \\ {\varvec{\dot{p}}} \end{array}} \right] = \left[ \begin{array}{*{20}{c}} \varvec{0_{n\times n}}&{}\varvec{I_{n \times n}} \\ - \varvec{I_{n \times n}}&{}\varvec{0_{n\times n}} \end{array} \right] \left[ \begin{array}{*{20}{c}} \varvec{\nabla _q} \varvec{H} \\ \varvec{\nabla _p} \varvec{H} \end{array} \right] + \left[ \begin{array}{*{20}{c}} \varvec{0_{n \times m}} \\ \varvec{B} \end{array} \right] \varvec{u}\nonumber \\ \end{aligned}$$

(1)

with total energy

$$\begin{aligned} H(\varvec{q},\varvec{p}) = \frac{1}{2}{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}(\varvec{q})\varvec{p} + V(\varvec{q}) \end{aligned}$$

(2)

where $\varvec{q} \in {\mathbb {R}^{n}}$, $\varvec{p} \in {\mathbb {R}^{n}}$, denote the generalized position and momentum respectively, $\varvec{u} \in {\mathbb {R}^{m}}$ is the control vector, $\varvec{I_{n\times n}} \in {\mathbb {R}^{n\times n}}$ expresses an identity matrix, $\varvec{0_{n\times n}} \in {\mathbb {R}^{n\times n}}$ represents a null matrix, $\varvec{\nabla _{q}}\varvec{H} = \partial H/\partial \varvec{q}$, $\varvec{\nabla _{p}}\varvec{H} = \partial H/\partial \varvec{p}$ are the gradients of the total energy with respect to the generalized position and momentum, respectively, $\varvec{B}$ is the input matrix, $H(\varvec{q},\varvec{p})\in \mathbb {R}$ represents the Hamiltonian, $\varvec{\mathcal {M}(q)}=\varvec{\mathcal {M}^T(q)}$ is a symmetric inertia matrix and $V(\varvec{q}) \in \mathbb {R}$ is the potential energy.

Applying the standard IDA-PBC strategy [1, 2], we obtain the desired port-controlled Hamiltonian dynamics as follows:

$$\begin{aligned} \left[ {\begin{array}{*{20}{c}} \varvec{ {\dot{q}}} \\ \varvec{ {\dot{p}}} \end{array}} \right] = \left[ {\varvec{J_d}(\varvec{q},\varvec{p}) - \varvec{R_d}(\varvec{q},\varvec{p})} \right] \left[ {\begin{array}{*{20}{c}} {\varvec{\nabla _q}{\varvec{H_d}}} \\ {\varvec{\nabla _p}{\varvec{H_d}}} \end{array}} \right] \end{aligned}$$

(3)

with the desired interconnection and dissipation matrices

$$\begin{aligned} {\varvec{J_d}}= & {} - \varvec{J_d}^T = \left[ {\begin{array}{*{20}{c}} \varvec{0}&{}{\varvec{\mathcal {{M}}}^{ - 1}\varvec{\mathcal {{M}}_d}} \\ { - \varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}&{}{\varvec{J_2}(\varvec{q},\varvec{p})} \end{array}} \right] , \\ \varvec{R_d}= & {} \varvec{R_d}^T = \left[ {\begin{array}{*{20}{c}} \varvec{0} &{} \varvec{0} \\ \varvec{0} &{}\varvec{R_2} \end{array}} \right] \ge \varvec{0} \end{aligned}$$

respectively, where the submatrix $\varvec{J_2}$ is skew-symmetric, also

$$\begin{aligned} \varvec{R_2={B{\varvec{K}_v}{B^T}} \quad \text {and} \quad {\varvec{K}_v} = \varvec{K}_v^T > 0}. \end{aligned}$$

The desired energy function takes the form

$$\begin{aligned} {H_d}(\varvec{q},\varvec{p}) = \frac{1}{2}{\varvec{p}^T}{\varvec{\mathcal {M}_d}}^{ - 1}(\varvec{q})\varvec{p} + {V_d}(\varvec{q}) \end{aligned}$$

(4)

where $\varvec{\mathcal {M}_d} = \varvec{\mathcal {M}_d}^T > \varvec{0}$ represents the desired closed-loop inertia matrix. $V_d$ stands for the desired potential energy function, and has by design an isolated minimum at $\varvec{q_*}$, i.e.,

$$\begin{aligned} \varvec{q_*} = \arg \min {V_d}(\varvec{q}) \end{aligned}$$

(5)

System (3) is equivalent to system (1) with

$$\begin{aligned} \varvec{u} = \varvec{u_{es}}(\varvec{q},\varvec{p}) + {\varvec{u_{di}}}(\varvec{q},\varvec{p}) \end{aligned}$$

(6)

where the first term is designed to achieve the energy-shaping and the second one injects the damping.

The energy-shaping term is given by

$$\begin{aligned}&\varvec{u_{es}(\varvec{q},\varvec{p})} \nonumber \\&\quad = ~ \left( {{\varvec{B}}^T} \! \varvec{B} \right) ^{-1} \!\! {\varvec{B}^T} \! \left( \varvec{\nabla _q}\varvec{H} \! - \! {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla _q}}{\varvec{H_d}} \! \right. \nonumber \\&\qquad \left. + \varvec{J_2}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p}\right) \end{aligned}$$

(7)

and the damping injection by

$$\begin{aligned} {\varvec{u_{di}}}(\varvec{q},\varvec{p}) = - {\varvec{K_v}}{\varvec{B}^T}{\varvec{\nabla _p}}{\varvec{H_d}} \end{aligned}$$

(8)

where $\varvec{\mathcal {M}_d}$, $\varvec{V_d}$ and $\varvec{J_2}$ satisfy the following matching equations

$$\begin{aligned}&{\varvec{B}^ \bot } [ {\varvec{\nabla _q}}({\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}) \nonumber \\&\qquad -\varvec{{\mathcal {M}_d}}\varvec{\mathcal {M}} ^{ - 1}{\varvec{\nabla _q}}({\varvec{p}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p}) \nonumber \\&\qquad + 2{\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p} ] = \varvec{0} \end{aligned}$$

(9)

$$\begin{aligned}&{\varvec{B}^ \bot }\left[ {{\varvec{\nabla _q}}\varvec{V} - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla _q}}{\varvec{V_d}}} \right] = \varvec{0} \end{aligned}$$

(10)

where $\varvec{B^ \bot }$ is a full rank left annihilator of $\varvec{B}$, i.e., $\varvec{B^ \bot B=0}$.

3 Robust IDA-PBC strategy

System (1) perturbed by a constant external disturbance $\varvec{d}$ is represented by

$$\begin{aligned} {\begin{array}{*{20}{c}} {\left[ {\begin{array}{*{20}{c}} \varvec{ {\dot{q}}} \\ \varvec{ {\dot{p}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} \varvec{0}&{}{ \varvec{ {I_{n \times n}}}} \\ { - \varvec{ {I_{n \times n}}}}&{} \varvec{0} \end{array}} \right] \left[ {\begin{array}{*{20}{c}} {\varvec{\nabla _q} } \varvec{H} \\ {\varvec{\nabla _p} } \varvec{H} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} \varvec{0} \\ {\varvec{{B}}} \end{array}} \right] (\varvec{u_T} + \varvec{d})} \\ \end{array}} \end{aligned}$$

(11)

where $\varvec{u_T}=\varvec{u}+\varvec{v}$ is the total control law, $\varvec{v}$ represents an outer-loop control formed by an integral action, and $\varvec{u}$ an IDA-PBC. Applying the controller (6) with an added outer-loop control to (11), we obtain the following closed-loop disturbed system:

$$\begin{aligned} \left[ {\begin{array}{*{20}{c}} \varvec{ {\dot{q}}} \\ \varvec{ {\dot{p}}} \end{array}} \right]= & {} \left[ {\begin{array}{*{20}{c}} \varvec{0}&{}{{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}} \\ { - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}}&{}{ \varvec{J_2}- {\varvec{R_2}}} \end{array}} \right] \varvec{\nabla } {\varvec{H_d}} \nonumber \\&+ \left[ {\begin{array}{*{20}{c}} \varvec{0} \\ { \varvec{ B}} \end{array}} \right] (\varvec{v}+\varvec{d}) \end{aligned}$$

(12)

with the Hamiltonian function

$$\begin{aligned}{H}_d = {\varvec{p}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p} + {V_d}(\varvec{q})\end{aligned}$$

In this work, we propose a new procedure to solve the problem of constant disturbances rejection for under-actuated systems with either inertia matrix dependent of the unactuated coordinates or constant inertia matrix. This approach is applicable to under-actuated systems where the input matrix $\varvec{B}$ and the closed-loop inertia matrix $\varvec{\mathcal {M}_d}$ are constant. In the following, consider the perturbed Hamiltonian system (12) with constant inertia matrix or inertia matrix dependent of the unactuated coordinates.

Remember that the main difficulty in the matching equations is the solution of the kinetic energy matching equation. Under the above conditions, we can rewrite the Partial Differential Equation (PDE) (9) as [2]

$$\begin{aligned} \varvec{B}^\bot \left[ \varvec{\nabla _q}\left( \varvec{p}^T\varvec{\mathcal {M}}^{ - 1}\varvec{p} \right) + 2\varvec{J_2}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p} \right] = \varvec{0} \end{aligned}$$

(13)

Remark 1

When $\varvec{\mathcal {M}}$ is constant ${\varvec{\nabla _q}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) =\varvec{0_{n}}$ and $\varvec{J_2}$ is a null matrix.

Without loss of generality, we assume that

$$\begin{aligned} \varvec{q} = {\left[ {\begin{array}{*{20}{c}} {{\varvec{q_a}}}&{{\varvec{q_u}}} \end{array}} \right] ^T}, \end{aligned}$$

where $\varvec{q_a}$ $\in$ $\mathbb {R}^{n_a}$ denotes the actuated coordinates and $\varvec{q_u}$ $\in$ $\mathbb {R}^{n_u}$ the unactuated coordinates, otherwise we can reorder the coordinates to come up with this structure. Note that in this case:

$$\begin{aligned} \varvec{B} = {\left[ {\begin{array}{*{20}{c}} {{\varvec{I_{{n_a} \times {n_a}}}}}&{{\varvec{0_{{n_u} \times {n_a}}}}} \end{array}} \right] ^T} \end{aligned}$$

and ${\varvec{B}^\bot } = \left[ {\begin{array}{*{20}{c}} {{\varvec{0_{{n_u} \times {n_a}}}}}&{\varvec{I_{{n_u} \times {n_u}}}} \end{array}} \right]$,

which complies with ${\varvec{B}^\bot }\varvec{B} = \varvec{0_{{n_u} \times {n_a}}}$. Therefore, we can represent (13) as follows

$$\begin{aligned}&\left[ {\begin{array}{*{20}{c}} {{\varvec{0_{{n_u} \times {n_a}}}}}&{\varvec{I_{{n_u}\times {n_u}}}} \end{array}} \right] \left[ {\begin{array}{*{20}{c}} {\varvec{\nabla _{{q_a}}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) } \\ {\varvec{\nabla _{{q_u}}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) } \end{array} + 2{\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p}}\right] \nonumber \\&\quad = \varvec{0_{n_u}} \end{aligned}$$

(14)

where $n_a$ denotes the order of actuated coordinates and $n_u$ of unactuated coordinates.

Now, we define the skew-symmetric matrix $\varvec{J_2}$ as a partitioned matrix, i.e.,

$$\begin{aligned} \varvec{{J_2}} = \left[ {\begin{array}{*{20}{c}} {{\varvec{0_{{n_a} \times {n_u}}}}}&{}{\varvec{A}(\varvec{j_{2}})} \\ { - {\varvec{j_{2}}}}&{}{{\varvec{0_{{n_u} \times {n_a}}}}} \end{array}} \right] \end{aligned}$$

(15)

where $\varvec{A}(\varvec{j_{2}})$ and $\varvec{j_{2}}$ represent square matrices of order ${n_a}$ and ${n_u}$, respectively,

$$\begin{aligned} \varvec{A}(\varvec{j_{2}}) = \left[ {\begin{array}{*{20}{c}} {{\varvec{0_{{n_u} \times ({n_a} - {n_u})}}}}&{}{{\varvec{j_{2}}}} \\ {{\varvec{0_{({n_a} - {n_u}) \times {n_u}}}}}&{}{{\varvec{0_{({n_a} - {n_u}) \times ({n_a} - {n_u})}}}} \end{array}} \right] \end{aligned}$$

The matrix ${\varvec{\mathcal {M}_d}^{ - 1}}$ can be partitioned into 4 blocks as follows

$$\begin{aligned} \varvec{\mathcal {M}_d}^{ - 1} = \left[ {\begin{array}{*{20}{c}} {\varvec{\mathcal {M}_d}^{ - 1}_1} \\ {\varvec{\mathcal {M}_d}^{ - 1}_2} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\varvec{\mathcal {M}_d}^{ - 1}_{11}}&{}{\varvec{\mathcal {M}_d}^{ - 1}_{12}} \\ {\varvec{\mathcal {M}_d}^{ - 1}_{21}}&{}{\varvec{\mathcal {M}_d}^{ - 1}_{22}} \end{array}} \right] \end{aligned}$$

with ${\varvec{\mathcal {M}_d}^{ - 1}_{1}}$ $\in$ $\mathbb {R}^{{{n_u} \times {n}}}$, ${\varvec{\mathcal {M}_d}^{ - 1}_{2}}$ $\in$ $\mathbb {R}^{{{n_a} \times {n}}}$, ${\varvec{\mathcal {M}_d}^{ - 1}_{11}}$ $\in$ $\mathbb {R}^{{{n_u} \times {n_a}}}$, ${\varvec{\mathcal {M}_d}^{ - 1}_{12}}$ $\in$ $\mathbb {R}^{{{n_u} \times {n_u}}}$, ${\varvec{\mathcal {M}_d}^{ - 1}_{21}}$ $\in$ $\mathbb {R}^{{{n_a} \times {n_a}}}$ and ${\varvec{\mathcal {M}_d}^{ - 1}_{22}}$ $\in$ $\mathbb {R}^{{{n_a} \times {n_u}}}$.

Substituting $\varvec{J_2}$ and $\varvec{\mathcal {M}_d}{^{ - 1}}$ into (14), it yields

$$\begin{aligned}&\left[ {\begin{array}{*{20}{c}} {{\varvec{0_{n_u \times n_a}}}}&{{\varvec{I_{{n_u \times n_u}}}}} \end{array}} \right] \left[ \begin{array}{*{20}{c}} \varvec{0_{n_a}} \\ {{\varvec{\nabla } _{{\varvec{q_u}}}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) } \end{array} \right. \\&\qquad + 2 \left. \left[ {\begin{array}{*{20}{c}} {\varvec{A}(\varvec{j_{2}})\varvec{\mathcal {M}_d}^{ - 1}_{21}}&{}{\varvec{A}(\varvec{j_{2}})\varvec{\mathcal {M}_d}^{ - 1}_{22}} \\ { - {\varvec{j_{2}}}\varvec{\mathcal {M}_d}^{ - 1}_{11}}&{}{ - {\varvec{j_{2}}}\varvec{\mathcal {M}_d}^{ - 1}_{12}} \end{array}} \right] \varvec{p} \right] = \varvec{0_{n_u}} \end{aligned}$$

Then, we obtain

$$\begin{aligned} {{\varvec{\nabla _{{q_u}}}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) - 2{\varvec{j_{2}}}{\varvec{\mathcal {M}_d}^{ - 1}_1}\varvec{p}} = \varvec{0_{n_u}}\end{aligned}$$

For the case when $n_u=1$,

$$\begin{aligned} \varvec{j_{2}} = \frac{1}{2}{\varvec{\nabla _{{q_u}}}}\left( {{\varvec{p}^T}{\varvec{\mathcal {M}}^{ - 1}}\varvec{p}} \right) {\left[ {\varvec{\mathcal {M}_d}^{ - 1}_1}\varvec{p} \right] ^{ - 1}} \end{aligned}$$

(16)

For the case when $n_u>1$,

$$\begin{aligned} \varvec{j_{2}}=&\frac{1}{2} {\varvec{\nabla _{q_u}}} \!\! \left( {\varvec{p}^T}{\varvec{\mathcal {M}}}^{ - 1}\varvec{p} \right) \!\! \left[ {\left[ {\varvec{\mathcal {M}_d}^{ - 1}_1}\varvec{p} \right] ^T} \!\! \left[ {\varvec{\mathcal {M}_d}^{ - 1}_1}\varvec{p} \right] \right] ^{- 1}\nonumber \\&\left[ {\varvec{\mathcal {M}_d}^{ - 1}_1}\varvec{p} \right] ^T \end{aligned}$$

(17)

3.1 Augmented PCH system

Now, consider the following state transformation:

$$\begin{aligned} {\varvec{z_1}}= & {} \varvec{q} \nonumber \\ {\varvec{z_2}}= & {} \varvec{p} + \varvec{B}{\varvec{K_i}}\left( {\varvec{w} - \varvec{\lambda } } \right) \nonumber \\ {\varvec{z_3}}= & {} \varvec{w} \end{aligned}$$

(18)

where $\varvec{w}$ represents the augmented state, $\varvec{\lambda }$ depends on the disturbance $\varvec{d}$ and $\varvec{{K}_i} = \varvec{{K}_i}^T > \varvec{0}$ is a gain matrix.

With the coordinates transformation, the new extended PCH system can be represented by

$$\begin{aligned}&\left[ {\begin{array}{*{20}{c}} {{{\varvec{\dot{z}_1}}}} \\ {{{\varvec{\dot{z}_2}}}} \\ {{{\varvec{\dot{z}_3}}}} \end{array}} \right] \nonumber \\&\quad =\left[ \! {\begin{array}{*{20}{c}} {{\varvec{0_{n \times n}}}}&{}{{\varvec{\mathcal {M}}^{ - 1}}\!{\varvec{\mathcal {M}_d}}}&{}{ - {\varvec{\mathcal {M}}^{ - 1}}\!\varvec{B}{\varvec{K_i}}} \\ { - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{-1}}}&{}{{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}}&{}{ - {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\!\varvec{B}{\varvec{K_i}}} \\ {{\varvec{K_i}}{\varvec{B}^T}\!{\varvec{\mathcal {M}}^{ - 1}}}&{}{{\varvec{K_i}}{\varvec{B}^T}\!\varvec{\mathcal {M}_d}^{ - 1}\!\varvec{J_2}^T}&{}{{\varvec{0_{m \times n}}}} \end{array}} \! \right] \!\! \varvec{\nabla } \! {\varvec{H_z}} \end{aligned}$$

(19)

with

$$\begin{aligned} {H_z} =&\frac{1}{2}{\varvec{z_2}}^T\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}} + {V_d}\left( {{\varvec{z_1}}} \right) \nonumber \\&+ \frac{1}{2}{{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) ^T}{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) }} \end{aligned}$$

(20)

From (19) and (20), it can be observed that the closed-loop extended system with the change of coordinates conserves the PCH structure.

In order to obtain the integral controller $\varvec{v}$, systems (12) and (19) are compared. The rows of the desired PCH system (19) are:

$$\begin{aligned} {\varvec{\dot{z}_1}}&= {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}{\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} - {\varvec{\mathcal {M}}^{ - 1}}\varvec{B}{\varvec{K_i}}{\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}} \nonumber \\&= {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}} \nonumber \\&\quad \,\, - {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \nonumber \\&= {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}\varvec{\mathcal {M}_d}^{ - 1}\left( {{\varvec{z_2}} - \varvec{B}{\varvec{K_i}}\left( {\varvec{z_3} - \varvec{\lambda } } \right) } \right) \nonumber \\&= {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{p} \nonumber \\&= {\varvec{\mathcal {M}}^{ - 1}}{\varvec{\mathcal {M}_d}}{\varvec{\nabla _p}}{\varvec{H_d}} \nonumber \\&= \dot{\varvec{q}} \end{aligned}$$

(21)

$$\begin{aligned} {\varvec{\dot{z} _2}}&= \dot{\varvec{p}} + \varvec{B}{\varvec{K_i}}\dot{\varvec{w}} \nonumber \\&= - {\varvec{\mathcal {M} _d}}{\varvec{\mathcal {M}}^{-1}}{\varvec{\nabla _q}}{\varvec{H_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla _p}}{\varvec{H_d}} \nonumber \\&+ \varvec{B}(\varvec{v}+\varvec{d}) + \varvec{B}{\varvec{K_i}}\dot{\varvec{w}} \nonumber \\&= - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}\varvec{p}} \right) \nonumber \\&+ \varvec{B}(\varvec{v}+\varvec{d}) + \varvec{B}{\varvec{K_i}}{\varvec{\dot{z}_3}} \nonumber \\&= - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} \nonumber \\&+ \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}} \right) \left( {{\varvec{z_2}} - \varvec{B}{\varvec{K_i}}\left( {\varvec{w} - \varvec{\lambda } } \right) } \right) \nonumber \\&+\varvec{B}(\varvec{v} +\varvec{d}) + \varvec{B}{\varvec{K_i}}{\varvec{\dot{z}_3}} \nonumber \\&= - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \varvec{\mathcal {M}_d}^{- 1}{\varvec{z_2}} \nonumber \\&- \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {\varvec{w} - \varvec{\lambda } } \right) \nonumber \\&+ \varvec{B}(\varvec{v}+\varvec{d}) + \varvec{B}{\varvec{K_i}}{\varvec{\dot{z}_3}} \nonumber \\&=- {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{-1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \nonumber \\&- {\varvec{J_2}}\varvec{\mathcal {M}_d}^{-1}\varvec{B}{\varvec{K_i}}\left( {\varvec{w} - \varvec{\lambda } } \right) \nonumber \\&+ \varvec{B}{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {\varvec{w} - \varvec{\lambda } } \right) \nonumber \\&+\varvec{B}(\varvec{v}+\varvec{d}) + \varvec{B}{\varvec{K_i}}{{\varvec{\dot{z}_3}}} \nonumber \\ =&- {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \nonumber \\&- {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \nonumber \\&+ \varvec{B}{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \nonumber \\&+ \varvec{B}(\varvec{v}+\varvec{d}) + \varvec{B}{\varvec{K_i}}{{\varvec{\dot{z}_3}}} \nonumber \\ =&- {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \nonumber \\&- {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}} + \varvec{B}{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{z_3}} \nonumber \\&- \varvec{B}{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\varvec{\lambda } + \varvec{B}\varvec{v} + \varvec{B}\varvec{d} + \varvec{B}{\varvec{K_i}}{\varvec{\dot{z}_3}} \nonumber \\ \end{aligned}$$

(22)

In Eqs. (21) and (22), it can be seen that the first row of such systems is clearly satisfied. Nevertheless, the second row is only satisfied if:

$$\begin{aligned}&{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{z_3}} - {\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\varvec{\lambda } \nonumber \\&\qquad + \varvec{v}+\varvec{d} + {\varvec{K_i}}{{\varvec{\dot{z}_3}}} = \varvec{0} \end{aligned}$$

(23)

Now, $\varvec{\lambda }$ is chosen as follows

$$\begin{aligned} \varvec{\lambda } = {\left( {{\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}} \right) ^{ - 1}}\varvec{d} \end{aligned}$$

(24)

Then, substituting $\varvec{\lambda }$ into (23), Eq. (23) can be expressed as

$$\begin{aligned} {\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{z_3}} + \varvec{v} + {\varvec{K_i}}{{\varvec{\dot{z}_3}}} = \varvec{0} \end{aligned}$$

(25)

From (25), the resulting control law $\varvec{v}$ is given as

$$\begin{aligned} \varvec{v} = - {\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{z_3}} - {\varvec{K_i}}{{\varvec{\dot{z}_3}}} \end{aligned}$$

(26)

where

$$\begin{aligned}{{\varvec{\dot{z}_3}}} = {\varvec{K_i}}{\varvec{B}^T}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{H_z}} + {\varvec{K_i}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{J_2}^T{\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}}\end{aligned}$$

Fig. 1

Robust IDA-PBC control scheme

From (26) and $\varvec{\dot{z}_3}$, it can be observed that the outer-loop control law $\varvec{v}$ has two actions, an integral action and a proportional action. The objective of this controller is to reject constant external disturbances. The overall scheme of the control strategy is depicted in Fig. 1.

3.2 Stability analysis

Theorem 1

Considering the closed-loop disturbed system with inertia matrix dependent of the unactuated coordinates (12), along with the state transformation (18), there exists a control law $\varvec{v} = - {\varvec{K_p}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1} \varvec{B}{\varvec{K_i}}{\varvec{z_3}} - {\varvec{K_i}}{{\varvec{\dot{z}_3}}}$, such that the augmented PCH system (19) with the Hamiltonian function (20) guarantees asymptotic stability at the equilibrium point $\varvec{z_e} = ({\varvec{z_{1*}}},\varvec{0},\varvec{z_{3*}})$ despite the presence of constant external disturbances.

Proof

The Hamiltonian function (20) is proposed as a Lyapunov candidate function for (19). Then, differentiating (20) along the trajectories of the system

$$\begin{aligned} {{\dot{H}}_z} = \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}{{\varvec{\dot{z}_2}}} + \left[ {\varvec{\nabla _{z1}}}\varvec{V_d}\right] ^T{{\varvec{\dot{z}_1}}} + {{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) ^T}}{{\varvec{\dot{z}_3}}}\nonumber \\ \end{aligned}$$

(27)

Introducing (19) into (27):

$$\begin{aligned} {{\dot{H}}_z} =&\varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1} \left( - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} \right. \\&\left. + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \right. \nonumber \\&\left. - {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}} \right) \\&\! + \! [ {\varvec{\nabla _{z1}}}\varvec{V_d}]^T \! \left( {{\varvec{\mathcal {M}}^{-1}}\!{\varvec{\mathcal {M}_d}}{\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \! - \! {\varvec{\mathcal {M}}^{-1}}\!\varvec{B}{\varvec{K_i}}{\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}}} \right) \\&+ {{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) }^T} \left( {\varvec{K_i}}{\varvec{B}^T}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{H_z}} \right. \\&\left. + {\varvec{K_i}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{J_2}^T{\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \right) \\ =&\varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1} \left( - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} \right. \nonumber \\&+ \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \\&\left. - {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \right) \\&+ [{\varvec{\nabla _{z1}}}\varvec{V_d}]^T\left( {{\varvec{\mathcal {M}}^{ - 1}}{\varvec{z_2}} - {\varvec{\mathcal {M}}^{ - 1}}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) } \right) \\&+ {{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) }^T} \left( {\varvec{K_i}}{\varvec{B}^T}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{H_z}} \right. \\&\left. + {\varvec{K_i}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{J_2}^T{\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \right) \\ \end{aligned}$$

$$\begin{aligned} =&- \varvec{z_2}^T{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} \nonumber \\&+ \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}\left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \nonumber \\&- \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}{\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \nonumber \\&+ [{\varvec{\nabla _{z1}}}\varvec{V_d}]^T{\varvec{\mathcal {M}}^{ - 1}}{\varvec{z_2}} - {\varvec{\nabla _{z1}}}\varvec{V_d}^T{\varvec{\mathcal {M}}^{ - 1}}\varvec{B}{\varvec{K_i}}\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) \nonumber \\&+ {{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) }^T}{\varvec{K_i}}{\varvec{B}^T}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{H_z}} \nonumber \\&+ {{\left( {{\varvec{z_3}} - \varvec{\lambda } } \right) }^T}\left( {{\varvec{K_i}}{\varvec{B}^T}\varvec{\mathcal {M}_d}^{ - 1}\varvec{J_2}^T\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \nonumber \\ =&\varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}{\varvec{J_2}}\left( {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \nonumber \\&- \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}\left( {\varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \nonumber \\ =&- \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}\left( {\varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) \left( {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \right) \nonumber \\ =&- \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}\varvec{R}_2 {\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}}} \end{aligned}$$

(28)

since $\varvec{J_2}$ is skew-symmetric and $\varvec{K_p}$ is positive-definite. From expression (28), it can be observed that ${{\dot{H}}_{z}} \leqslant 0$, i.e., the stability is preserved for this type of system (under-actuated system with either inertia matrix dependent of the unactuated coordinates or constant inertia matrix). Now, using the LaSalle’s invariance principle, it is possible to prove that all the trajectories of the system converge at the equilibrium point, which implies asymptotic stability.

Let $\varvec{\varOmega } \subset \mathbb {R}^{n}$ be a compact positively invariant set with respect to the system (19). Let $\mathrm \mathbb {X}$ be the set of all points in $\varvec{\varOmega }$ where $\dot{H}_{z} = 0$. If $\dot{H}_{z} = 0$ $\rightarrow$ $\mathrm \mathbb {X}= \{ (\varvec{z_2})$ | $\dot{H}_{z}$ $(\varvec{z_2}) = 0 \}$ $\rightarrow - \varvec{z_2}^T\varvec{\mathcal {M}_d}^{ - 1}{\varvec{R_2}}\varvec{\mathcal {M}_d}^{ - 1}{\varvec{z_2}} = \varvec{0} \rightarrow \varvec{z_2} = \varvec{0}$. In the equilibrium point $\varvec{z_e}=(\varvec{z_{1*}},\varvec{0}, \varvec{z_{3*}})$, $\varvec{z_2}=\varvec{0}$ $\rightarrow$ ${\varvec{\dot{z}_2}}=\varvec{0}$.

From (19),

$$\begin{aligned} {{\varvec{\dot{z}_2}}}= & {} - {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}{\varvec{V_d}} + \left( {{\varvec{J_2}} - \varvec{B}{\varvec{K_p}}{\varvec{B}^T}} \right) {\varvec{\nabla } _{{\varvec{z_2}}}}{\varvec{H_z}} \nonumber \\&- {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}{\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}}, \end{aligned}$$

therefore if $\varvec{z_2}=\varvec{0}$ $\wedge$ ${\varvec{\dot{z}_2}}=\varvec{0}$ $\rightarrow$ $- {\varvec{\mathcal {M}_d}}{\varvec{\mathcal {M}}^{ - 1}}{\varvec{\nabla } _{{\varvec{z_1}}}}\varvec{V_d}$ $- {\varvec{J_2}}\varvec{\mathcal {M}_d}^{ - 1}\varvec{B}{\varvec{K_i}}$ ${\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}}=\varvec{0}$ $\rightarrow$ $\varvec{\nabla } _{{\varvec{z_1}}}\varvec{V_d}=\varvec{0}$ $\wedge$ ${\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}}=\varvec{0}$ $\rightarrow$ $\varvec{\nabla } _{{\varvec{z_1}}}\varvec{H_z}=\varvec{0}$ $\wedge$ ${\varvec{\nabla } _{{\varvec{z_3}}}}{\varvec{H_z}}=\varvec{0}$.

The IDA-PBC strategy requires that $V_d$ has an isolated minimum at $\varvec{q_*}$, i.e., ${\varvec{q_ *} } = \arg \min {V_d}(\varvec{q})$. Thus,

$$\begin{aligned}{\left. {{\varvec{\nabla } _{{\varvec{z_1}}}}\varvec{V_d} \rightarrow {\varvec{\nabla } _{{\varvec{z_1}}}}\varvec{V_d}} \right| _{{\varvec{z_{1*}}}}} = \varvec{0}\end{aligned}$$

Hence, the conditions of the invariance principle are satisfied and the proof is complete. Therefore, the asymptotic stability is guaranteed, and the system converges to the equilibrium point $\varvec{z_e}=(\varvec{z_{1*}}, \varvec{0}, \varvec{z_{3*}})$. $\square$

In summary, the novelty of the proposed Robust IDA-PBC approach in this paper is that it provides a solution to the up to now still open problem in the literature for under-actuated systems with inertia matrices dependent of the unactuated coordinates. In this new algorithm, it is proven that the disturbance rejection is achieved for some class of under-actuated systems, by satisfying the two properties that must be required for all extended PCH systems in closed-loop: preserving the structure of a PCH system and the asymptotic stability.

4 Case Study: robust IDA-PBC for a PVTOL carrying a suspended load

In this section, the new Robust IDA-PBC approach presented in Sect. 3 for under-actuated systems with inertia matrix dependent on the unactuated variables is applied to a Planar Vertical Take-Off and Landing (PVTOL) transporting a load suspended by a cable, as depicted in Fig. 2, which is a kind of under-actuated system with inertia matrix dependent of the unactuated coordinates. The Robust IDA-PBC methodology is used to stabilize the vehicle and the load swing. These goals should be achieved despite constant external disturbances acting on the system.

Fig. 2

PVTOL transporting a suspended payload

4.1 Mathematical model

The dynamic behavior of a UAV transporting a cable-suspended payload is described in [20,21,22]. The presented mathematical model in this work is based on [19]; nevertheless, due to the presented complexity, and in order to achieve the main goal of illustrating the validity of the proposed Robust IDA-PBC, some assumptions are made:

• The cable connecting payload and UAV fuselage is rigid, massless, and inelastic.

• The length of the connecting cable is constant and known.

• The swing angle $\alpha$ of the payload is not actuated and constrained to $-\pi /2<\alpha <\pi /2$.

• The payload can be considered as a point-mass.

• The aerodynamic effects on the load are neglected.

• The vehicle orientation dynamics will be neglected in this paper to have a constant input matrix $\varvec{B}$.

Considering the above, we have obtained a reduced system with 3 degrees of freedom and two control inputs, i.e., $n=3$ and $m=2$. The mathematical model can be expressed in the form

$$\begin{aligned} \begin{gathered} \varvec{\mathcal {M}}(\varvec{q})\ddot{\varvec{q}} + \varvec{C}(\varvec{q},\dot{\varvec{q}})\dot{\varvec{q}} + \varvec{G}(\varvec{q}) = \varvec{B}\varvec{u} \end{gathered} \end{aligned}$$

(29)

with the following matrices:

$$\begin{aligned} \varvec{\mathcal {M}}= & {} \left[ {\begin{array}{*{20}{c}} {M + m}&{}0&{}{ml\cos \alpha } \\ 0&{}{M + m}&{}{ml\sin \alpha } \\ {ml\cos \alpha }&{}{ml\sin \alpha }&{}{m{l^2}} \end{array}} \right] , \quad \\ \varvec{C}= & {} \left[ {\begin{array}{*{20}{c}} 0&{}0&{}{ - ml\sin \alpha {{\dot{\alpha }}} } \\ 0&{}0&{}{ml\cos \alpha {{\dot{\alpha }}} } \\ 0&{}0&{}0 \end{array}} \right] , \quad \varvec{G} = \left[ {\begin{array}{*{20}{c}} 0 \\ {(M + m)g} \\ {mlg \sin \alpha } \end{array}} \right] , \quad \\ \varvec{B}= & {} \left[ {\begin{array}{*{20}{c}} 1&{}0 \\ 0&{}{\cos \theta } \\ 0&{}0 \end{array}} \right] \\ \end{aligned}$$

where M is the mass of the UAV, m is the mass of the load, the length of the cable is represented by l, the gravitational acceleration is given by g, $\varvec{q} = {\left[ {\begin{array}{*{20}{c}} x&z&\alpha \end{array}} \right] ^T}$ denotes the state variables, x, z describe the position of the UAV and $\alpha$ the swing angle of the payload. The control input is defined as $\varvec{u} = {\left[ {\begin{array}{*{20}{c}} {{\tau _\theta }}&f \end{array}} \right] ^T}$, where $\tau _\theta$ represents the torque produced by the differential velocity between opposite rotors, and f the total thrust magnitude. $\varvec{B} \in {\mathbb {R}^{3 \times 2}}$ is the input matrix where it can be seen that the system is under-actuated.

Remark 2

Note that the system is third order and the inertia matrix $\varvec{\mathcal {M}}$ depends on the unactuated coordinate, i.e., the payload’s angle of oscillation $\alpha$. The problem of robustification of IDA-PBC to reject constant external disturbances in systems with these characteristics had not been addressed in previous works.

4.2 Robust IDA-PBC strategy

The Robust IDA-PBC strategy is applied to the dynamic model (29) of the PVTOL transporting a cable- suspended payload.

The methodology for designing the control law $\varvec{u_T}$ of the Robust IDA-PBC strategy consists of two main stages. The first step of the Robust IDA-PBC algorithm requires to develop a traditional IDA-PBC scheme for the mathematical model (29).

Thus, $\varvec{\mathcal {M}_d}$ is selected as a constant matrix with the following structure:

$$\begin{aligned}\varvec{\mathcal {M}_d} = \left[ {\begin{array}{*{20}{c}} {{a_1}}&{}0&{}{{0}} \\ 0&{}{{a_2}}&{}0 \\ {{0}}&{}0&{}{{a_3}} \end{array}} \right] , {a_1}{a_2}a_3 > 0\end{aligned}$$

From (15) and (16)

$$\begin{aligned} {\varvec{J_2}}= & {} \left[ {\begin{array}{*{20}{c}} 0&{}0&{}{{j_{2}}} \\ 0&{}0&{}0 \\ { - {j_{2}}}&{}0&{}0 \end{array}} \right] ,\\ {j_{2}}= & {} \frac{{{a_1}m\sin \left( {2\alpha } \right) \left( {p_3^2 - p_1^2} \right) }}{{2M(M + m){p_1}}} + \frac{{{a_1}\sin \alpha {p_5}}}{{Ml}} \nonumber \\&+ \frac{{{a_1}m\cos \left( {2\alpha } \right) {p_3}}}{{M(M + m)}} - \frac{{{a_1}\cos \alpha {p_3}{p_5}}}{{Ml{p_1}}} \end{aligned}$$

where $\varvec{p} = [\begin{array}{*{20}{c}} {{p_1}}&{{p_3}}&{{p_5}} \end{array}]$.

With ${\varvec{B}^ \bot } = \left[ {\begin{array}{*{20}{c}} 0&0&1 \end{array}} \right]$, the potential energy PDE (10) takes the form:

$$\begin{aligned}&glm\sin \alpha - \frac{{\left( {M + m} \right) {a_3}}}{{M{l^2}m}}{\nabla _\alpha }{{V_d}} \nonumber \\&\qquad + \frac{{{a_3}}}{{Ml}}\cos \alpha {\nabla _x}{{V_d}} + \frac{{{a_3}}}{{M{l}}}\sin \alpha {\nabla _z}{{V_d}} = 0 \end{aligned}$$

(30)

which is solved and thus the desired potential energy is obtained by

$$\begin{aligned}{V_d} = - \frac{{M{l^3}{m^2}g}}{{\left( {M + m} \right) {a_3}}}\cos \alpha + \varPhi \left( {x,z,\alpha } \right) \end{aligned}$$

We choose $\varPhi ({x,z,\alpha })$, to be a quadratic function which leads to

$$\begin{aligned} {V_d}= & {} - \frac{{M{l^3}{m^2}g}}{{\left( {M + m} \right) {a_3}}}\cos \alpha \nonumber \\&+ \frac{{{k_{px}}}}{2}{\left( {x - {x_d} + \frac{{lm}}{{M + m}}\sin \alpha } \right) ^2} \nonumber \\&+ \frac{{{k_{pz}}}}{2}{\left( {z - {{z_d}} - \frac{lm}{{M + m}}\cos \alpha } \right) ^2} \end{aligned}$$

(31)

where $k_{px}>0$ and $k_{pz}>0$ are constant gains.

Then, from (4) and (31) the Hamiltonian function is obtained

$$\begin{aligned} {H_d}= & {} \frac{{p_1^2}}{{2{a_1}}} + \frac{{p_3^2}}{{2{a_2}}} + \frac{{p_5^2}}{{2{a_3}}} - \frac{{M{l^3}{m^2}g}}{{\left( {M + m} \right) {a_3}}}\cos \alpha \nonumber \\&+ \frac{{{k_{px}}}}{2}{\left( {x - {x_d} + \frac{{lm}}{{M + m}}\sin \alpha } \right) ^2} \nonumber \\&+ \frac{{{k_{pz}}}}{2}{\left( {z - {z_d} - \frac{lm}{{M + m}}\cos \alpha } \right) ^2} \end{aligned}$$

(32)

To compute the control law $\varvec{u}=\varvec{u_{es}}+\varvec{u_{di}}$ of the IDA-PBC strategy, we first determine the energy-shaping term $\varvec{u_{es}}$ from (7) and then, we determine the damping injection term $\varvec{u_{di}}$ from (8).

After completing the first stage, we continue to the second stage. In this stage, the presented methodology in Sect. 3 is used to find the term $\varvec{v}$ of the total control law $\varvec{u_T}$ of the Robust IDA-PBC strategy, which corresponds to the proportional-integral action.

Now, we obtain a Hamiltonian representation of the system (29), for this, we propose the following coordinate transformation:

$$\begin{aligned}\left[ {\begin{array}{*{20}{c}} {{z_{11}}} \\ {{z_{12}}} \\ {{z_{13}}} \\ {{z_{21}}} \\ {{z_{22}}} \\ {{z_{23}}} \\ {{z_{31}}} \\ {{z_{32}}} \\ {{z_{33}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} x \\ z \\ \alpha \\ {{p_1} + {k_{11}}\left( {{\omega _1} - {\lambda _1}} \right) } \\ {{p_3} + {k_{22}}\left( {{\omega _2} - {\lambda _2}} \right) } \\ {{p_5} + {k_{33}}\left( {{\omega _3} - {\lambda _3}} \right) } \\ {{\omega _1}} \\ {{\omega _2}} \\ {{\omega _3}} \end{array}} \right] \end{aligned}$$

The extended PCH model of the PVTOL transporting a payload has the form (19) with the Hamiltonian function defined as follows:

$$\begin{aligned} {H_{z}}= & {} \frac{{z_{21}^2}}{{2{a_1}}} + \frac{{z_{22}^2}}{{2{a_2}}} + \frac{{z_{23}^2}}{{2{a_3}}}\nonumber \\&+ \frac{{{{\left( {{z_{31}} - {\lambda _1}} \right) }^2}}}{2} + \frac{{{{\left( {{z_{32}} - {\lambda _2}} \right) }^2}}}{2} \nonumber \\&+ \frac{{{{\left( {{z_{33}} - {\lambda _3}} \right) }^2}}}{2} - \frac{{M{l^3}{m^2}g}}{{\left( {M + m} \right) {a_3}}}\cos {z_{13}} \nonumber \\&+ \frac{{{k_{px}}}}{2}{\left( {{z_{11}} - {z_{d11}} + \frac{{lm}}{{M + m}}\sin {z_{13}}} \right) ^2} \nonumber \\&+ \frac{{{k_{pz}}}}{2}{\left( {{z_{12}} - {z_{d12}} - \frac{m}{{M + m}}\cos {z_{13}}} \right) ^2} \end{aligned}$$

(33)

From (26) and (33), we obtain the outer-loop control law $\varvec{v}$, which corresponds to the proportional-integral action. Remember that this part of the Robust IDA-PBC algorithm rejects the external disturbances.

$$\begin{aligned} \varvec{v} = \left[ {\begin{array}{*{20}{c}} { - \frac{{{k_{i1}}{k_{p1}}}}{{{a_1}}}{z_{31}} - {k_{i1}}{\dot{z}}_{31}} \\ { - \frac{{{k_{i2}}{k_{p2}}}}{{{a_2}}}{z_{32}} - {k_{i2}}{\dot{z}_{32}}} \end{array}} \right] \end{aligned}$$

(34)

where $k_{i1}>0$, $k_{i2}>0$ are components of the gain matrix $\varvec{K_i}$ and $k_{p1}>0$, $k_{p2}>0$ of the matrix $\varvec{K_p}$.

Table 1 Model parameters

Finally, we compute the final control law $\varvec{u_T}$ of the Robust IDA-PBC strategy as the sum of two terms. The first term due to the traditional IDA-PBC strategy is given by the sum of (7) and (8), and the second term is obtained from the new proposed approach due to the addition of the proportional-integral action (34).

In summary, as a direct consequence of Theorem 1 given in Sect. 3, the total control law $\varvec{u_T}$ applied to a PVTOL transporting a suspended payload, guarantees the following points:

• Asymptotic stability at the equilibrium point.

• Rejection of constant external disturbances.

• Stabilization of the cable-suspended payload swing.

Remark 3

Note that the same model and control methodology can be applied analogously for movements in the xy plane.

Fig. 3

Robustness analysis under different disturbance values

Fig. 4

Comparison results for the proposed robust IDA-PBC with the controller of [22]

4.3 Simulations and results with external disturbances

In order to analyze the performance and versatility of the proposed control scheme, some simulations have been carried out. In this subsection, we present the performance of the Robust IDA-PBC algorithm applied to a UAV transporting a payload in the presence of constant disturbances.

The initial conditions of the Hamiltonian system are $z_{11}(0)=z_{12}(0)=z_{13}(0)=z_{21}(0)=z_{22}(0)=z_{23}(0)=z_{31}(0)=z_{32}(0)=z_{33}(0)=0$. The goal is to move the vehicle transporting a payload to the desired position of 1 meter up and 2 meters forward along the coordinate x. In simulations, we used model parameters close to small-scale real aerial platforms, such parameters are shown on Table 1.

Table 2 Qualitative comparison of robust IDA-PBC algorithms

The control objective is to stabilize the vehicle’s position and attenuate the oscillations on the swing angle of the payload, despite the presence of constant external disturbances.

The considered disturbances are:

• A constant force of 10N, 20N, and 30N injected in the total thrust force f at $t = 15$s.

• A constant torque of 10Nm, 15Nm, and 20Nm injected in the torque $\tau _\theta$ at $t=25$s.

Figure 3a shows the x and z positions of the vehicle during the validation. We can see that the position for each axis is stabilized according to the desired reference points $x_d=2$m and $z_d=1$m. It is also very important to appreciate that the control law stabilizes the positions to their desired references, despite the existence of the external matched perturbations occurring at 15s and 25s.

Figure 3b presents the payload swing angle $\alpha$. It is clear that the proposed control law exhibits good performance since the payload swing angle is rapidly regulated to $0^o$ despite of the existence of the constant external disturbances.

Figure 3c displays the vehicle orientation. We can observe that the attitude converges to the desired points with a null steady-state error despite the strong external disturbances induced to the system.

Figure 3d illustrates the states produced by the integral action. The dynamics of the states have variations, which after a few seconds remain constant. This considerably helps the control system to reject the constant disturbances present in the inputs f and $\tau _\theta$.

In Fig. 3e, it can be seen that the total thrust force and the torque are smooth signals. Also, Fig. 3e shows that the controller produces the necessary control inputs to reject constant disturbances at time 15s and 25s and thus maintain the output desired values.

4.3.1 Comparison with other control strategy

The proposed robust IDA-PBC algorithm was compared with respect to the second control law from [22]. The responses of both algorithms are shown in Fig. 4. For better understanding the figures, the subscript p indicates the proposed IDA-PBC robust controller while the subscript c associates the controller from [22]. It is clear that the proposed controller rejects the disturbances at 20s and 60s. In contrast, the controller from [22] fails to reject the disturbances and presents a huge change in the x and z positions when they occur, manifesting a very large steady-state error, i.e., it presents an offset of twice the reference in z and more than three times the reference in the case of the position x.

Finally, in order to compare this work with the robust IDA-PBC strategies available in the literature, we provide Table 2. Several characteristics are considered to discuss the generality and efficiency of the proposed robust IDA-PBC technique. From Table 2 we can observe that all presented controllers have advantages and disadvantages. However, we consider that the main limitations are: the previous algorithms can not be applied to systems with inertia matrix dependent of the unactuated coordinates, and the presented applications have only been studied with second-order systems with a single input.

5 Conclusions

An algorithm to design a robust IDA-PBC strategy for the case of under-actuated PCH systems with inertia matrix dependent of the unactuated coordinates has been presented in this work. The proposed methodology requires adding an outer-loop to the IDA-PBC strategy to reject external disturbances. We have proved that an integral action with a particular change of coordinates can be added in an outer-loop to the IDA-PBC strategy to reject constant external disturbances. The asymptotic stability of the proposed controller despite the constant external disturbances is guaranteed using the closed-loop Hamiltonian as a Lyapunov candidate function and LaSalle’s invariance principle.

As a proof of concept, the control scheme has been applied to a PVTOL transporting a cable-suspended payload. The objective was to stabilize the PVTOL position and attenuate the swing angle of the payload. Simulations have shown that the performance of the proposed robust control strategy is satisfactory, despite of the existence of the constant external disturbances, illustrating the potential of this control scheme. Future work includes extending the scheme for the case of variable external disturbances and unmatched disturbances. Also, to apply the proposed methodology to other cases of study such as a quadrotor. The main challenge in the case of variable external disturbances lies in proposing the appropriate state transformation that retains the PCH structure in the closed-loop extended system with the change of coordinates, while preserving the asymptotic stability.