Part II : Inverse Dynamics ControlIn 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.
(1)where we define
(2)We want to find a control vector 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)Substituting this into (1) and using the property that is invertible, we have
(4)where is the input to be chosen. The equation (4) is the desired double integrator system, with the joints decoupled; i.e., each input is applied to control a linear SISO feedback system. Moreover, if is a function of only and , 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)Substituting into (4) yields a second order equation
(6)When the desired trajectory is specified, tracking can be achieved by choosing
(7)Substituting (7) into (6) gives
(8)This second order equation describes the dynamics of tracking error . A practical choice is to choose matrices and in diagonal form as
This results in a decoupled closed-loop system, with the joint responses corresponding to those of second order system with natural frequency and damping ratio . 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.
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
l1 = 1; l2 = 1; m1 = 5; m2 = 5; lc1 = l1/2; lc2 = l2/2; I1 = 2; I2 = 2; g = 9.81; d22=m2*lc2^2+I2; 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); d22_e=m2_e*lc2_e^2+I2_e;
and put into the simulation via “From workspace” blocks as in Figure 2.3. We start the experiment by choosing the gains (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 . 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.
t=0:0.01:5; [q1_d,qd1_d,qdd1_d]=lspb(0,30,t); [q2_d,qd2_d,qdd2_d]=lspb(0,-10,t); 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;
Scilab/Xcos FilesFor your convenience, here are the model and script files used in this article.
- invdyn_2link.zcos — inverse dynamics control of 2-link manipulator
- setup2link.sce — model initialization script (random parameter variation)
- setup2link_d.sce — model initialization script (deterministic parameter variation)
- M.W.Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control. John Wiley & Sons. 2006.
- V. Toochinda, Robot Analysis and Control with Scilab and RTSX, Mushin Dynamics, 2014.