A Novel Passivity-Based Trajectory Tracking Control For Conservative Mechanical Systems

The industry standard trajectory tracking control algorithms for mechanical systems depend on the well known Euler-Lagrange formulation of the dynamics in terms of generalised coordinates $q\in\mathbb{R}^{n}$ (for example [SHV06])

for a bounded generalised inertia matrix $M(q)>0$, Coriolis matrix $C(q,\dot{q})$, potential $U(q)$, and exogenous generalised input $\tau$. The goal is to find an input $\tau(t)$ such that the systems trajectory $q(t)$ converges asymptotically to a known reference trajectory $q_{\text{r}}(t)$. The standard approach (computed torque with passive damping injection) exploits the Euclidean structure of the generalised coordinates $q\in\mathbb{R}^{n}$ explicitly in creating an error $\tilde{q}=q-q_{\text{r}}$. A trajectory and state dependent feed-forward torque $\tau_{\text{ff}}$ is computed to bring the error dynamics into a time-varying linear structure

where $\tilde{\tau}=\tau-\tau_{\text{ff}}$ is the residue torque. From here the algebraic passivity properties of $M(q)$ and $C(q,\dot{q})$ are exploited to show a $PD$ control (from $\tilde{q}$ to $\tilde{\tau}$) stabilizes the error coordinates $\tilde{q}\rightarrow 0$ and hence $q(t)\rightarrow q_{\text{r}}(t)$.

Although this approach has been an effective tool in nonlinear control design for many years, it is subject to funamental limitations:

1. 1.

   The error $\tilde{q}$ is a Euclidean difference that has no physical interpretation in terms of the structure of the mechanical system and is only defined locally (in a coordinate patch) for a general system with a manifold state-space.

2. 2.

   The feed-forward computed torque $\tau_{\text{ff}}$ has no clear physical interpretation. Computing this requires real-time measurements of $(q,\dot{q})$ as well as $(q_{\text{r}},\dot{q}_{\text{r}},\ddot{q}_{\text{r}})$ and a good model of the system.

As a consequence, systems for which the state space is inherently non-Euclidean, such as mobile robots moving in $\mathbf{SE}(3)$, or systems where the model is uncertain, such as manipulators moving objects that are not well characterised, are difficult to address using the classical tracking framework. Furthermore, due to the complex nonlinear dependence of the feed-forward control on the system state many of the tricks used for velocity free passivity based control, or passivity based robust disturbance rejection [SSF07] cannot be exploited.

In this paper, I propose a ‘true’ passivity based tracking control for conservative mechanical systems. The key innovation is to use the reference trajectory as a parametrization for a single coordinate Hamiltonian system with kinetic and potential energy inherited from the full system. The desired reference trajectory behaviour corresponds to constant velocity motion of the single parameter reference system and the exogenous input that generates this trajectory is uniquely defined. Due to the fact that the system has a single coordinate the input associated to the reference trajectory can be integrated to generate a reference potential. Interpreting the reference input as shaping the potential of the reference system, the constant velocity solution becomes a ‘free’ solution of the single coordinate Hamiltonian reference system.

The second contribution of the paper is to show how to use the reference potential to shape the true system potential energy in such a way that the reference trajectory is a free solution of the (shaped) full Hamiltonian system. The proposed control then interconnects the shaped reference and shaped true system using non-linear spring dampers in order that the response of both systems is synchronised. This step is the primary control that forces the true system to converge towards the desired reference behaviour. The final stage of the control introduces an energy pump that stabilises the energy level in both true and reference system to the desired reference energy level ensuring that the response of the interconnected systems converges asymptotically to the desired reference trajectory.

The resulting control is inherently energy based and does not involve linearisation of the dynamics. The response should be highly robust to model errors and external disturbances and particularly well suited to trajectory tracking in the presence of unknown but energy bounded disturbances. The philosophy of the design draws strongly from the inspiration of Full et al. [FK99] and Westervelt et al. [WGK03] in stabilising an embedded behaviour in a larger mechanical system to obtain emergent behaviour in the zero dynamics. A limitation of the approach is that the desired trajectory $q_{r}(t)$ is only tracked up to an arbitrary time-offset $t_{0}$; that is, $q(t)\rightarrow q_{r}(t-t_{0})$.

Trajectory tracking control for dynamical mechanical systems is a classical problem robotics [SL91, SHV06]. The industry standard method for trajectory tracking is that of computed torque control, first developed in the seventies [Mar73, Pau82]. The summary of this approach covered in the introduction of the paper was based on classical robotics texts [Cra89, SL91, SHV06]. Set point regulation has received considerably more attention. In 1981 Takegaki et al. [TA81] demonstrated that a PD control with gravity compensation can regulate a robotic manipulator to any fixed point in its workspace, and this work was generalised to adaptive passivity based set point regulation during the eighties [MG86, LS89] with the global stability analysis for PD, PID, and PI²D controls provided in the nineties [Kel95, OLNS98, Lef00]. Although approximate trajectory tracking can be accomplished with joint based PD [Kod87, KMA88] most theoretical developments of these techniques have been aimed at regulation of set-point or constant energy trajectories [Lef00].

A seminal contribution to tracking trajectory control is the work by Fujimoto et al. [FSS03] in 2003. In this work the authors state; “The key idea to realize [the objective of tracking control] is to embed the desired trajectory into the Hamiltonian function of the original system.” They provide a characterisation of a generalised canonical transform that transforms a port Hamiltonian system into one written in terms of the error coordinates

(compare to (2)) which can be stabilised using passive damping injection. The approach is conceptually rigorous but formally requires the solution of a Partial Differential Equation (PDE) to find the canonical transformation that defines the global error $\tilde{q}$ coordinates. This requirement is reminiscent of the IDA-PCA algorithms [OLNS98] developed around the same time and the complexity of the design process has limited their adoption in the wider community.

A separate research thread I draw from in this paper is summarised in the philosophy stated in Westervelt et al. [WGK03] “to encode the dynamic task via a lower dimensional target, itself represented by a set of differential equations” [NFK00] motivated by the study of how biological systems move [FK99]. This work has been important in development of advanced trajectory tracking algorithms for gait control for humanoid robots [WGK03] in particular, and in general for control of robotic motion. I also draw from the pioneering work by Spong [Spo95] on energy pumping for stabilisation of constant energy set-points for under-actuated mechanical systems.

Let $q\in\mathbb{R}^{N}$ denote (local) generalized coordinates for a fully actuated mechanical system with generalized inertia matrix $M(q)>0$ and potential function $U(q)$. Lagrange’s equations yield dynamics (3) for a Coriolis matrix $C(q,\dot{q})$. The passivity of the system is expressed algebraically by the relationship that

is skew symmetric along trajectories of the system. The Hamiltonian for this system is

and passivity of the system ensures that

where $\left\langle\cdot\;\vline\;\cdot\right\rangle$ is notation for the power bracket between a generalised input and a velocity, and can be read as a simple inner product $\tau^{\top}\dot{q}$ in local coordinates.

The trajectory tracking problem is associated with a pre-specified reference trajectory $q_{\text{r}}(t)$. In this paper, I will consider only non-stationary reference trajectories that are bounded, twice differentiable with bounded differentials. The non-stationary assumption,

is the only unusual assumption. Indeed, stationary trajectories are exactly those trajectories that can be easily tackled using classical passivity based techniques [OLNS98]. This assumption emphasises the fundamental differences between the present and previous energy based approaches to tracking. Although I believe that it may be possible to relax this condition in future work, the assumption considerably simplifies the present work. In fact, I will assume slightly more than (6), that the velocity is bounded away from zero. That is, that there exists a uniform bound $\beta>0$ such that $\dot{q}_{\text{r}}^{\top}M(q_{\text{r}})\dot{q}_{\text{r}}>\beta$ for all time.

The nominal goal of the trajectory tracking problem is to find a control signal $\tau$ for (3) depending on the known reference trajectory $q_{\text{r}}$ and its derivatives $(\dot{q}_{\text{r}},\ddot{q}_{\text{r}})$ along with state measurements $(q,\dot{q})$ such that for any initial condition $q_{0}$ the solution $q(t;q_{0})$ of the closed-loop system (3) converges to the reference, $q(t;q_{0})\rightarrow q_{r}(t)$ asymptotically. The proposed algorithm achieves $q(t;q_{0})\rightarrow q_{r}(t-t_{0})$ asymptotically for some time-offset $t_{0}\in\mathbb{R}$. Although this is not explicitly a solution to the classical trajectory tracking problem it is clearly applicable to a wide range of applied problems and the difference again emphasises the difference in approach.

In this section, I consider the dynamics of the reference trajectory. The approach taken is to reparametrize the reference trajectory as a one parameter mechanical system constrained to follow a reference path. The key advantage of this reformulation is to derive a potential function that encodes the information necessary to generate the exogeneous generalised force input associated with the reference trajectory. The goal of the section is to formulate a closed Hamiltonian system for which the reference trajectory is a free solution.

The starting position is to consider the reference $q_{\text{r}}(t)$ as a solution of the mechanical system (3). In particular, there must be a reference input $\tau_{\text{r}}$ associated with the actual generalised forces required to track the reference trajectory. More correctly, we consider a reference trajectory to be a quadruple of variable trajectories $(q_{\text{r}}(t),\dot{q}_{\text{r}}(t),\ddot{q}_{\text{r}}(t),\tau_{\text{r}}(t))$ defined on a time interval $[0,\infty)$ that satisfy the system relationship

for $t\in(0,\infty)$. Note that the reference trajectory information, including the reference input $\tau_{\text{r}}$ can be computed in advance.

Let $s$ be a path parameter for the reference trajectory. That is we consider the map $q_{\text{r}}:\mathbb{R}\rightarrow\mathbb{R}^{n}$, $s\mapsto q_{\text{r}}(s)$. Consider the mechanical system obtained by taking $s$ to be a generalised coordinate $s:=s(t)$ for a new one-dimensional mechanical system that characterises the motion of the reference along its predefined trajectory. That is, as $s(t)$ varies in one dimension, the reference variable $q_{\text{r}}$ moves along the constrained path $q_{\text{r}}(s(t))$. Define the partial differentials of $q_{\text{r}}$ with respect to $s$ by

For a solution $s:=s(t)$ one has $\dot{q}_{\text{r}}=q^{\prime}_{\text{r}}(s)\dot{s}$ and

The kinetic energy of the constrained reference system is

which defines $M_{\text{r}}(s)$ while the potential is a simple reparameterization $U_{\text{r}}(s)=U(q_{\text{r}}(s))$.

The Lagrangian for this system is given by $\mathcal{L}_{\text{r}}(s,\dot{s})=K_{\text{r}}(s,\dot{s})-U_{\text{r}}(s)$. Consider the Euler-Lagrange equations $\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial}{\partial\dot{s}}\mathcal{L}-\frac{\partial}{\partial s}\mathcal{L}=e$ where $e$ is a scalar exogenous input added as a driving term. It is easily verified that

The dynamics (9), for a general input $e$, models the motion of a mechanical single variable system along the parametrization provided by $q_{\text{r}}(s)$. The reference trajectory itself $q_{\text{r}}(t)$ is recovered by the particular solution $s(t)=t$ and will correspond to a particular choice of scalar reference input $e=e_{\text{r}}(s(t))$. Since the defining equations are time-invariant, then any solution $s(t)=t-t_{0}$, $t_{0}\in\mathbb{R}$, will also be a solution (9) associated with the offset reference trajectory $q_{\text{r}}(t-t_{0})$ for $t-t_{0}\in(0,\infty)$. It follows that the scalar reference input $e_{\text{r}}(s)$ can be computed as function of path parameter $s$ independent of time. Substituting the relationships $\dot{s}=1$, $\ddot{s}=0$, characteristic of the trajectory $s(t)=t-t_{0}$, into (9) yields

Define a reference potential

The potential $W_{\text{r}}(s)$ encodes information on the scalar reference input associated with the reference trajectory for the scalar system (9). The input $e_{\text{r}}(s)=-\frac{\partial}{\partial s}W_{\text{r}}(s)$ (10) should be thought of as a potential shaping input where the term $-\frac{\partial}{\partial s}U_{\text{r}}(s)$ cancels the effect of the potential $U_{\text{r}}$ inherited from the full system, while the term $-\frac{\partial}{\partial s}\frac{1}{2}M_{\text{r}}(s)$ is associated with a new shaped potential that compensates the energy balance for variation in the kinetic energy $\frac{1}{2}M_{\text{r}}(s)\dot{s}^{2}$ along the reference trajectory. That is, the reference potential $W_{\text{r}}$ shapes the potential of the reference system to supply the power required to track the reference trajectory.

Set $e=e_{\text{r}}(s)+e_{\text{c}}$ where $e_{\text{c}}$ is a coupling input from the reference system to the true system and will be defined in §5. The Euler-Lagrange equations for (9) become

The dynamics (12) are associated with the port Hamiltonian system with Hamiltonian

One has

where $\left\langle\dot{s}\;\vline\;e_{\text{c}}\right\rangle=\dot{s}e_{\text{c}}$ is the power port for the scalar reference system.

It is easily verified that the unforced or zero dynamics of (12) (with $e_{\text{c}}\equiv 0$) include the solution $\ddot{s}=0$, $\dot{s}=1$ as expected. This solution is associated with a constant (shaped) energy level

since $\dot{s}\equiv 1$ along the reference trajectory. This energy level is, however, not a minimum energy level for the system, with trajectories in phase portrait (Fig. 1) that lie inside the $\dot{s}=\pm 1$ lines all corresponding to lower energy levels. The minimum energy trajectories for the reference system will correspond to the stationary points $\dot{s}=0$. These stationary points occur at points where $\frac{\partial M_{\text{r}}}{\partial s}=0$ and their stability will depend on the sign of $\frac{\partial^{2}M_{\text{r}}}{\partial s^{2}}=0$ (see Fig. 1). The variation in $M_{\text{r}}(s)$ is a function of the reference trajectory, both in the change in magnitude of $q^{\prime}_{\text{r}}(t)$ as well as the distribution of inertia in $M(q(s))$. Thus, reference trajectories with high accelerations and complex inertia matrices will lead to complex zero dynamics in the reference system, while tracking simply trajectories with near constant velocities and homogeneous inertia will lead to simple zero dynamics. Interestingly, the unforced system also includes solutions associated with negative time evolution of the reference system, and a zero energy solution $\dot{s}=-1$ in particular.

In this section I present the first part of the proposed tracking control design. The idea is to shape the potential of the true system with the reference potential $W_{\text{r}}$ and then couple the true and reference systems together using passive interconnections. The resulting control synchronises the state $q(t)$ of the true system (3) with the state $q_{\text{r}}(s(t))$ of the reference system (12). The final closed-loop system will require an additional energy pumping control discussed in §6 to stabilise the desired energy level set.

The principle control used for synchronization between the true and reference systems is a spring potential introduced between $q(t)$ and $q_{\text{r}}(s(t))$. Let $\phi:\mathcal{M}\times\mathcal{M}\rightarrow\mathbb{R}$ be a positive definite smooth cost function with $\phi(q,q_{\text{r}})=0$ if and only if $q=q_{\text{r}}$. Note that there is no need to compute the linear error $q-q_{\text{r}}$ in order to find such a spring potential function although the potential $\frac{1}{2}K|q-q_{\text{r}}|^{2}$ used in the classical tracking algorithms is an example of such a potential.

The generalised inertia $M(q)$ provides a Riemannian metric on the tangent bundle $T\mathcal{M}$. Using this metric one can measure the speed of a trajectory $\dot{q}$ by computing $\sqrt{\dot{q}(t)^{\top}M(q(t))\dot{q}(t)}$. In a similar manner one can measure the desired speed of the reference trajectory at a point $s$ by computing $\sqrt{q^{\prime}_{\text{r}}(s)^{\top}M(q(s))q^{\prime}_{\text{r}}(s)}=\sqrt{M_{\text{r}}(s)}$. Note that the reference speed is associated with the desired reference trajectory and does not depend on the velocity of the path parameter $\dot{s}$. Since only non-stationary reference trajectories were considered, the reference speed $\sqrt{M_{\text{r}}(s)}\not=0$. The ratio of these two speeds as a function of time

can be interpreted as a measure of the speed at which the true system is moving relative to a desired reference speed associated with a point on the reference trajectory specified by the path parameter $s(t)$. If $q(t)\approx q_{\text{r}}(s(t))$ and $\dot{q}(t)\approx q_{\text{r}}^{\prime}(s(t))$ then $\dot{\sigma}\approx 1$, that is the true trajectory is moving at the same speed as the desired trajectory.

The integral of (15),

can be interpreted as a (relative) path parameter for the true system, analogous to the path parameter $s(t)$ introduced for the reference trajectory $q_{\text{r}}$. If $q(t)\approx q_{\text{r}}(s(t))$ and $\dot{q}(t)\approx q_{\text{r}}^{\prime}(s(t))$ then $\sigma(t)\approx t-t_{0}$. The new path parameter $\sigma$ will be used as a storage variable in a copy of the reference potential $W_{\text{r}}(\sigma)$ that can be used to shape the potential for the true system.

Define $\tau_{\dot{q}}\in T^{\star}_{q}\mathcal{M}$, the dual (generalised) torque to a velocity $\dot{q}\in T_{q}\mathcal{M}$, to be the unique element that solves

for all $V\in T_{q}\mathcal{M}$. Note that $\tau_{\dot{q}}=0$ in the case where the velocity $\dot{q}=0$ is zero. Recalling the reference input $\tau_{\text{r}}$ associated with the reference trajectory (7), define the constraint force

By construction $\left\langle\tau_{\text{r}}^{0}\;\vline\;\dot{q}\right\rangle=0$ and thus does no work on the system. In the asymptotic case, where $\sigma(t)=s(t)=t-t_{0}$ then $\dot{\sigma}^{2}=1$ and $\frac{\dot{q}^{\top}M(q)\dot{q}}{M_{\text{r}}(s)}=\frac{\left\langle\tau_{\dot{q}}\;\vline\;\dot{q}\right\rangle}{M_{\text{r}}(s)}=1$. It follows that in this case $\tau^{0}_{\text{r}}=\tau_{\text{r}}-\frac{\left\langle\dot{q}\;\vline\;\tau_{r}\right\rangle}{M_{\text{r}}(s)}\tau_{\dot{q}}$ is the component of the reference input $\tau_{\text{r}}$ orthogonal to the dual torque $\tau_{\dot{q}}$. That is, the component of the reference input that acts orthogonal to the velocity of the system as a constraint force and steers the system to follow the desired trajectory. The power required to accelerate and decelerate the system along the trajectory is not provided by the constraint force $\tau^{0}_{\text{r}}$ and must be separately modelled. The active force in direction $\tau_{\dot{q}}$ will be coupled to the potential $W_{\text{r}}(\sigma)$ (22).

A key requirement of the proposed control, separate from the synchronisation of $q(t)$ and $q_{\text{r}}(s(t))$, is to synchronise the two path parameters $\sigma(t)$ and $s(t)$. To this end define a second potential

Note that since both $s$ and $\sigma\in\mathbb{R}$ are scalars, then this potential can be defined globally as a simple quadratic error. The introduction of this potential will bound growth of $\sigma(t)$ with respect to growth of $s(t)$, and vice-versa, ensuring that the two path parameters remain close during the system transient.

The proposed synchronisation control is a closed passive interconnection of Hamiltonian systems and resulting combined system is itself a passive Hamiltonian system. Theorem 5.1 demonstrates that the combined solution will converge asymptotically to a trajectory that lies in the set of forward invariant trajectories characterised by $\dot{E}=0$ and $\tilde{s}=\text{const.}$. As shown in the next section, the free trajectories $\dot{s}=1$ and $q(t)=q_{\text{r}}(t)$ certainly lie in this forward invariant set. However, without additional control, the damping term will continue to extract energy from the system, even if it is in small increments due to small disturbances, until the limiting trajectory has zero kinetic energy and lies at a local minimum of the combined potential $U(q)+W_{\text{r}}(\sigma)+W_{\text{r}}(s)+\phi(q,q_{\text{r}})+\psi(s,\sigma)$. (Note that for $\dot{q}=0$ then $\tau_{\text{r}}^{0}=0$ by construction.) Such solutions also lie in (a separate component of) the forward invariant set of the combined system trajectories. In particular, synchronisation of the trajectories by itself is not sufficient to ensure the desired trajectory tracking and in the next section I propose an additional external energy pumping control to stabilise the energy level the combined system.

This section presents two results; firstly an explicit demonstration that the reference trajectory is a forward invariant trajectory for the combined Hamiltonian system considered in Theorem 5.1. Secondly, I characterise the energy level of the reference trajectory for the combined system and propose a simple energy pump outer loop control that stabilises this energy level in the system and consequently stabilises the desired trajectory.