Dynamic Gravity Cancellation and Regulation Control
in Robots with Flexible Transmissions: Constant,
Nonlinear, and Variable Stiffness
Alessandro De Luca Fabrizio Flacco
Dynamic Gravity Cancellation and Regulation Control
in Robots with Flexible Transmissions:
Constant, Nonlinear, and Variable Stiffness
Alessandro De Luca Fabrizio Flacco
Dipartimento di Informatica e Sistemistica Universit`a di Roma “La Sapienza” Via Ariosto 25, 00185 Roma, Italy {deluca,fflacco}@dis.uniroma1.it
July 2, 2010
Abstract
We consider the problem of perfect cancellation of gravity effects in the dynamics of robot manipulators having flexible transmissions at the joints. Based on the feedback equivalence principle, we aim at designing feedback control laws that let the system outputs behave as those of the same robot device when gravity is absent. The cases of constant stiffness (elastic joints), nonlinear flexible, and variable nonlinear flexible transmissions with antago-nistic actuation are analyzed. As a particular case, antagoantago-nistic actuation with transmissions having constant but different stiffness is also considered. In all these situations, viable solutions are obtained either in closed algebraic form or by a simple numerical technique. The compensated system can then be controlled without taking into account the gravity bias, which is particularly relevant for safe physical human-robot interaction tasks where such compli-ant manipulators are commonly used. Moreover, dynamic gravity cancella-tion allows to design new PD-type regulacancella-tion controllers and to show their global asymptotic stability without the need of any positive lower bound nei-ther on the stiffness nor on the proportional control gain. A Lyapunov-based proof is provided for the case of robots with elastic joints. Simulation results are reported to illustrate the obtained performance in the various robotic sys-tems with flexible transmissions.
Keywords: robots with flexible joints, variable stiffness actuation, feedback equivalence, gravity cancellation, motion and stiffness control.
1
Introduction
Robots in physical interaction with humans are conveniently controlled so as to achieve zero-gravity operation [1]. This avoids biasing the robot reaction to unin-tended collisions along the gradient of the gravitational potential, with a uniform and more predictable (thus safer) robot behavior in its whole workspace [2]. Per-fect cancellation of gravity is trivial for fully rigid manipulators. In fact, for their standard dynamic model
M (q)¨q + c(q, ˙q) + g(q) = τ , the choice
τ = τg+ τ0, τg = g(q)
removes gravity from the picture in a complete way (i.e., both statically and dy-namically), thanks to the colocation of gravity and input torques (and to the full actuation of the system). The additional command τ0 is left to the control
de-signer for performing desired tasks, e.g., set-point regulation, trajectory tracking, or reaction to a contact with the environment.
However, robots intended for physical Human-Robot Interaction (pHRI) in-clude compliant elements in their mechanical construction, in order to reduce the possibility of injuries due to unexpected collisions [3]. Robot links are designed as lightweight but rigid, while compliance is typically concentrated in the transmis-sions at the joints, either with finite constant stiffness K, e.g., when using harmonic drives [4], or with variable (and independently actuated) nonlinear stiffness [5].
The common dynamic model of robots with constant joint elasticity takes the form [6]
M (q)¨q + c(q, ˙q) + g(q) + K(q − θ) = 0 B ¨θ + K(θ − q) = τ ,
where actuation torques τ appear on the motor side of the elastic joints (i.e., per-forming work on θ), while gravity loading g(q) affects primarily the dynamic behavior of the variables on the link side (i.e., q). This non-colocation is a major problem for control. Gravity compensation laws have been proposed for regula-tion tasks, when the link posiregula-tion q has to be asympotically stabilized to a desired constant value qd. A first solution is based on motor PD feedback with constant gravity compensation at steady state [7]
τ0 = KP(θd− θ) − KD˙θ, τg = g(qd),
with θd= qd+ K−1g(qd), KP > 0, and KD > 0. In order to show asympotic
stability by Lyapunov arguments, the proportional gain should be chosen so that the norm of KP is larger than a positive constant related to gravity, whereas the
stiffness matrix K is assumed to dominate the gradient of g(q). Indeed, this com-pensation cancels gravity only in the final static condition. Since the gravity term
changes with the robot configuration, an on-line compensation has been proposed in [8] by evaluating g in τgwith a gravity-biased measure of the motor position
τg = g(˜θ), θ = θ − K˜ −1g(qd).
While the transient performance is largely improved, the theoretical restriction on KP could not be removed in the Lyapunov analysis. A better result is achieved
in [9], with a gravity compensation of the form τg = g(¯q(θ)),
where, for a measured motor position θ, ¯q(θ) is computed by numerically solving the quasi-static relation g(q) + K(q − θ) = 0. This variant is able to relax the lower bound on KP so that asymptotic stability can be shown through a modified
Lyapunov function. On the other hand, the structural condition on the joint stiffness kKk > k∂g(q)/∂qk should still hold.
All the above control laws have the merit of using only feedback from the motor variables θ and ˙θ. However, none of them removes completely the effects of gravity, especially in highly dynamic tasks: only a partial compensation, and not a cancellation, of the gravitational load acting on the robot link motion is obtained. In the context of robot reaction to collisions, we also note that a practical solution for compensating gravity in elastic joint robots has been proposed in [10], based on the availability of joint torque sensors. The use of this additional sensor can be interpreted as involving also the link position q in the control law. Furthermore, under the assumption that full state is available, it is known that all robots with elastic joints can be exactly linearized by means of a static [11] or dynamic state feedback (the latter is needed when some extra inertial terms are included in the model) [12]. This structural control property will be further exploited in this paper. The most recent research in pHRI calls for the use of variable stiffness actua-tion(VSA), in which each joint is driven by two independent actuators that allow to control link motion as well as device stiffness [13, 14, 15, 16] and to shape the compliant interaction with the environment. Actuators are typically arranged in antagonistic mode [17], with both motors of each joint being involved in robot motion and stiffness variation. Other systems use a separate actuation [18, 19] for stiffness control. In any event, in order to modify the device stiffness, a nonlin-ear characteristics of the flexible transmissions is needed. This can be realized using either nonlinear springs or linear springs mounted on nonlinear kinematic mechanisms. For VSA systems, the paradigm is “design for safety, control for per-formance” [5]. In particular, the robot can be made more compliant at high speeds and stiffer at low speeds, thus limiting the energy exchange in the first few instants after an unexpected impact. Up to now, the presence of gravity in VSA-based robots has not been treated rigorously, with experimental single-dof devices mov-ing in the horizontal plane or usmov-ing only a partial gravity compensation —just as in the constant stiffness case. Nonetheless, a large class of VSA-based systems of the antagonistic type has been shown to be feedback equivalent to linear, controllable,
and input-output decoupled systems [20, 21], with the linearizing outputs being the link position and the device stiffness. Also for this class of flexible devices, such a control property will be useful for removing the dynamic effects of gravity in a complete and efficient way.
In this paper, we present new control results that allow perfect gravity cancel-lation for a variety of robotic systems with flexible transmissions. Based on the general principle of feedback equivalence [22], we will design for all cases static state feedback laws that accurately match the same dynamic behavior of the driven links as if they were moving in the absence of gravity. In Sect. 2, we consider the case of robots with n elastic joints having constant stiffness and single actuation. To illustrate one of the benefits of this approach, Section 3 presents and analyzes a global PD-type regulation controller for robots with elastic joints, which is de-signed on top of the available dynamic gravity cancellation law. An interesting outcome is that there are no restrictions imposed by gravity neither on the con-trol gains nor on the joint stiffness. In Sect. 4, the design of a dynamic gravity cancellation law is extended to transmissions with nonlinear flexibility and single actuation. The case of double antagonistic actuation with variable nonlinear stiff-ness is handled in Sect. 5. For VSA-based robots, we will be able to impose also a dynamic behavior to the nonlinear stiffness of the device which is identical to that of the no-gravity case. Finally, the particular instance of antagonistic actuation with transmissions having constant but different stiffness is considered in Sect. 6. In this case, the actuation becomes redundant for motion purposes and space is left for optimizing the control torques. Illustrative simulation results are given in each section.
2
Robots with Elastic Joints
Consider a robot manipulator having n elastic joints of constant stiffness and with n driving motors. Let q and θ be the n-dimensional vectors of link and motor vari-ables. Under the simplifying modeling assumption of Spong [11], and including also viscous effects at the motor and link side, the dynamic model takes the form
M (q)¨q + c(q, ˙q) + g(q) + Dq˙q + K(q − θ) = 0 (1)
B ¨θ + Dθ˙θ + K(θ − q) = τ , (2)
where M > 0 is the robot inertia matrix, the constant diagonal matrix B > 0 contains the motor inertias, c is the vector of centrifugal and Coriolis terms, g is the gravity vector, K > 0 is the diagonal matrix of joint stiffnesses, and Dq and
Dθare positive semi-definite diagonal matrices of viscous friction coefficients. In
terms of transmission deformation φ = q − θ, the elastic potential Ue= 12φTKφ
associated to (1–2) leads to the linear elasticity torque vector τeand constant
de-vice stiffness (diagonal) matrix σ τe = ∂Ue ∂q T = Kφ, σ = ∂τe ∂q = K.
Our control goal is to define a (nonlinear) feedback law τ = τ (q, θ, ˙q, ˙θ, τ0)
in (2) such that the behavior of the compensated system matches in suitable coor-dinatesthat of an identical model but without gravity, i.e.,
M (q0)¨q0+ c(q0, ˙q0) + Dq˙q0+ K(q0− θ0) = 0 (3)
B ¨θ0+ Dθ˙θ0+ K(θ0− q0) = τ0, (4)
where the subscript 0 characterizes the variables of the robot in the absence of gravity.
It is well known [11] that system (1–2) is exactly linearizable by means of a static state feedback into decoupled chains of four integrators, with q and its first three time derivatives being the linearizing coordinates. Indeed, the same holds true also for system (3–4). Therefore, thanks to the feedback equivalence principle, by imposing the equality
q(t) ≡ q0(t), ∀t ≥ 0 (5)
one should obtain the desired result without resorting to the complexity of a com-plete feedback linearization process. One challenge in the application of this sim-ple idea is whether the solution can be found in closed form or not. Below, we show constructively that this is feasible for robots with elastic joints. The same concep-tual steps will be followed in all considered instances of transmission flexibility, albeit in some cases the solution will require a numerical procedure.
Differentiating once eq. (1) w.r.t. time yields
M (q)q[3]+ ( ˙M (q) + Dq)¨q + ˙c(q, ˙q) + ˙g(q) + K( ˙q − ˙θ) = 0,
with the notation q[i]= diq/dti. Differentiating one more time, and substituting ¨θ from (2), we obtain M (q)q[4]+ (2 ˙M (q) + Dq)q[3]+ ¨M (q)¨q + ¨c(q, ˙q) + K ¨q = KB−1 τ − Dθ˙θ − K(θ − q) − ¨g(q). (6) Noting that the left-hand side of (6) is a function of q and its first four deriva-tives only, we will write it compactly as f (q, ¨q, ¨q, q[3], q[4]). Repeating the same computation for the no-gravity model (3–4) leads to
f (q0, ¨q0, ¨q0, q[3]0 , q0[4]) = KB−1τ0− Dθ˙θ0− K(θ0− q)
. (7)
By imposing (5), the left-hand sides of (6) and (7) will be equal, and thus KB−1τ − Dθ˙θ − K(θ − q) − ¨g(q) = KB−1 τ0− Dθ˙θ0− K(θ0− q) . (8)
In order to eliminate the presence of the motor variables in (8), we use eqs. (1) and (3). By imposing again (5), one has
K(θ − q) = M (q)¨q + c(q, ˙q) + Dq˙q + g(q)
K(θ0− q) = M (q)¨q + c(q, ˙q) + Dq˙q,
or
θ = θ0+ K−1g(q), (10)
and then
˙θ = ˙θ0+ K−1˙g(q). (11)
Replacing eqs. (10–11) into (8) and simplifying, the solution to our problem is obtained by choosing the control law as
τ = τg+ τ0 (12) with τg = g(q) + DθK−1˙g(q) + BK−1g(q),¨ (13) where ˙g(q) = ∂g(q) ∂q ˙q ¨ g(q) = ∂g(q) ∂q M −1 (q) K(θ − q)−c(q, ˙q)−g(q)−Dq˙q + n X i=1 ∂2g(q) ∂q ∂qi ˙q ˙qi.
In addition, matched initial conditions should hold at time t = 0: q(0) = q0(0) ˙q(0) = ˙q0(0) ¨ q(0) = ¨q0(0) q[3](0) = q[3] 0 (0). (14) Note that, thanks to the control law (12–13), the identities (14) will be enforced for all t ≥ 0. The matching conditions (14) are not really a restriction. In fact, these conditions can be read in both directions: for a given initial state of the gravity-loaded system, we can always find an equivalent gravity-free system that has its initial state matched. This implies that the link coordinates of the two systems will evolve in the same way under the same command τ0.
A notable feature is that the control solution can be computed in closed form. Moreover, in static conditions, i.e., with ˙q = ¨q = 0, the gravity cancellation torque (13) becomes τg = g(q), as to be expected. Instead, in dynamic conditions
τgincludes terms that are proportional to the inverse of the joint stiffness K. Thus,
the more rigid are the transmissions the less extra dynamic torque is needed for gravity cancellation. In the limit, for K → ∞, we recover the standard gravity cancellation torque of the rigid case also in dynamic conditions.
We remark that, despite of the need of inverting the robot inertia matrix M (q), the gravity cancellation torque (13) is much simpler than the expression of a feed-back linearization control law, which involves in fact also the time derivatives of the model terms M (q) and c(q, ˙q) up to the second order.
Indeed, there are still differences in the state behavior between the gravity-free system (3–4) and system (1–2) under the gravity cancellation control law (12– 13). While the two systems will evolve in an identical way when looking at the linearizing coordinates q(t) ≡ q0(t), the inverse mappings of this evolution in
terms of the respective motor variables will be different, as dictated by eqs. (10) and (11). This should not be surprising from a physical point of view: the gravity-loaded robot needs the presence of a deformation q − θ 6= q − θ0that dynamically
balances the gravity on the link side. The control law (12–13) will only cancel the effects on the link (output) motion, which is what we actually need during robot interaction with the environment or a human.
The torque input τ0in (12) can be chosen according to the robot primary task,
e.g., as in [2] for a torque-based robot reaction to detected collisions in pHRI. In this context, perfect gravity cancellation allows a link behavior during transients and at steady-state that is totally unaffected by gravity bias. Furthermore, for a regulation task to a desired link position qd, it can be shown that the PD-type control law τ0= KP qd− θ + K−1g(q) −KD ˙θ − K−1 ∂g(q) ∂q ˙q ,
achieves global asymptotic stabilization for any KP > 0 and KD > 0, i.e.,
with-out the need of a strictly positive lower bound on KP. This result holds for any
K > 0, i.e., also for very soft joints. A formal proof of this result is presented in Sect. 3.
2.1 Simulation results
To illustrate the performance of the control law (12–13), it is sufficient to compare the behavior of a single link with an elastic joint in the absence of gravity and that under gravity but with dynamic gravity cancellation. In this case, the scalar link inertia M is constant and the gravity term is g(q) = mdg0sin q, where m is the
mass of the link, d is the distance of its center of mass from the joint, and g0 is the
gravity acceleration. The explicit expression of the dynamic gravity cancellation term τgin (13) is then τg = mdg0 n 1 −BKq˙2 sin q − MB mdg0 K sin q cos q + M Dθ−BDq KM q cos q +˙ B M(θ − q) cos q o . (15) Using M = 8.333, B = 50 [N·mm·s2/rad], m = 0.1 [kg], d = 250 [mm], Dq = 0.1, Dθ= 1 [N·mm·s/rad], and K = 100 [N·mm/rad] as data, we simulated
the two systems starting at rest from the downward equilibrium, and applying an open-loop torque τ0 = sin 0.1πt for T = 10 s. Figure 1 shows the obtained
evo-lution of the link (a) and motor (b) angles in the absence or presence of gravity. For the latter case, we have considered also the use of a simpler link-based com-pensation g(q) in place of the dynamic cancellation law τg given by (15). From
Fig. 1(a), it can be seen that q(t) = q0(t) exactly in the case of dynamic
cancella-tion, while an error is present when using g(q). On the other hand, θ(t) 6= θ0(t)
0 1 2 3 4 5 6 7 8 9 10 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time [s]
Link position [rad]
q with τ=τ0+τg q0 with τ=τ0 q with τ=τ0+g(q) (a) 0 1 2 3 4 5 6 7 8 9 10 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time [s]
Motor position [rad]
θ with τ = τ0+τg θ
0 with τ = τ0
θ with τ = τ0+g(q)
(b)
Figure 1: Comparison of link (a) and motor (b) position for a single elastic joint without gravity under τ0[dot-dashed, black], and with gravity under τ0and a
link-based compensation g(q) [dashed, blue] or under τ0 and the dynamic cancellation
law τg in (15) [continuous, red]
0 1 2 3 4 5 6 7 8 9 10 0 2 4 6 8 10 12 14 Time [s] Torque [N mm] τ = τ0 + τg τ = τ0 + g(q)
Figure 2: Total applied torques with g(q) only [dashed, blue] and with τg in (15)
states of the systems with and without gravity were fully matched at t = 0, with no initial joint deformation (see Fig. 1(b)). The total torques (i.e., including τ0) for
the link-based gravity compensation and for its perfect cancellation are reported in Fig. 2, showing that the dynamic torque contribution is indeed non-negligible.
3
A New PD-type Regulator for Robots with Elastic Joints
Consider again the gravity-loaded system (1–2) in the absence of dissipative terms1, i.e.,
M (q)¨q + S(q, ˙q) ˙q + g(q) + K(q − θ) = 0 (16)
B ¨θ + K(θ − q) = τ , (17)
where any factorization c(q, ˙q) = S(q, ˙q) ˙q can be used for the Coriolis/centrifugal vector. We address the problem of asymptotic stabilization of a desired (closed-loop) equilibrium state
q = qd, θ = θd:= qd+ K −1
g(qd), ˙q = ˙θ = 0. (18) The desired motor position θd is obtained from the static analysis (i.e., setting
˙q = ¨q = 0) of equation (16) at the desired link position qd.
Taking advantage of the dynamic gravity cancellation law, we present a new regulation controller realizing the task. The complete control law is defined as
τ = τg+ τ0, (19)
where τgis given by (13) (having set Dθ = O)
τg = g(q) + BK−1g(q),¨ (20)
and τ0is chosen as the PD-type control law
τ0= KP(qd−θ+K−1g(q))−KD( ˙θ−K−1˙g(q)). (21)
The following result holds.
Theorem 1 The desired state (18) for system (16–17) with control law (19–21) is the unique equilibrium state of the closed-loop system. Moreover, if
KP > 0, KD > 0,
the desired state is globally asymptotically stable.
1
Neglecting dissipative terms (e.g., viscous friction on the motor and link sides) is the worst situation from the point of view of stability of the robotic system. Their inclusion would make the analysis simpler.
ProofThe proof is based on Lyapunov analysis and LaSalle theorem. First, we show that there is a unique equilibrium state for the closed-loop system, i.e., a unique equilibrium configuration (qe, θe) with zero velocities ˙q and ˙θ. By
set-ting ¨q = ¨θ = 0 in the closed-loop equations given by (16–17) and (19–21), any equilibrium configuration should satisfy
g(qe) + K(qe− θe) = 0 K(qe− θe) + g(qe) + KP(qd− θe+ K−1g(qe)) = 0.
Subtracting the two equations leads to
θe= qd+ K−1g(qe),
while the first equation yields
θe= qe+ K −1
g(qe). By comparison, it follows that the the unique equilibrium is
qe= qd, θe= qd+ K−1g(qd) = θd.
Let a Lyapunov candidate be defined by the following quadratic function: V = 12 ˙qTM (q) ˙q + ˙θ − K−1˙g(q)TB ˙θ − K−1˙g(q) + q − θ + K−1g(q)T K q − θ + K−1g(q) + qd− θ + K−1g(q)TKP qd− θ + K−1g(q) .
As the sum of positive definite quadratic terms, V is positive definite. Moreover, V = 0 if and only if ˙q = 0, ˙θ − K−1∂g(q) ∂q ˙q = 0 ⇒ ˙θ = 0 and q − θ + K−1g(q) = 0 qd− θ + K−1g(q) = 0 ) ⇒ ( q = qd θ = qd+ K−1g(qd).
Therefore, the desired state is the unique minimum of V . Dropping for compact-ness dependencies, the time derivative of V is
˙ V = ˙qTM ¨q +12˙qTM ˙q +˙ ˙θ − K−1˙gTB ¨θ − K−1g¨ + q − θ + K−1gTK ˙q − ˙θ + K−1˙g − qd− θ + K−1gTKP ˙θ − K−1˙g .
The closed-loop equations (16–17) with (19–21) can be conveniently rewritten in the form M ¨q = K(θ − q) − S ˙q − g B ¨θ − K−1g¨= K(q − θ) + g + KP qd− θ + K−1g − KD ˙θ − K−1˙g . Substituting these into the expression of ˙V and simplifying terms yields
˙ V = ˙qT K(θ − q) + 12M − 2S˙ ˙q − g + ˙θ − K−1˙gT K(q − θ) + g + KP qd− θ + K−1g − KD ˙θ − K−1˙g + (K(q − θ) + g)T˙q − ˙θ + K−1˙g − qd− θ + K−1gT KP ˙θ − K−1˙g = − ˙θ − K−1˙gT KD ˙θ − K−1˙g ≤ 0, where the general relation ˙qT
˙ M − 2S
˙q = 0 has been used. Thus, it is ˙
V = 0 ⇔ ˙g(q) − K ˙θ = 0.
We proceed by using LaSalle arguments. The desired state satisfies indeed ˙V = 0, and thus V (t) ≡ 0. We should verify whether there are other system trajectories that are invariant with respect to the set of states where ˙V = 0. When ˙V = 0, note first that
d
dt(g(q) − Kθ) = 0 ⇒ g(q) − Kθ = k1,
where k1is a constant vector. Moreover, the model equation (17) with τ as in (19–
21) becomes
B ¨θ + K(θ − q) = g(q) + BK−1¨g(q) + KP(qd− θ + K−1g(q)),
or, by simple manipulation,
BK−1 d dt K ˙θ − ˙g(q)= I + KPK−1 h g(q) − Kθi+ Kq + KPqd.
For a closed-loop system trajectory remaining in the set of states such that ˙V = 0, the left-hand side of this equation must be zero. Since the term in square brackets on the right-hand side is constant, it follows that
Kq + KPqd= k2,
and hence q is constant by itself. As a consequence, θ is also a constant, and thus ˙q = ˙θ = 0. Therefore, the only invariant trajectory of the closed-loop system
that is compatible with ˙V = 0 is an equilibrium state. Since q = qd, θ = θd,
with ˙q = ˙θ = 0, is the unique equilibrium, then the desired state is globally asymptotically stable thanks to LaSalle theorem. This completes the proof2.
A series of remarks are in order:
• The expression of the control law (21) is logically derived from a pure PD scheme on the motor variables θ0and ˙θ0of the gravity-free system,
τ0 = KP(θd0− θ0) − KD˙θ0
= KP(qd− θ + K−1g(q))−KD( ˙θ − K−1˙g(q)),
using the relations (10–11) between motor variables of the gravity-free sys-tem and of the gravity-loaded syssys-tem under the action of dynamic gravity cancellation, and noting that the motor reference for the PD is θd0 = qd
since gravitational effects are canceled by τg. Another way of
interpret-ing terms in the control law (19–21) is to note that the motor reference θd = qd+ K−1g(qd) in the PD law with constant gravity compensation
of [7] is replaced by its on-line version qd+ K−1g(q) .
• The PD term in the control law, i.e., eq. (21), needs feedback from the full state of the robot, just as the term τg. This is the same requisite of exact
linearization laws by static state feedback. Conversely, energy-based Lya-punov designs for elastic joint robots use only motor feedback. The proposed controller realizes thus a compromise, eliminating the gravity-dependent dy-namic terms by means of a full state feedback but avoiding the need of com-plete cancellation of the dynamics of the elastic joint robot.
• Using gravity cancellation, there is no need of a positive lower bound on the joint stiffness K, as opposed to the previous literature [7, 8, 9]. While in practice joint stiffness always dominates the gradient of gravity torques in industrial robots with elastic joints (e.g., using harmonic drives), this re-laxation can be of interest for actuation systems with variable stiffness (see Sect. 5), where very low values of stiffness may be desirable to limit injuries due to accidental collisions between robot and humans.
• Looking at the proof of Theorem 1, it is easy to see that the desired state would still be the unique equilibrium for the closed-loop system when reduc-ing the gravity-term in the controller to τg = g(q). However, a
Lyapunov-based proof of global asymptotic stability with such a term added to a PD controller is not available.
2The introduced constants are k
0 1 2 3 4 5 6 7 8 9 10 0 0.5 1 1.5 2
Without Gravity vs Dynamic Gravity Cancellation (with PD)
link position (rad)
0 1 2 3 4 5 6 7 8 9 10 −1 0 1 2 time (s)
link velocity (rad/sec)
(a) 0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5
Without Gravity vs Dynamic Gravity Cancellation (with PD)
motor position (rad)
0 1 2 3 4 5 6 7 8 9 10 −2 0 2 4 6 time (s)
motor velocity (rad/s)
(b) 0 1 2 3 4 5 6 7 8 9 10 −300 −200 −100 0 100 200 300 400 500
Without Gravity vs Dynamic Gravity Cancellation (with PD)
time (s)
control torque (Nm)
(c)
Figure 3: Comparison of link variables (a), motor variables (b), and control torques (c) for a single link with elastic joint without gravity under PD control on the motor side [dashed, blue], and with gravity under the PD-type control law with dynamic cancellation (19–21) [continuous, red]; the joint stiffness is K = 100
0 1 2 3 4 5 6 7 8 9 10 0 0.5 1 1.5 2
Without Gravity vs Dynamic Gravity Cancellation (with PD)
link position (rad)
0 1 2 3 4 5 6 7 8 9 10 −0.5 0 0.5 1 1.5 time (s)
link velocity (rad/sec)
(a) 0 1 2 3 4 5 6 7 8 9 10 0 0.5 1 1.5 2 2.5
Without Gravity vs Dynamic Gravity Cancellation (with PD)
motor position (rad)
0 1 2 3 4 5 6 7 8 9 10 −0.5 0 0.5 1 1.5 2 time (s)
motor velocity (rad/s)
(b) 0 1 2 3 4 5 6 7 8 9 10 −50 0 50 100 150 200 250 300
Without Gravity vs Dynamic Gravity Cancellation (with PD)
time (s)
control torque (Nm)
(c)
Figure 4: Comparison of link variables (a), motor variables (b), and control torques (c) for a single link with elastic joint without gravity under PD control on the motor side [dashed, blue], and with gravity under the PD-type control law with dynamic cancellation (19–21) [continuous, red]; the joint stiffness is K = 500
3.1 Simulation results
We present some simulation results that show the typical behavior obtained un-der the action of the PD-type control law (21) with the gravity cancellation (20). A single link with elastic joint is considered, using the same numerical data of Sect. 2.1. The link is commanded to move from the downward equilibrium q = 0 to qd = π/2. The PD (scalar) gains were chosen as kP = 100 and kD = 80.
These values were conveniently tuned in the absence of gravity. Figure 3 shows the comparative evolution of the relevant variables in the two situations of no grav-ity and presence of gravgrav-ity with gravgrav-ity cancellation. The link motion in Fig. 3(a) is exactly the same, as expected. The motor position has a different evolution in the two cases (Fig. 3(b)), due to the need to charge the elastic joint for dynami-cally balancing gravity on the link. The total applied motor torque is also different (Fig. 3(c)): in fact, it should vanish at steady state when gravity is absent, whereas it should at least provide the static gravity torque at final destination in the other case. On the other hand, the PD component of the new control law is exactly the same as the PD action in the absence of gravity (plots not shown).
The new control law is able to regulate the link to the desired position even if the joint stiffness is here very small, in particular lower than the maximum gradient of the gravity term (K = 100 < 245.25 = mdg0). Note that the small oscillations
experienced by the link while approaching the goal are due to the poor transient performance achievable by motor PD feedback in the absence of gravity, and not to the nature of the dynamic gravity cancellation law: the only role of τgis to allow
the exact reproduction of the link behavior in the absence of gravity, no matter how good or bad this is3. The very low value of joint stiffness is partly responsible for this behavior. In fact, Figure 4 shows the results under the same PD gains when increasing the joint stiffness to K = 500. The improvement in the transient behavior is quite apparent.
4
Joints with Nonlinear Flexibility
In this section, we extend the analysis of Sect. 2 to the case of transmissions with nonlinear flexibility [23]. For the sake of simplicity, only a single dof will be con-sidered, but the generalization to multi-dof systems is straightforward. Using the same notation of Sec. 2, we assume that a potential energy Ue(φ) ≥ 0 is associated
to the deformation φ = q − θ, so that the flexibility torque τe = ∂Ue/∂q = τe(φ)
is a nonlinear function of φ and the stiffness σ = ∂τe/∂q = σ(φ) will be
non-constant.
The dynamic model of a single link moving under gravity and driven through
3We have also used the Simulink Design Optimization tool of Matlab for tuning the PD gains in the absence of gravity, leading to the choice kP = 41.3291, kD = 87.1717. While the control peaking in the first couple of seconds is eliminated, the transient response is somewhat slower and the link oscillations are practically left the same.
such a flexible transmission is then
M ¨q + Dqq + g(q) + τ˙ e(φ) = 0 (22)
B ¨θ + Dθθ − τ˙ e(φ) = τ. (23)
We wish to define a feedback law τ = τ (q, θ, ˙q, ˙θ, τ0) in (23) so as to match the
behavior of some output variable of the model without gravity
M ¨q0+ Dqq˙0+ τe(φ0) = 0 (24)
B ¨θ0+ Dθθ˙0− τe(φ0) = τ0. (25)
It is easy to verify that both nonlinear systems (22–23) and (24–25) are exactly linearizable by means of a static state feedback into a chain of four integrators, with q and its first three time derivatives as linearizing coordinates. Therefore, the two systems are feedback equivalent and the solution to our problem is obtained by imposing q(t) = q0(t) for all t ≥ 0. In particular, from q[4] = q0[4]we get
τ = g(q) + Dθ σ(φ) ˙g(q) + B σ(φ)¨g(q) +σ(φ) − σ(φ0) σ(φ) (B + M )¨q + (Dq+ Dθ) ˙q + B σ(φ) ∂σ(φ) ∂φ ˙ φ2−∂σ(φ0) ∂φ0 ˙ φ20 +σ(φ0) σ(φ) τ0 = τg+ αgτ0, (26)
where ¨q (to be used also in ¨g(q)) is computed from (22) as ¨
q = − 1
M (Dqq + g(q) + τ˙ e(φ)) . In addition, the initial matching requires
q(0) = q0(0) q(0) = ¨¨ q0(0)
˙
q(0) = ˙q0(0) q[3](0) = q[3]0 (0).
(27) Note that (26) collapses into (12–13) for a transmission with constant stiffness σ = K. However, differently from the case of linear elasticity, the control law (26) contains terms that require the knowledge of the deformation φ0 = q − θ0, and of
its rate ˙φ0, pertaining to the model without gravity. Also, the torque τ0applied in
the gravity-free case needs now to be scaled by the factor αg= σ(φ0)/σ(φ).
The value φ0is computed by solving the nonlinear equation τe(φ0) = −M ¨q −
Dqq, which is obtained from (24) by taking into account the first three identities˙
in (27). Using (22), the right-hand side can be written as a function of the state (actually, of the configuration variables only) of the gravity-loaded system as
Equation (28) needs to be solved for φ0 at each time t ≥ 0 as a function of the
current system state. As a representative example, consider a flexible joint trans-mission with associated potential given by Ue = 12Kφ2+ 14Kcφ4, with K > 0
and Kc> 0. The flexibility torque is a cubic function of φ and the stiffness has a
quadraticdependence:
τe(φ) = Kφ + Kcφ3, σ(φ) = K + 3Kcφ2. (29)
At a given (q, θ), equation (28) results in the cubic equation Kcφ30 + Kφ0 −
a(q, θ) = 0, which has always two complex roots and one real (positive or nega-tive) root, thanks to the positivity of K and Kc. The real root is given by
φ0= 3 s 1 2 a(q, θ) Kc + b(q, θ) + 3 s 1 2 a(q, θ) Kc − b(q, θ), where b(q, θ) = s 1 27 K Kc 3 +14 a(q,θ) Kc 2
> 0. For more general stiffness pro-files, a solution to (28) should be searched numerically.
Once φ0has been found, the value of ˙φ0that appears in the control law (26) is
obtained by time differentiation of (28) (or, equivalently, from the fourth identity in (27)) as ˙ φ0 = 1 σ(φ0) σ(φ) ˙φ + ∂g(q) ∂q q˙ . (30)
As a result, the gravity cancellation control law (26) can be computed in closed form from full state measurements in the case of cubic stiffness (and for other sim-ple nonlinear dependencies). Note that for multi-dof robots with nonlinear flexible joints one needs to solve n similar equations of the form (28), whereas (30) is replicated component-wise.
4.1 Simulation results
We simulated a joint with cubic flexibility torque τe(φ) having K = 100 [N·mm/rad]
and Kc = 500 [N·mm/rad]. With these data, the joint stiffness σ(φ) increases by
about 45% w.r.t. its value at φ = 0 when the joint deformation is |φ| = 0.18 [rad]. All other model parameters, the initial conditions, and the open-loop input torque are the same as in Sect. 2.1. Figure 5 shows the evolution of the link (a) and motor (b) angles obtained in the absence or in the presence of gravity under the dynamic gravity cancellation law (26).
5
Variable Stiffness Joints with Antagonistic Actuation
Progressing in the generalization of our dynamic gravity cancellation approach, we consider in this section the case of joints with (actuated) variable stiffness. Use of variable nonlinear stiffness actuation is highly recommended for safe pHRI, with
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 Time [s]
Link position [rad]
q q0 (a) 0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 Time [s]
Motor position [rad]
θ θ
0
(b)
Figure 5: Comparison of link (a) and motor (b) position for a single nonlinear flexible joint without gravity under τ0 [dot-dashed, black], and with gravity under
the dynamic cancellation law (26) [continuous, red]
the antagonistic arrangement of two motors for each joint as the most common realization [13, 15, 16].
For the sake of notational simplicity, consider a single link under gravity driven by an antagonistic VSA system. The dynamic model is expressed in terms of three generalized coordinates, q for the link position, and θ1 and θ2 for the position of
the two motors. Let φi= q − θi, i = 1, 2, be the deformations of the transmissions
at the two sides of the joint. We have
M ¨q + Dqq + g(q) + τ˙ e(φ1) + τe(φ2) = 0 (31)
B ¨θ1+ Dθθ˙1− τe(φ1) = τ1 (32)
B ¨θ2+ Dθθ˙2− τe(φ2) = τ2, (33)
where τ1and τ2are the torques supplied by the two motors. Without loss of
gener-ality, we have assumed in (31–33) a full symmetry for the two actuation/transmission systems. Accordingly, the total stiffness σtof the device is given by the separable
function
σt(φ1, φ2) =
∂(τe(φ1) + τe(φ2))
∂q = σ(φ1) + σ(φ2). (34)
As before, the target behavior is specified by a dynamic system of the same form (31– 33), but with g(q) ≡ 0 and all its variables labeled by a 0 subscript.
Since the system has two inputs, according to the feedback equivalence prin-ciple, we should determine two independent system output functions that play the role of linearizing coordinates in a feedback linearization scheme. Based on the results in [21], these two outputs are the link position q and the total stiffness σt.
In fact, differentiating once (31) w.r.t. time gives
M q[3]+ Dqq + ˙g(q) + σ(φ¨ 1) ˙φ1+ σ(φ2) ˙φ2 = 0. (35)
DIfferentiating once more, using (32–33), and rearraging terms, we obtain M q[4]+ Dqq[3]+ ¨g(q) + ∂σ(φ1) ∂φ1 ˙ φ21+∂σ(φ2) ∂φ2 ˙ φ22+ σtq¨ = σ(φ1)¨θ1+ σ(φ2)¨θ2 = 1 B σ(φ1) σ(φ2) τ1+ τe(φ1) − Dθθ˙1 τ2+ τe(φ2) − Dθθ˙2 . (36)
Similarly, by differentiating (34) w.r.t. time, we have ˙σt= ∂σ(φ1) ∂φ1 ˙ φ1+ ∂σ(φ2) ∂φ2 ˙ φ2 (37)
and, by rearraging terms and using again (32–33), ¨ σt= ∂2σ(φ 1) ∂φ21 ˙ φ21+ ∂ 2σ(φ 2) ∂φ22 ˙ φ22+ ∂σ(φ1) ∂φ1 +∂σ(φ2) ∂φ2 ¨ q =∂σ(φ1) ∂φ1 ¨ θ1+ ∂σ(φ2) ∂φ2 ¨ θ2 = 1 B ∂σ(φ1) ∂φ1 ∂σ(φ2) ∂φ2 τ 1+ τe(φ1) − Dθθ˙1 τ2+ τe(φ2) − Dθθ˙2 ! . (38)
It can be shown that the decoupling matrix associated to the output vector (q, σt)
is proportional to the matrix
A(φ1, φ2) = σ(φ1) σ(φ2) ∂σ(φ1) ∂φ1 ∂σ(φ2) ∂φ2 ,
which is generically nonsingular, except when θ1 = θ2 (a condition that can
outputs q, together with its first three derivatives, and σt, with its first derivative,
are linearizing coordinates for system (31–33).
Comparing the expressions (36) and (38) with those of the gravity-free case (with a 0 subscript), the solution to the problem of dynamic gravity cancellation is given by the control torques τ1and τ2
τ1 τ2 = Dθθ˙1− τe(φ1) Dθθ˙2− τe(φ2) + A−1(φ1, φ2) · ( A(φ10, φ20) τ10 τ20 + τe(φ10) − Dθθ˙10 τe(φ20) − Dθθ˙20 + B ¨ g(q) + 2 X i=1 ∂σ(φi) ∂φi ˙ φ2i −∂σ(φi0) ∂φi0 ˙ φ2i0 2 X i=1 ∂σ(φi) ∂φi −∂σ(φi0) ∂φi0 ¨ q + 2 X i=1 ∂2σ(φ i) ∂φ2i ˙ φ2i −∂ 2σ(φ i0) ∂φ2i0 ˙ φ2i0 ) , (39)
where the link acceleration ¨q (to be used also in ¨g(q)) is computed from (31) as ¨
q = − 1
M (Dqq + g(q) + τ˙ e(φ1) + τe(φ2)) . In addition, an initial state matching given by
q(0) = q0(0) q(0) = ˙˙ q0(0) q(0) = ¨¨ q0(0) q[3](0) = q0[3](0)
and
σt(0) = σt(φ1(0), φ2(0)) = σt(φ10(0), φ20(0)) = σt0(0)
˙σt(0) = ˙σt0(0)
should hold between the gravity-loaded and the gravity-free system. Note that the above identities will hold for all t ≥ 0 thanks to the chosen control law.
The control law (39) physically replaces all terms that are affected by gravity (motor variables, flexible deformation torques, partial derivatives of the stiffness functions) with those of the gravity-free target system. For the considered single-dof VSA-based joint, the dynamic gravity cancellation law is very similar to a feedback linearization law from the point of view of complexity. However, these two controllers will differ consistently when considering multi-dof VSA robotic systems —in much the same way as for the case of single actuation of linear or nonlinear flexible joints.
Beside measurements of the state of the gravity-loaded system, in order to evaluate (39) we need also knowledge of the deformations φi0, i = 1, 2, and of
that from φi0, we directly obtain also θi0 = q − φi0. Similarly to Sect. 4, the
deformations φ10 and φ20 are determined by solving the coupled system of two
nonlinear equations
τe(φ10) + τe(φ20) = −M ¨q − Dqq = a˙ 1(q, θ1, θ2)
σ(φ10) + σ(φ20) = σt(q, θ1, θ2),
(40) where the right-hand sides of (40) are expressed in terms of current state measure-ments using (31) and (34). Due to the symmetry, if (φa, φb) is a solution of (40)
then (φb, φa) is a solution as well.
In general, system (40) needs to be solved numerically. Some additional insight is provided in the case of cubic flexibility torques, see (29). We have then
K(φ10+ φ20) + Kc(φ310+ φ320) = a1(q, θ1, θ2) (41) 2K + 3Kc(φ210+ φ220) = σt(q, θ1, θ2). (42) Since by definition σt− 2K 3Kc := R2 ≥ 0,
the solutions to equation (42) can be parametrized by a scalar α ∈ [0, 2π) as φ10=
R cos α and φ20= R sin α. Replacing these in (41) yields the single trigonometric
equation in α (cos α + sin α) +σt− 2K 3K cos 3α + sin3α = a1 KR. (43) −2 −1.5 −1 −0.5 0 0.5 −2 −1.5 −1 −0.5 0 0.5 alpha [rad]
Figure 6: A typical functional form of eq. (43) and a possible solution Figure 6 shows a plot of one of the two branches of the expression on the left-hand side of (43), obtained for K = 100, Kc= 500, and σt= 220. The horizontal
line corresponds to the case a1 = 10, and the associated root α provides the
is sufficiently smooth, and thus easily solvable by a numerical root finder (e.g., the fzero routine of Matlab). Assume now that the device stiffness σt can be
changed within the interval (2K, 4K), i.e., from its minimum physical value to a 100% increase. It can be shown that a pair of α solutions to (43) always exist in this interval for σt, provided that |a1| <
√
2KR [1 + 0.5(σt− 2K)/(3K)].
It should be stressed that the existence of pairs of solutions is not a source of problems. In fact, system (40) will be solved at every (discretized) instant t ≥ 0. Once a specific solution has been chosen at t = 0, the process is repeated on line and a local numerical search around the previous solution generates a single update.
FInally, having determined φ10and φ20, their rates are obtained by time
differ-entiation of (40) as ˙ φ10 ˙ φ20 = A−1(φ10, φ20) −M q[3]− D qq¨ ˙σt = A−1(φ10, φ20) σ(φ1) ˙φ1+ σ(φ2) ˙φ2+ ∂g(q)∂q q˙ ∂σ(φ1) ∂φ1 ˙ φ1+∂σ(φ∂φ22)φ˙2 ,
where (35) and (37) have been used to express all quantities in terms of the original VSA system state only.
5.1 Simulation results
We have simulated the dynamic gravity cancellation law (39) for a symmetric an-tagonistic joint with cubic flexibility torques, using the numerical data of Sec. 4.1 duplicated as needed. In the present case, the input torques τ10and τ20have been
chosen of the bang-bang type as in Fig. 7(c). Figure 7 shows the validity of the pro-posed scheme: both the link position (a) and the device stiffness (b) have identical evolutions in the absence of gravity and when gravity is present but dynamically canceled. Note that the stiffness variation during motion is as large as 2.5 [Nm/rad]. The total applied torques are shown in Fig. 8.
5.2 The VSA-II Variable Stiffness Joint
We report also a numerical result on gravity cancellation for the VSA-II experi-mental device developed at the University of Pisa [16] and sketched in Fig. 9. The VSA-II is based on a bi-directional antagonistic arrangement of two motors driv-ing a sdriv-ingle joint through a nonlinear flexible transmission system that uses pairs of 4-bar mechanisms (the so-called Grashof neutral linkage) with linear springs.
The VSA-II dynamic model takes the form (31–33), with τe(φi) = 2K β(φi)
∂β(φi)
∂φi
0 1 2 3 4 5 6 7 8 9 10 0 10 20 30 40 50 60 70 Time [s]
Link position [rad]
q q0 (a) 0 1 2 3 4 5 6 7 8 9 10 500 1000 1500 2000 2500 3000 3500 Time [s] Stiffness [N mm /rad] σ (Φ ) σ (Φ0) (b) 0 1 2 3 4 5 6 7 8 9 10 −400 −300 −200 −100 0 100 200 300 400 Time [s] Input torque [N mm] τ 1 0 τ 2 0 (c)
Figure 7: Comparison of link position (a) and device stiffness (b) for a vari-able stiffness antagonistic joint with cubic flexibility torques without gravity [dot-dashed, black], and with gravity under the dynamic cancellation law (39) [contin-uous, red] when the bang-bang torque inputs τ10and τ10(c) are applied
0 1 2 3 4 5 6 7 8 9 10 −600 −400 −200 0 200 400 600 Time [s] Torque [N mm] τ 1 τ 2
Figure 8: Total applied torques (39) for the link motion and device stiffness evolu-tion of Fig. 7
0 1 2 3 4 5 6 7 8 9 10 0 5 10 15 Time [s]
Link position [rad]
q q0 (a) 0 1 2 3 4 5 6 7 8 9 10 500 1000 1500 2000 2500 3000 3500 4000 4500 Time [s] Stiffness [N mm / red] σ (Φ ) σ (Φ0) (b)
Figure 10: Comparison of link position (a) and device stiffness (b) for the VSA-II without gravity [dot-dashed, black], and with gravity under the dynamic cancella-tion law [continuous, red] when the bang-bang torque inputs τ10and τ10of Fig. 7(c)
0 1 2 3 4 5 6 7 8 9 10 −600 −500 −400 −300 −200 −100 0 100 200 300 400 Time [s] Torque [N mm] τ 1 τ 2
Figure 11: Total applied torques for the link motion and device stiffness evolution of Fig. 10
where K is the constant stiffness of each of the two torsional elastic springs and β(φi) = arcsin C sin φi 2 −φi 2, i = 1, 2,
being C > 1 a geometric parameter of the 4-bar mechanisms. Due to this arrange-ment, the dynamic gravity cancellation law for the VSA-II is a particular instance of eq. (39).
Figure 10 shows the obtained evolution when using the same open-loop torque input of Fig. 7(c) and the numerical data from [16, 21]. The total applied torques are reported in Fig. 11.
6
Antagonistic Actuation with Constant but Different
Stiff-ness
As a particular case of the previous section, and still focusing on a single dof for simplicity, we consider here the antagonistic arrangement of two motors at the joint with transmissions having constant stiffness. However, we relax the symmetry assumption made in (31–33) and allow for possibly different stiffness of the two transmissions, as well as for different motor inertias and viscous coefficients on each motor side. Therefore, we have the elastic torques
τei(φi) = Kiφi = Ki(q − θi), i = 1, 2,
and constant (but typically different) stiffnesses σi =
∂τei(φi)
The dynamic model is
M ¨q + Dqq + g(q) + K˙ 1(q − θ1) + K2(q − θ2) = 0 (44)
B1θ¨1+ Dθ1θ˙1+ K1(θ1− q) = τ1 (45)
B2θ¨2+ Dθ2θ˙2+ K2(θ2− q) = τ2. (46)
The control goal is again to design a dynamic gravity cancellation feedback law so that the behavior of selected outputs of the gravity-loaded system (44– 46) matches that associated to the gravity-free model. However, the procedure in Sect. 5 cannot be used as such here. In fact, the total device stiffness is now constant,
σt= K1+ K2, (47)
and cannot be used as a linearizing coordinate (and, of course, it cannot be changed either). Therefore, the two motors provide actuation redundancy in this case. This redundancy will be used to define how the dynamic gravity load should be shared between the two motors and transmissions. The solution can be derived again within a feedback equivalence approach.
Before proceeding, the static conditions are analyzed with some detail. At any forced equilibrium configuration (¯q, ¯θ1, ¯θ2), with ˙q = ¨q = 0 and ˙θi = ¨θi = 0,
i = 1, 2, the following relations hold:
g(¯q) + K1(¯q − ¯θ1) + K2(¯q − ¯θ2) = 0
K1(¯θ1− ¯q) = ¯τ1
K2(¯θ2− ¯q) = ¯τ2.
Adding them up, it follows that ¯τ1+ ¯τ2 = g(¯q). To resolve this linear constraint
in terms of the single static motor torques ¯τ1and ¯τ2, one can minimize a weighted
quadratic function H = 1 2 w1τ¯ 2 1 + w2τ¯22 , wi> 0, i = 1, 2. The solution is ¯ τ1= w2 w1+ w2 g(¯q), τ¯2= w1 w1+ w2 g(¯q). (48)
While the positive weights w1 and w2 can be chosen in principle in an arbitrary
way, a relevant case is obtained by selecting the transmission compliances as weights, since the system will then satisfy an intrinsic physical property.
In fact, for any value ¯q and any set of static torques satisfying ¯τ1+ ¯τ2 = g(¯q),
the natural equilibrium of the motor positions ¯θ1and ¯θ2is found by minimizing the
total elastic potential:
min Ue= 12 K1(θ1− ¯q)2+ K2(θ2− ¯q)2
The unique solution is given by ¯θi = ¯q + g(¯q)/(K1+ K2), i = 1, 2, and provides ¯ τ1 = K1 K1+ K2 g(¯q), τ¯2= K2 K1+ K2 g(¯q). (49)
The two equations in (49) are in the form (48), with weights chosen as the com-pliances w1 = 1/K1and w2 = 1/K2. Therefore, the motor with the stiffer
trans-mission will carry the larger part of the static gravity load. In the limit, when, e.g., K1→ ∞, we will have ¯τ1,∞= g(¯q), ¯τ2,∞= 0.
Turning to dynamic conditions, we define next an auxiliary output function y that can be used as part of the linearizing coordinate transformation. Motivated by the fact that the total flexibility torque τe1+ τe2 is a linear function of θ1 and θ2,
we choose
y = α1θ1+ α2θ2, (50)
with arbitrary constants αi, i = 1, 2. Differentiating (44) and (50) twice, and using
eqs. (45–46) and (47), yields
M q[4]+ Dqq[3]+ ¨g(q) + σtq = K¨ 1θ¨1+ K2θ¨2 = K1 B1 K2 B2 τ1+ K1(q − θ1) − Dθ1θ˙1 τ2+ K2(q − θ2) − Dθ2θ˙2 (51) and ¨ y = α1θ¨1+ α2θ¨2 = α1 B1 α2 B2 τ 1+ K1(q − θ1) − Dθ1θ˙1 τ2+ K2(q − θ2) − Dθ2θ˙2 . (52)
The decoupling matrix associated to (M q, y),
A = K1 K2 α1 α2 ! 1 B1 0 0 B1 2 ! , (53)
is now constant, and nonsingular iff K1α2− K2α1 6= 0. In particular, this is
cer-tainly true for α1α2 < 0, as well as when only one of the αi’s is zero. Indeed,
the first block matrix in A appears also in the change of state coordinates between (q, θ1, θ2, ˙q, ˙θ1, ˙θ2) and (q, ˙q, ¨q, q[3], y, ˙y) and its nonsingularity makes this a
glob-ally invertible map.
At this stage, we can repeat the same calculations for the gravity-free system (where variables have a 0 subscript) and impose q(t) ≡ q0(t) and y(t) ≡ y0(t) for
all t ≥ 0. Following the same line of thoughts of the previous sections, comparison of expressions (51) and (52) of the loaded system with those of the gravity-free case yields, after some manipulation, the dynamic gravity cancellation law
τ1 τ2 = τ10 τ20 + τ1g τ2g (54)
where τ1g = α2 K1α2− K2α1 K1g(q) + Dθ1˙g(q) + B1¨g(q) τ2g = −α1 K1α2− K2α1 K2g(q) + Dθ2˙g(q) + B2¨g(q) (55)
and τi0, i = 1, 2 are the control commands designed in the absence of gravity.
Note that (54–55) can be seen as a simpler (and more explicit) form of eq. (39), obtained by replacing σt(φ1, φ2) with y and taking into account the fact that the
device stiffness and the A matrix are constant. The above closed-form control expression is obtained without the need for a numerical search, similarly to the case of constant elastic joints with single actuation considered in Sect. 2.
The control expressions (55) contain the arbitrary design parameters α1 and
α2. As such, they can be view as the general solution to the problem of sharing
dynamic gravity cancellation between the two motors of the antagonistic joint. In particular, wishing to use only one motor for gravity cancellation, e.g., the first one, we set α1= 0 (with any α2 6= 0) obtaining τ2g = 0 and
τ1g = g(q) + Dθ1 K1 ˙g(q, ˙q) + B1 K1 ¨ g(q).
This is exactly the scalar version of eq. (13) in Sect. 2. On the other hand, by setting α1 = −1 and α2 = 1 we obtain a dynamic sharing of gravity that, in static
conditions, collapses into the associated natural optimum (49). Note also that when K1= K2, the only forbidden choice is α1= α2. Furthermore, it is easy to realize
that the output structure (50) could be generalized to y = α1(q, ˙q)θ1+ α2(q, ˙q)θ1
without any major changes in the derivations. Provided that invertibility of the de-coupling matrix A is enforced (this matrix would have the same structure of (53), but depending then on (q, ˙q)), such output structure may allow for a state-dependent, and possibly optimal, dynamic sharing.
Note finally that, if m motors were connected in parallel to the link through m transmissions of constant (and different) stiffnesses and we were using a combina-tion of motors to cancel gravity from the link mocombina-tion, the solucombina-tion would take the form τig = αi Kiαi− m P j=1 j6=i Kjαj Kig(q) + Dθi˙g(q) + Big(q),¨ (56)
with αi being the discounting factor of the i-th motor, for i = 1, . . . , m.
All developments in this section can be replicated in a straightforward way for the case of n-dof robots with antagonistic actuation at all joints, each with two (different or equal) constant stiffnesses.
6.1 Simulation results
We tested the control law (54–55) for a single link with an antagonistic elas-tic joint having the two transmissions, respectively, of stiffness K1 = 100 and
K2 = 50 [N·mm/rad], and thus with a constant σt = 150 [N·mm/rad]. The other
numerical data are the same of Sect. 2.1, duplicated as needed. Two equal open-loop sinusoidal torques τ10 = τ20 = sin 2πt were applied for T = 10 s, see
Fig. 13(c). Choosing α1 = α2 = 0.5, the results of Fig. 12 indicate full
agree-ment with the theory. The total torques applied by the two motors are shown in Fig. 13(a),(b). For comparison, we report also the motor evolutions for two other choices of the output constants in the same previous operative conditions. Fig-ure 14 refers to the case α1 = 0 and α2 = 2, where the first motor is fully
respon-sible for canceling the gravity torque. Figure 15 refers to the choice α1 = −1 and
α2 = 1. In this case, the smallest global deviations from the unloaded case are
obtained, indicating a potential condition of optimal dynamic sharing of gravity. Indeed, in all previous situations the link motion replicates the no-gravity behav-ior.
7
Conclusions
We have considered the problem of perfect cancellation of dynamic gravity effects acting on the link motion of robot manipulators having flexible transmissions. The cases of flexible transmissions having constant or nonlinear stiffness characteristics with single actuation at each joint, and of variable nonlinear or constant stiffness with (double) antagonistic actuation have been analyzed. Based on the feedback equivalence principle, nonlinear control laws have been designed that allow the outputs of the gravity-loaded to behave as those of a reference model where grav-ity is absent. In the case of VSA-based manipulators, this includes the dynamic shaping of both the link motion and the evolution of the device stiffness. For dou-ble antagonistic actuation with constant stiffness, an actuation redundancy occurs and gravity load sharing among the motors can be optimized in static or dynamic terms.
Dynamic gravity cancellation involves in general the on-line computation of inertial terms, but the presented control laws are still much simpler than those needed for feedback linearization. The control laws solving the problem have been obtained either in closed algebraic form or by using simple numerical techniques. In particular, a parallel simulation of the gravity-free system to be matched is never required.
The presented results can be used for different control purposes. For set-point regulation tasks, a PD-type state feedback law has been designed on top of the gravity cancellation law in the case of robots with elastic joints. Global asymptotic stability has been shown using Lyapunov techniques, without the need of a strictly positive lower bound neither on the proportional gain nor on the joint stiffness. In a similar way, we foresee that enhanced regulation controllers could be obtained with
0 1 2 3 4 5 6 7 8 9 10 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Time [s]
Link position [rad]
q q0 (a) 0 1 2 3 4 5 6 7 8 9 10 −1.5 −1 −0.5 0 0.5 1 1.5 Time [s]
Motor position [rad]
θ 1 θ 1 0 (b) 0 1 2 3 4 5 6 7 8 9 10 −1.5 −1 −0.5 0 0.5 1 1.5 Time [s]
Motor position [rad]
θ 2 θ 2 0 (c)
Figure 12: Comparison of the position of the link (a) and of the two motors (b,c) for the antagonistic joint arrangement having different but constant stiffnesses with-out gravity [dot-dashed, black], and with gravity under the dynamic cancellation law (54–55) [continuous, red]; the output coefficients in (50) are α1 = α2= 0.5
0 1 2 3 4 5 6 7 8 9 10 −150 −100 −50 0 50 100 150 Time [s] Torque [Nmm] τ 1 (a) 0 1 2 3 4 5 6 7 8 9 10 −150 −100 −50 0 50 100 150 Time [s] Torque [Nmm] τ 2 (b) 0 1 2 3 4 5 6 7 8 9 10 −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 Time [s] Input torque [Nmm] (c)
Figure 13: Total applied torques by the first (a) and second (b) motor for the link motion of Fig. 12 when the sinusoidal open-loop torques τ10 = τ20 (c) are
com-manded
relative ease also for VSA-based manipulators, where the link position as well as the device stiffness need to be asymptotically stabilized to a desired constant value. The proposed dynamic gravity cancellation is also useful in safe physical
human-0 1 2 3 4 5 6 7 8 9 10 −1.5 −1 −0.5 0 0.5 1 Time [s]
Motor position [rad]
θ 1 θ 1 0 (a) 0 1 2 3 4 5 6 7 8 9 10 −1 −0.5 0 0.5 1 1.5 Time [s]
Motor position [rad]
θ 2 θ 2 0 (b)
Figure 14: Comparison of the first (a) and second (b) motor positions for the antag-onistic joint arrangement having different but constant stiffnesses without gravity [dot-dashed, black], and with gravity under the dynamic cancellation law (54–55) [continuous, red]; the output coefficients in (50) are α1 = 0 and α2 = 2
0 1 2 3 4 5 6 7 8 9 10 −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 Time [s]
Motor position [rad]
θ 1 θ 1 0 (a) 0 1 2 3 4 5 6 7 8 9 10 −1 −0.5 0 0.5 1 1.5 Time [s]
Motor position [rad]
θ 2 θ 2 0 (b)
Figure 15: Comparison of the first (a) and second (b) motor positions for the antag-onistic joint arrangement having different but constant stiffnesses without gravity [dot-dashed, black], and with gravity under the dynamic cancellation law (54–55) [continuous, red]; the output coefficients in (50) are α1 = −1 and α2 = 1
robot interaction. In general, unexpected collisions may occur at any time during motion and the compliant robot should react as soon as the impact is detected (e.g., with a sensorless residual-based method as in [2]). Through the permanent cancel-lation of the gravitational loads on the robot links, a physical torque-based reaction strategy can be designed so that the controlled robot rapidly flees away from the danger area in a gravity-unbiased dynamic fashion. This subject is currently under investigation.
Acknowledgments
This work has been funded by the MIUR project PRIN 2007 SICURA.
References
[1] J. Heinzmann and A. Zelinsky, “Quantitative safety guarantees for physical human-robot interaction,” Int. J. of Robotics Research, vol. 22, no. 7/8, pp. 479–504, 2003.
[2] A. De Luca, A. Albu-Sch¨affer, S. Haddadin, and G. Hirzinger, “Collision detection and safe reaction with the DLR-III lightweight robot arm,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2006, pp. 1623–1630. [3] A. De Santis, B. Siciliano, A. De Luca, and A. Bicchi, “An atlas of physical human-robot interaction,” Mechanism and Machine Theory, vol. 43, no. 3, pp. 253–270, 2008.
[4] G. Hirzinger, A. Albu-Sch¨affer, M. H¨ahnle, I. Schaefer, and N. Sporer, “On a new generation of torque controlled light-weight robots,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2001, pp. 3356–3363.
[5] A. Bicchi and G. Tonietti, “Fast and soft arm tactics: Dealing with the safety-performance trade-off in robot arms design and control,” IEEE Robotics and Automation Mag., vol. 11, no. 2, pp. 22–33, 2004.
[6] A. De Luca and W. Book, “Robots with flexible elements,” in Handbook of Robotics, B. Siciliano and O. Khatib, Eds., Springer, 2008, pp. 287–319. [7] P. Tomei, “A simple PD controller for robots with elastic joints,” IEEE Trans.
on Automatic Control, vol. 36, no. 10, pp. 1208–1213, 1991.
[8] A. De Luca, B. Siciliano, and L. Zollo, “PD control with on-line gravity compensation for robots with elastic joints: Theory and experiments,” Auto-matica, vol. 41, no. 10, pp. 1809–1819, 2005.
[9] A. Kugi, C. Ott, A. Albu-Sch¨affer, and G. Hirzinger, “On the passivity-based impedance control of flexible joint robots,” IEEE Trans. on Robotics, vol. 24, no. 2, pp. 416 –429, 2008.
[10] S. Haddadin, A. Albu-Sch¨affer, A. De Luca, and G. Hirzinger, “Collision detection and reaction: A contribution to safe physical human-robot interac-tion,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2008, pp. 3356–3363.
[11] M. W. Spong, “Modeling and control of elastic joint robots,” ASME J. of Dynamic Systems, Measurement, and Control, vol. 109, no. 4, pp. 310–319, 1987.
[12] A. De Luca and P. Lucibello, “A general algorithm for dynamic feedback lin-earization of robots with elastic joints,” in Proc. IEEE Int. Conf. on Robotics and Automation, 1998, pp. 504–510.
[13] A. Bicchi, S. L. Rizzini, and G. Tonietti, “Compliant design for intrinsic safety: General issue and preliminary design,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2001, pp. 1864–1869.
[14] M. Zinn, O. Khatib, B. Roth, and J. K. Salisbury, “A new actuation approach for human-friendly robot design,” Int. J. of Robotics Research, vol. 23, no. 4/5, pp. 379–398, 2005.
[15] S. A. Migliore, E. A. Brown, and S. P. DeWeerth, “Biologically inspired joint stiffness control,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2005, pp. 4508–4513.
[16] R. Schiavi, G. Grioli, S. Sen, and A. Bicchi, “VSA-II: A novel prototype of variable stiffness actuator for safe and performing robots interacting with humans,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2008, pp. 2171–2176.
[17] G. Boccadamo, R. Schiavi, S. Sen, G. Tonietti, and A. Bicchi, “Optimiza-tion and fail-safety analysis of antagonistic actua“Optimiza-tion for pHRI,” in Euro-pean Robotics Symp., Springer Tracts in Advanced Robotics, Springer Berlin, 2006, pp. 109–118.
[18] S. Wolf and G. Hirzinger, “A new variable stiffness design: Matching require-ments of the next robot generation,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2008, pp. 1741–1746.
[19] J. Choi, S. Park, W. Lee, and S.-C. Kang, “Design of a robot joint with vari-able stiffness,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2008, pp. 1760–1765.
[20] G. Palli, C. Melchiorri, and A. De Luca, “On the feedback linearization of robots with variable joint stiffness,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2008, pp. 1753–1759.
[21] A. De Luca, F. Flacco, A. Bicchi, and R. Schiavi, “Nonlinear decoupled motion-stiffness control and collision detection/reaction for the VSA-II vari-able stiffness device,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2009, pp. 5487–5494.
[22] A. Isidori, Nonlinear Control Systems, 3rd ed., Springer, 1995.
[23] N. Kircanski and A. Goldenberg, “An experimental study of nonlinear stiff-ness, hysteresis, and friction effects in robot joints with harmonic drives and torque sensors,” Int. J. of Robotics Research, vol. 16, no. 2, pp. 214–239, 1997.
