Nonlinear Control of 2-link Manipulator with Scilab and RTSX (Part II)

Part II : Inverse Dynamics Control

In the last article, we exploit some part of the robot model, the gravity term, and compensate it with the controller. That makes the control law nonlinear and more computationally-intensive than the standard PID. With the vast improvement in computer technology, this complexity has become less an issue nowadays. In fact, we are now tempted to take advantage of the whole robot dynamics when a reliable model is available. This is a rationale behind inverse dynamics control scheme.

First we have to make a distinction between inverse dynamics (Figure 2.1) and feedforward (Figure 2.2) control scheme. As can be seen from the diagrams, the former has the nonlinear inverse dynamics terms embedded in the feedback loops, unlike the latter where the compensation terms are in the feedforward path (and hence can be precomputed).

Note also that in the literature there are some variations of the inverse dynamics control schemes. Some are implemented in joint space while others in Cartesian (or operational) space. For introductory purpose, here we mention only joint space inverse dynamics control law.

 inverse dynamics

Figure 2.1 an inverse dynamics control scheme


Figure 2.2 a feedforward control scheme

The idea of inverse dynamics control is simple. Given a general robot structure with interaction between its joints, the robot plant model data is used to decouple the joints to linear double integrators, so that linear SISO control can be applied. To elaborate, let us consider again the robot equation of motion from Part I. Assume for simplicity that the friction term is negligible, the equation can be described as

(1)   \begin{equation*}  M(q)\ddot{q}+f(q,\dot{q})=u \end{equation*}

where we define

(2)   \begin{equation*}  f(q,\dot{q})=C(q,\dot{q})\dot{q}+q(q) \end{equation*}

We want to find a control vector u as a function of system state that, when substituting into (1), yields a linear feedback system. Finding such control law for a general nonlinear system could be difficult, if not impossible. For the robot dynamics in (1), however, the problem is quite easy. From observation, suppose we choose a control variable

(3)   \begin{equation*}  u=M(q)v+f(q,\dot{q}) \end{equation*}

Substituting this into (1) and using the property that M is invertible, we have

(4)   \begin{equation*}  \ddot{q}=v \end{equation*}

where v is the input to be chosen. The equation (4) is the desired double integrator system, with the joints decoupled; i.e., each input v_k is applied to control a linear SISO feedback system. Moreover, if v_k is a function of only q_k and \dot{q}_k, the feedback systems are also decoupled.

Hence, the goal of inverse dynamics control is to choose v to control a second order linear system. Consider

(5)   \begin{equation*}  v = r - K_qq - K_d\dot{q} \end{equation*}

Substituting into (4) yields a second order equation

(6)   \begin{equation*}  \ddot{q} + K_d\dot{q}+K_pq = r \end{equation*}

When the desired trajectory q_d(t) is specified, tracking can be achieved by choosing

(7)   \begin{equation*}  r = \ddot{q}_d+K_d\dot{q}_d + K_pq_d \end{equation*}

Substituting (7) into (6) gives

(8)   \begin{equation*}  \ddot{\tilde{q}} + K_d\dot{\tilde{q}} + K_p\tilde{q} = 0 \end{equation*}

This second order equation describes the dynamics of tracking error \tilde{q} = q_d - q. A practical choice is to choose matrices K_p and K_d in diagonal form as

    \begin{displaymath} K_p = \left( \begin{array}{cccc} \omega_1^2 & 0 & \ldots & 0 \\ 0 & \omega_2^2 & \ldots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \ldots & \omega_n^2  \end{array} \right) \end{displaymath}

    \begin{displaymath} K_d = \left( \begin{array}{cccc} 2*\zeta_1\omega_1 & 0 & \ldots & 0 \\ 0 & 2*\zeta_2\omega_2 & \ldots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \ldots & 2*\zeta_n\omega_n  \end{array} \right) \end{displaymath}

This results in a decoupled closed-loop system, with the joint responses corresponding to those of second order system with natural frequency \omega_i and damping ratio \zeta_i. The block diagram of this inverse dynamics control shows an inner/outer loop structure in Figure 2.1. The purpose of inner loop is to transform the nonlinear, coupled dynamics to a linear, decoupled system, while the outer loop takes care of stabilization and trajectory tracking.

To simulate tracking response of the 2-link manipulator under inverse dynamics control, an Xcos model conforming to the diagram in Figure 2.1 must be constructed as shown in Figure 2.3. For your convenience, this model is included with RTSX in the /xcos/nonlinear_examples subdirectory, or can be downloaded from the link below this article. Click open the file to see the model more clearly. Though appeared messy, it is nothing more than a realization of Figure 2.1.

Figure 2.3 Xcos model for inverse dyamics control invdyn_2link.zcos

Once again, to be realistic, we assume that the parameters used in the control law vary within +/- 35 % of actual values. All the required parameters are initialized by executing this chunk of code

l1 = 1; l2 = 1;
m1 = 5; m2 = 5;
lc1 = l1/2; lc2 = l2/2;
I1 = 2; I2 = 2;
g = 9.81;
et = 0.7;  // change this to adjust parameter variation range
m1_e = m1 + et*m1*(rand()-0.5);
m2_e = m2 + et*m2*(rand()-0.5);
lc1_e = lc1 + et*lc1*(rand()-0.5);
lc2_e = lc2 + et*lc2*(rand()-0.5);
I1_e = I1 + et*I1*(rand()-0.5);
I2_e = I2 + et*I2*(rand()-0.5);

which are also provided as a script file setup2link.sce. Another data that needs to be constructed are the trajectory commands and their derivatives, which are provided by an RTSX command such as lspb, for example

Q1_d.time = t'; Q1_d.values = q1_d;
Qd1_d.time = t'; Qd1_d.values = qd1_d;
Qdd1_d.time = t'; Qdd1_d.values = qdd1_d;
Q2_d.time = t'; Q2_d.values = q2_d;
Qd2_d.time = t'; Qd2_d.values = qd2_d;
Qdd2_d.time = t'; Qdd2_d.values = qdd2_d;

and put into the simulation via “From workspace” blocks as in Figure 2.3.

We start the experiment by choosing the gains K_p = 300I, K_d =20I (which are lower than the gains in the Part I by a magnitude of ten). The corresponding simulation result is shown as red color in Figure 2.4. The tracking performance for joint 1 is good, though that of joint 2 still looks unsatisfactory. So we increase the feedback gains of second loop to k_{p2} = 800, k_{d2} = 70. The tracking response seems to improve a bit. Figure 2.5 plots the tracking errors of both joints. Comparing this result to the case of PD control with only gravity compensation in Figure 1.9 from Part I, we see that tracking performance can be improved by inverse dynamics control, without relying on excessively high PD gains as in Part I.

Figure 2.4 tracking performance of inverse dynamics control

Figure 2.5 tracking error of inverse dynamics control

Next, we want to observe how tracking performance deteriorates as parameter variations increase. The feedback gains are kept at some certain values that yield good result; i.e., p_{p1} = 300, k_{p2} = 800, k_{d1} = 20, k_{d2} = 70. Suppose that our lab equipment has some difficulty in measuring the inertia I_i, i = 1,2 accurately. So we run the simulation for 3 inertia value variation: -60%, 30%, 50% The effects on tracking performance are shown in Figure 2.6. The joint angles swing out of the desired path at some point. The effect on second link is somewhat more pronounced, and the effect is more sensitive towards positive variation of the inertia values. From our simulation, the feedback system goes unstable when the measured inertia values increase beyond 60%.

Figure 2.6 effect of inertia variation on tracking performance

We continue our experiment by varying some other values such as link masses m_i, i=1,2 and link center of mass . The simulation results show no significant effect to closed-loop stability and performance from the center of mass variation, though the errors from link mass measurement at -80%, 20%, 40% deteriorate the tracking performance as shown in Figure 2.7. Again the effect is more sensitive in positive direction of the mass variation. The feedback system becomes unstable when the measured link masses are 50% higher than the actual values.

Figure 2.7 effect of link mass variation on tracking performance

Note: to perform the simulation with deterministic parameter variations, replace the rand() function from the initialization code by your desired values, or simply edit the script file setup2link_d.sce provided with RTSX.

Scilab/Xcos Files

For your convenience, here are the model and script files used in this article.

  1. invdyn_2link.zcos — inverse dynamics control of 2-link manipulator
  2. setup2link.sce — model initialization script (random parameter variation)
  3. setup2link_d.sce — model initialization script (deterministic parameter variation)

To run a script file in Scilab, such as setup2link.sce, type exec(‘setup2link.sce’,-1);


  1. M.W.Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control. John Wiley & Sons. 2006.
  2. V. Toochinda, Robot Analysis and Control with Scilab and RTSX, Mushin Dynamics, 2014.



Comments are closed.