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{aligned}\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{aligned}}$

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.