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

Part III : Adaptive Control

In Part II of this trilogy, we demonstrate some effects of model uncertainty on inverse dynamics control. When modeling error increases, closed-loop performance degrades. More parameter variation could even destabilize the system. Some robust nonlinear control scheme can be implemented, normally by switching the control laws in accordance with some specified constraints to compensate model uncertainty. This approach requires estimation of nominal values for plant parameters, and may cause some undesirable effect such as shattering. In this section we introduce an alternative approach, called adaptive control, that is useful when only plant structure is known, but no estimated parameter value is needed.

Here we provide an example of adaptive control law that is discussed in [1],[2], among other robotic texts. It relies on a plant property known as linearity in the parameter, which can be stated briefly as follow: There exist an n \times l matrix function Y(q,\dot{q},\ddot{q}) and an l-dimensional vector \Theta such that the Euler-Lagrange equation can be parameterized as

(1)   \begin{equation*}  D(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = Y(q,\dot{q},\ddot{q})\Theta \end{equation*}

Y(q,\dot{q},\ddot{q}) is called a regressor, and \Theta \in \Re^l is a parameter vector. Note that the size of parameter space, or the total number of parameters needed to describe the dynamics, is not unique. In general, a rigid body can be described with 10 parameters: total mass, 6 independent members of inertia tensor, and 3 Cartesian coordinates representing the center of mass. Therefore, a robot with n links has at most 10n parameters. However, since the link motion is constrained by the connection between adjacent joints, the number of parameters is practically less than that.

Here we show how the 2-link manipulator is parameterized. This structure will be exploited by the adaptive control scheme in this article.

From the equation of motion of the 2-link manipulator in Part I , the inertia terms can be grouped as

(2)   \begin{equation*}  \begin{array}{l} \Theta_1 = m_1l_{c1}^2 + m_2\left(l_1^2 + l_{c2}^2\right) + I_1 + I_2 \\ \Theta_2 = m_2l_1l_{c2} \\ \Theta_3 = m_2l_{c2}^2 + I_2 \end{array}  \end{equation*}

by substituting them into the members of inertia matrix

(3)   \begin{equation*}  \begin{array}{l} d_{11} = \Theta_1 + 2\Theta_2\cos q_2 \\ d_{12} = d_{21} = \Theta_3 + \Theta_2\cos q_2 \\ d_{22} = \Theta_3 \end{array} \end{equation*}

The Christoffel symbols need no additional parameter since they are functions of the members of inertia matrix. The gravity terms, on the other hand, have new parameters after setting

(4)   \begin{equation*}  \begin{array}{l} \Theta_4 = m_1l_{c1} + m_2l_1 \\ \Theta_5 = m_2l_2 \end{array} \end{equation*}

The gravity terms g_1 and g_2 hence becomes

(5)   \begin{equation*}  \begin{array}{l} g_1 = \Theta_4g\cos q_1 + \Theta_5 \cos(q_1 + q_2) \\ g_2 = \Theta_5g \cos(q_1 + q_2) \end{array} \end{equation*}

Substituting all these terms into the equation of motion yields an equation in the form (1), with

(6)   \begin{equation*}  Y(q,\dot{q},\ddot{q})  = \left( \begin{array}{ccccc} \ddot{q}_1 & \cos(q_2)(2\ddot{q}_1+\ddot{q}_2)-\sin(q_2)(\dot{q}_2^2+2\dot{q}_1\dot{q}_2) & \ddot{q}_2 & g\cos(q_1) & g\cos(q_1+q_2) \\ 0 & \cos(q_2)\ddot{q}_1+\sin(q_2)\dot{q}_1^2 & \ddot{q}_1+\ddot{q}_2 & 0 & g\cos(q_1+q_2) \end{array} \right) \end{equation*}

and the parameter vector

(7)   \begin{equation*}  \Theta = \left( \begin{array}{c}  \Theta_1 \\ \Theta_2 \\ \Theta_3 \\ \Theta_4 \\ \Theta_5  \end{array} \right) = \left( \begin{array}{c} m_1l_{c1}^2 + m_2\left(l_1^2 + l_{c2}^2\right) + I_1 + I_2 \\ m_2l_1l_{c2} \\ m_2l_{c2}^2 + I_2 \\ m_1l_{c1} + m_2l_1 \\ m_2l_{c2} \end{array} \right) \end{equation*}

Now we state without proof an adaptive control problem for a general robot dynamical equation in the form

(8)   \begin{equation*}  M(q)\ddot{q} + C(q,\dot{q})\dot{q} +B\dot{q} + g(q) = Y(q,\dot{q},\ddot{q})\Theta = u \end{equation*}

with the adaptive control law chosen as

(9)   \begin{equation*}  u = Y(q,\dot{q},\dot{q}_r, \ddot{q}_r)\hat{\Theta} + K_vv  \end{equation*}

where K_v is a positive definite matrix, \Lambda positive definite and usually diagonal, \tilde{q} = q_d - q, \dot{q}_r = \dot{q}_d + \Lambda\tilde{q}, \ddot{q}_r = \ddot{q}_d + \Lambda\dot{\tilde{q}}, and v = \dot{q}_r - \dot{q} = \dot{\tilde{q}} + \Lambda\tilde{q} , and the parameter adaptation law

(10)   \begin{equation*}  \dot{\hat{\Theta}} = K_\Theta^{-1}Y^T(q,\dot{q},\dot{q}_r, \ddot{q}_r)(\dot{\tilde{q}} + \Lambda\tilde{q}) \end{equation*}

The robot trajectory described by the model (8) will globally converge to v = 0 and \tilde{q} = 0 asymptotically, which implies \tilde{q}, \dot{\tilde{q}} must converge to zero. Since \hat{\Theta} is bounded, it can be shown that

(11)   \begin{equation*}  Y(q,\dot{q},\dot{q}_r, \ddot{q}_r)(\hat{\Theta} - \Theta) = 0 \end{equation*}

asymptotically. The derivation details, rather too lengthy for an online article, are provided in [3].

Note that the equation (11) does not imply that \hat{\Theta} must converge to \Theta since parameter convergence to the real value depends on the structure of Y(q,\dot{q},\dot{q}_r, \ddot{q}_r) matrix, together with both trajectory commands and actual values. Nevertheless, the mentioned approach can be classified as direct adaptive control problem; that is, finding a control such that tracking error is bounded, instead of finding the real values of system parameters (the so-called indirect adaptive control problem).

A block diagram of adaptive control scheme discussed thus far can be shown as in Figure 3.1, consisting of 3 major parts

  • Y\hat{\Theta} : describing the control from inverse dynamics estimation, which is basically nonlinear compensation and joint decoupling
  • K_vv : providing stability and tracking error reduction, similar to a PD control
  • \hat{\Theta} : parameter vector estimation, updated online with convergence rate specified by K_\Theta matrix

 adaptive control block diagram

Figure 3.1 block diagram of a joint-space adaptive control scheme

Using the structure in Figure 3.1, now we are ready to construct an adaptive control scheme for the 2-link manipulator. First, we need to make a slight modification to the development in (1) – (7) above, since from Figure 3.1 the dynamical equation to be parameterized is in the form

(12)   \begin{equation*}  D(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = Y(q,\dot{q},\dot{q}_r,\ddot{q}_r)\Theta \end{equation*}

This gives the same parameter vector as in (7), but the regressor in this case is not Y(q,\dot{q},\ddot{q}), but Y(q,\dot{q},\dot{q}_r,\ddot{q}_r). It is straightforward to verify that

(13)   \begin{equation*}  Y(q,\dot{q},\dot{q}_r,\ddot{q}_r)  = \left( \begin{array}{ccccc} \ddot{q}_{1r} & c_2(2\ddot{q}_{1r}+\ddot{q}_{2r})-s_2(\dot{q}_1\dot{q}_{2r}+\dot{q}_2\dot{q}_{1r}+\dot{q}_2\dot{q}_{2r}) & \ddot{q}_{2r} & gc_1 & gc_{12} \\ 0 & c_2\ddot{q}_{1r}+s_2\dot{q}_1\dot{q}_{1r} & \ddot{q}_{1r}+\ddot{q}_{2r} & 0 & gc_{12}  \end{array} \right) \end{equation*}

Note : Here we use abbreviation c_1 = \cos(q_1), s_1 = \sin(q_1), c_{12} = \cos(q_1 + q_2) etc.

By using (13), we can construct an Xcos model for adaptive control simulation of the 2-link manipulator as in Figure 3.2, which, however complicated, is just a concrete example of the structure in Figure 3.1. Note in the diagram of Figure 3.2 that the adaptive controller is wrapped in a superblock, with its internal detail as shown in Figure 3.3. So, instead of building all of these by your own, it is strongly recommended that you use the adaptive_2link_track.zcos file that comes with RTSX to save time and effort. Also for your convenience, all the parameter setup, trajectory generation, and gains for the adaptive controller are contained in the script file setup_adaptive_2link.sce .

 adaptive_2link_track

Figure 3.2 Xcos model for adaptive control of 2-link manipulator adaptive_2link_track.zcos

 adaptive control internal

Figure 3.3 internal structure of adaptive controller superblock

To demonstrate tracking capability of adaptive control, we apply a pair of multi-segment trajectory commands for the two joints. From Chapter 4 of [3], we see that RTSX has a specific command mstraj to generate multi-segment trajectory. Unfortunately, this command provides only joint positions, not their derivatives required by the diagram in Figure 3.2. So for this example we need to create a multi-segment trajectory manually.

Suppose we want to create a two-joint, three-segment trajectory during the period 0 – 5 seconds, with specified boundary values for each segment, using the quintic polynomial generator qpoly. The following set of commands is just an example on how to do it

t1=0:0.01:1.99;
t2 = 0:0.01:0.99;
// first joint trajectory
[q11_d,qd11_d,qdd11_d]=qpoly(0,10,t1,0,10);
[q12_d,qd12_d,qdd12_d]=qpoly(10,-5,t1,10,-5);
[q13_d,qd13_d,qdd13_d]=qpoly(-5,0,t2,-5,0);
q1_d = [q11_d;q12_d;q13_d];
qd1_d = [qd11_d;qd12_d;qd13_d];
qdd1_d = [qdd11_d;qdd12_d;qdd13_d];
// second joint trajectory
[q21_d,qd21_d,qdd21_d]=qpoly(0,-5,t2,0,-10);
[q22_d,qd22_d,qdd22_d]=qpoly(-5,10,t1,-10,10);
[q23_d,qd23_d,qdd23_d]=qpoly(10,0,t1,10,0);
q2_d = [q21_d;q22_d;q23_d];
qd2_d = [qd21_d;qd22_d;qd23_d];
qdd2_d = [qdd21_d;qdd22_d;qdd23_d];

Note that nonzero velocity are specified at the segment boundaries other than the start and end of trajectory. Now we convert the data to “From workspace” block format as before

t=0:0.01:4.99;
t=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;

Since the adaptive controller has 3 sets of adjustable gains, and unlike a standard controller such as PID, there is no rule of thumb on how to select good values for them. So the tuning procedure is more like trial and error.

First we play around on the K_v gain, which is quite similar to the gain of PD control in some aspects; i.e., used to stabilize the closed-loop system and improve tracking performance. From our experiment, the values k_{v1} = 1000, k_{v2} = 100 yield good tracking.

Next, we examine K_\Theta = k_\Theta I, which controls parameter convergence. Varying the scalar value k_\Theta from 10 – 100 has no significant effect on the responses. So we finally leave it at K_\Theta = 100I.

The last set of gains is \Lambda = \lambda I , which casts weights on velocity and acceleration compensation for the trajectory. Choosing \lambda = 10 gives the trajectory tracking responses as shown in Figure 3.4, with, from top to bottom, position, velocity, and acceleration of each joint. It can be seen that tracking performance of joint 2 (right) is satisfactory, while there is still noticeable tracking error at joint 1 (left). So we crank the gain up to \lambda = 60 , resulting in the tracking response of Figure 3.5. The position and velocity for both joints now track the command quite well, though the acceleration now becomes more noisy.

 adaptive tracking 1

Figure 3.4 tracking performance of adaptive control with \Lambda = 10I

 adaptive tracking 2

Figure 3.5 tracking performance of adaptive control with \Lambda = 60I

Note : one must keep in mind this is a simulation result. In practice, a mechanical structure is unlikely to provide such abrupt change in acceleration.

The above simulation results reveals the nice feature of adaptive control scheme. The controller does not need an estimated value of any parameter. Only knowledge of plant structure in parameterized form is sufficient. Moreover, no expensive (and noise-prone) sensor is required to measure joint acceleration. The adaptive control does have some drawbacks such as 1) Its complexity might lead to implementation problem, especially on an embedded system with limited resources. 2) There is no systematic way to tune the controller gains 3) It may not be suitable for plant with unmodelled dynamics, or in an environment with high exogenous noise signals.

Scilab/Xcos Files

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

  1. adaptive_2link_track.zcos — adaptive control of 2-link manipulator
  2. setup_adaptive_2link.sce — model initialization script

To run the above script file in Scilab, type exec(‘setup_adaptive_2link.sce’,-1);

References

  1. M.W.Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control. John Wiley & Sons. 2006.
  2. B. Siciliano, L. Sciavicco, L. Villani and G. Oriolo, Robotics: Modeling, Planning and Control, Springer 2010.
  3. V. Toochinda, Robot Analysis and Control with Scilab and RTSX, Mushin Dynamics, 2014.

Comments

comments

Comments

Nonlinear Control of 2-link Manipulator with Scilab and RTSX (Part III) — 1 Comment

  1. Pingback: Scilab Recipe 3: Xcos Blocks Seasoning | Scilab Ninja