Computed torque control

Computed torque control is a control scheme used in motion control in robotics. It combines feedback linearization via a PID controller of the error with a dynamical model of the controlled robot.

Let the dynamics of the controlled robot be described by

$$\mathbf{M}\left( \vec\theta \right) \ddot\vec\theta + \mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta + \vec\tau_g \left(\vec\theta\right) = \vec\tau$$ where $$\vec\theta \in \mathbb{R}^N$$ is the state vector of joint variables that describe the system, $$\mathbf{M}\left(\vec\theta\right)$$ is the inertia matrix, $$\mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta

$$ is the vector Coriolis and centrifugal torques, $$\vec\tau_g \left(\vec\theta\right)$$ are the torques caused by gravity and $$\vec\tau $$ is the vector of joint torque inputs.

Assume that we have an approximate model of the system made up of $$\tilde\mathbf{M}\left( \vec\theta \right), \tilde\mathbf{C}\left( \vec\theta, \dot\vec\theta \right), \tilde\vec\tau_g \left(\vec\theta\right) $$. This model does not need to be perfect, but it should justify the approximations $$\mathbf{M}\left( \vec\theta \right)^{-1} \tilde\mathbf{M}\left( \vec\theta \right) \approx \mathbf 1 $$ and $$\mathbf{M} ^{-1} \left( \mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta + \vec\tau_g \left(\vec\theta\right) \right) \approx \mathbf{M} ^{-1} \left( \tilde\mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta + \tilde\vec\tau_g \left(\vec\theta\right) \right)$$.

Given a desired trajectory $$\vec\theta_d(t)$$ the error relative to the current state $$\vec\theta(t)$$ is then $$\vec\theta_e(t) = \vec\theta_d(t) - \vec\theta(t)$$.

We can then set the input of the system to be

$$\vec\tau(t) = \tilde\mathbf{M}\left( \vec\theta \right) \left(   \ddot\vec\theta_d(t)     + K_p \vec\theta_e(t)     + K_i \int_0^t \ddot\vec\theta_e(t') dt'     + K_d \dot\vec\theta_e(t) \right) + \tilde\mathbf{C}\left( \vec\theta, \dot\vec\theta \right) + \tilde\vec\tau_g \left(\vec\theta\right) $$

With this input the dynamics of the entire systems becomes

$$\begin{align} \mathbf{M}\left( \vec\theta \right) \ddot\vec\theta + \mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta + \vec\tau_g \left(\vec\theta\right) =& \tilde\mathbf{M}\left( \vec\theta \right) \left(   \ddot\vec\theta_d(t)     + K_p \vec\theta_e(t)     + K_i \int_0^t \ddot\vec\theta_e(t') dt'     + K_d \dot\vec\theta_e(t) \right) + \tilde\mathbf{C}\left( \vec\theta, \dot\vec\theta \right) + \tilde\vec\tau_g \left(\vec\theta\right)

\\

\ddot\vec\theta + \mathbf{M}\left( \vec\theta \right)^{-1} \left( \mathbf{C}\left( \vec\theta, \dot\vec\theta \right) \dot\vec\theta + \vec\tau_g \left(\vec\theta\right) \right) =& \underbrace{ \mathbf{M}\left( \vec\theta \right)^{-1} \tilde\mathbf{M}\left( \vec\theta \right) }_{\approx \mathbf{1}} \left(   \ddot\vec\theta_d(t)     + K_p \vec\theta_e(t)     + K_i \int_0^t \ddot\vec\theta_e(t') dt'     + K_d \dot\vec\theta_e(t) \right) + \mathbf{M}\left( \vec\theta \right)^{-1} \left( \tilde\mathbf{C}\left( \vec\theta, \dot\vec\theta \right) + \tilde\vec\tau_g \left(\vec\theta\right) \right)

\\

\ddot\vec\theta = & \ddot\vec\theta_d(t) + K_p \vec\theta_e(t) + K_i \int_0^t \ddot\vec\theta_e(t') dt' + K_d \dot\vec\theta_e(t)

\\

0 = & \ddot\vec\theta_e + K_p \vec\theta_e(t) + K_i \int_0^t \ddot\vec\theta_e(t') dt' + K_d \dot\vec\theta_e(t)

\end{align} $$

and the normal methods for PID controller tuning can be applied. In this way the complicated nonlinear control problem has been reduced to a relatively simple linear control problem.