Fundamentals of Dynamics and Control of Flexible Joint Robots

Apr 10, 2023 | Tech Robotics

Motivation

Most textbooks model robot dynamics with rigid links and joints, and derive controllers based on such assumptions.

Such models are good enough for industrial robots with rigid joints and link position encoders. However, for collaborative robots with harmonic gears and joint torque sensors, flexible joint dynamics has to be considered to achieve good overall control performance.

This post summarizes some results from 2008@Ott and 2020@Iskandar to compare and show the dynamics and control of rigid and flexible joint robots.

Nomenclature

  • \( q \) - robot link position from link position encoder
  • \( \theta \) - joint motor position from motor position encoder
  • \( B \) - motor space reflected inertia matrix, typically $ B = \text{diag}\{r^2_i \mathcal{I}_i\} $, where $r_i$ and $\mathcal{I}_i$ denote the gear ratio and inertia of the $i$-th motor
  • \( K \) - motor space stiffness matrix, typically $ K = \text{diag}\{k_i\} $
  • \( \tau \) - joint torque sensor measured torque, and \( \tau = K(\theta - q) \)

Refer to Fig. 2 in 2020@Iskandar for a physical configuration of a flexible robot joint.

  • \( M \) - joint space mass matrix, also \( M(q) \)
  • \( C \) - joint space Coriolis matrix, also \( C(q, \dot{q}) \)
  • \( c \) - joint space Coriolis force, with \( c = C(q, \dot{q}) \dot{q}\)
  • \( \tau_{ext} \) - robot external torque
  • \( \tau_m \) - joint motor control torque
  • \( \tau_d \) - desired robot control torque
  • \( K_{\tau} \) - motor torque-loop controller gain
  • \( D_{\tau} \) - motor torque-loop controller damping

Futhermore,

  • $\lceil \mathcal{X} \rfloor ^{\kappa^+} _{\kappa^-}$ - denotes an element-wise vector clamping operator such that for $\mathcal{X} = (\mathcal{X}_1, \mathcal{X}_2, \cdots, \mathcal{X}_n) \in \mathbb{R}^n$ and $ \kappa^- \prec \kappa^+ \in \mathbb{R}^n$, it yields $$ \lceil \mathcal{X} \rfloor ^{\kappa^+}_{\kappa^-} = (\bar{\mathcal{X}}_1, \bar{\mathcal{X}}_2, \cdots, \bar{\mathcal{X}}_n) $$ with $$ \bar{\mathcal{X}}_i = \begin{cases} \kappa_i^+ &,\text{$\mathcal{X}_i \in (\kappa_i^+, \infty) $} \\ \mathcal{X}_i &,\text{$\mathcal{X}_i \in [\kappa_i^-, \kappa_i^+]$} \\ \kappa_i^- &,\text{$\mathcal{X}_i \in (-\infty, \kappa_i^-)$} \end{cases} $$ for $i=1, 2, \cdots, n$.

Robot with Rigid Joints

Typical textbook dynamics model of a robot with rigid joints, for example in 2017@Lynch , is described by $$ M\ddot{q} + c + g = \tau_d + \tau_{ext} \label{eq:rigid-robot-dynamics} $$ assuming that the external force at the Jacobian point \( J^T \mathcal{F}_{tip} \) is sensed by the joint torque sensors as external torques.

To track a desired joint trajectory \( \{ q_d, \dot{q}_d, \ddot{q}_d \} \), a simple impedance plus feed-forward controller would do, such as: $$ \tau_d = M \tau^* + c + g - \tau_{ext} $$ with $$ \tau^* = -K_p(q - q_d) - K_d(\dot{q} - \dot{q}_d) + \ddot{q}_d $$

Robot with Flexible Joints and Singular Perturbation Theory

Refer to 2020@Iskandar , the dynamics of a flexible joint robot can be described by $$ M\ddot{q} + c + g = K(\theta - q) + \tau_{ext} $$ $$ B\ddot{\theta} + K(\theta - q) = \tau_m $$ In this case, to achieve the same joint trajectory tracking control as in the previous section, it requires a full-state motor controller. One can choose $$ \tau_m = \tau_{d} - K_{\tau} (\tau_d - \tau) - \epsilon D_{\tau}\dot{\tau} \label{eq:flexible-joint-torque} $$ as proposed in 2008@Ott , where \( \epsilon \in \mathbb{R}^+ \) is a small number based on the singular perturbation analysis in Sec. 5.2. Then, based on further derivation, the closed-loop system dynamics becomes $$ \bigl\{ M + (I + K_{\tau}^{-1})B \bigl\} \, \ddot{q} + c + g = \tau_d \label{eq:flexible-joint-mass} $$ in contrast to \eqref{eq:rigid-robot-dynamics}, where \( \tau_d \) is a new control input which can be designed according to the control law for a rigid robot with its mass matrix given by $$ \mathcal{M} = M + (I + K_{\tau}^{-1})B $$ Then, according to the previous section, $ \tau_d $ is $$ \tau_d = \mathcal{M} \tau^* + c + g - \tau_{ext} $$ However, in term of \( \tau^* \), since there are flexible elements and additional motor position encoders, the following cases should be considered:

  • if robot end-effector precision is critical, one can consider the following design, since \( q \) is the measured robot link position that directly translates to end-effector position based on robot kinematics, $$ \tau^* = -K_p(q - q_d) - K_d(\dot{q} - \dot{q}_d) + \ddot{q}_d $$
  • if robot stability or vibration reduction is critical, one can consider the following design, since \( \theta \) is the measured motor position, and is not affected joint flexible elements, $$ \tau^* = -K_p(\theta - \theta_d) - K_d(\dot{\theta} - \dot{\theta}_d) + \ddot{\theta}_d $$
  • or, one can consider $$ \tau^* = -K_p(\psi - \psi_d) - K_d(\dot{\psi} - \dot{\psi}_d) + \ddot{\psi}_d $$ where \( \psi \) is the fusion of \( q \) and \( \theta \), which approaches \( q \) at low speed or frequency to make sure robot link and end-effector precision is guaranteed; and approaches \( \theta \) at high speed or frequency to suppress vibration introduced by joint flexibility.

Summary and Practical Considerations

Given a robot with flexible joints, if the robot dynamics can be captured by \( M, c, g \) and the motor inertia can be captured by \( B \), one can design the robot level control law to be $$ \tau_d = \mathcal{M} \tau^* + c + g - \tau_{ext} $$ as if the robot is rigid with an altered mass matrix \( \mathcal{M} = M + (I + K_{\tau}^{-1})B \). In practice, one my choose the simplified the control law to be $$ \tau_d = \mathcal{M} \tau^* + g $$ given that the Coriolis force estimate can be inaccurate since \( C(q, \dot{q}) \dot{q} \) requires multiplication of derivative terms, and external torque could also be difficult to estimate.

The final torque sent to joint motors is $$ \tau_m = \tau_{d} - K_{\tau} (\tau_d - \tau) - \epsilon D_{\tau}\dot{\tau} $$

Furthermore, even the unaltered robot mass matrix can sometimes be difficult to estimate, and inaccurate estimation introduces undesirable dynamics coupling that will negatively affect control performance. Methods to deal with that will be discussed in another post.

Also, this post did not discuss the joint disturbance such as friction, which is typically difficult to model. In practice, the magnitude of friction during acceleration and deceleration could be as large as the control torque, and should be accounted for. That is, in additional to compensating for gravity and Coriolis force, one should consider estimate joint disturbances using a disturbance observer. And add the observed term to the control law. That is: $$ \tau_m = \tau_{d} - K_{\tau} (\tau_d - \tau) - \epsilon D_{\tau}\dot{\tau} + \hat{\tau}_{dist} $$ where $ \hat{\tau}_{dist} $ is disturbance torque term, including friction, stiction, and effect of gearbox mechanical efficiency causing power loss. The total disturbance torque can be estimated by various joint disturbance observers. Details on that will be discussed in a separate post.

Also, in practice, integral terms and anti-windup schemes, such as clamping, are typically used in additional to the impedance control terms to improve performance, for example: $$ \tau^* = \ddot{q}_d - K_p(q - q_d) - K_d(\dot{q} - \dot{q}_d) - \lceil K_i \int_0^t (q - q_d) dt \rfloor_{\kappa^-}^{\kappa^+} $$

Notes on Singular Perturbation Theory Applied to Flexible Joint Dynamics

Analysis and derivation in Ch. 5 of 2008@Ott is elegant but the theoretical depth makes the physical meaning behind it rather difficult to interpret from the equations. In essence, what it is saying to robots with flexible joints is the following.

A joint with flexible dynamics is a 4th-order system. It is theoretically an underactuated system, consists of a fast dynamics (joint flexibility level) and a slow dynamics (robot/motor dynamics level). One cannot completely control all states of the system due to the underactuated nature of the system.

However, since the transient response of the fast dynamics is much faster than the slow dynamics; as long as the robot level control is not pushing its control bandwidth too high, the system can be analyzed and partially controlled to achieve desired performance. To facilitate the partial control, one should apply closed-loop torque control at the motor level, then employ control joint torque given by \eqref{eq:flexible-joint-torque}, and estimate system mass according to \eqref{eq:flexible-joint-mass}.

If the robot level slow dynamics control tries to push its bandwidth close the fast dynamics transient response level, the fast dynamics cannot be ignored, the two dynamics layers become strongly coupled, and the system will be practically uncontrollable.