An Approach to the Dynamics and Control of Uncertain Robot Manipulators

Abstract

In this paper, a novel constraint-following control for uncertain robot manipulators that is inspired by analytical dynamics is developed. The motion can be regarded as external constraints of the system. However, it is not easy to obtain explicit equations for dynamic modeling of constrained systems. For a multibody system subject to motion constraints, it is a common practice to introduce Lagrange multipliers, but using these to obtain explicit dynamical equations is a very difficult task. In order to obtain such equations more simply, motion constraints are handled here using the Udwadia-Kalaba equation(UKE). Then, considering real-life robot manipulators are usually uncertain(but bounded), by using continuous controllers compensate for the uncertainties. No linearizations/approximations of the robot manipulators systems are made throughout, and the tracking errors are bounds. A redundant manipulator of the SCARA type as the example to illustrates the methodology. Numerical results are demonstrates the simplicity and ease of implementation of the methodology.

1. Introduction

The main methods currently used for dynamic modeling of robot manipulators with motion constraints are the Newton–Euler method [1,2,3], Lagrange’s method [4,5], and Kane’s method [6]. The Newton–Euler method describes motion and force through the use of vectors. In the modeling procedure, every component of the mechanism is isolated and the corresponding Newton and Euler equations are established. The calculations involved are quite efficient, but it is hard to use this approach when attempting to design control systems for manipulators. For a multibody system with motion constraints, Lagrange’s method can be used, generally with the introduction of Lagrange multipliers, a widely used technique for constrained systems. However, controlling these multipliers is difficult, and the approach is not very well suited for symbolic considerations. Kane’s method combines the advantages of vector mechanics and analytical mechanics, with a generalized rate being used as an independent variable in the equations of motion of the system. The fundamental vector projection of the main force and the inertial force of the system is extended directly to derive the equations of motion. However, with this method, these dynamical equations cannot be obtained in the appropriate analytical form for a constrained mechanical system. All of these approaches share the common fundamental problem that they cannot provide explicit expressions for the dynamical equations of the constrained system. As is well known, the generation of dynamical equations for constrained systems in symbolic form has a number of advantages with regard to issues of both control and mechanical design [7].

To address the control of robot manipulators, valuable control approaches have been presented, such as Jacobian-Matrix-Adaption (JMA) Method [8], sliding mode control (SMC) [9], Robust adaptive control [10], and adaptive neural network control [11]. Unlike most control methods for the control problem of robot manipulators, the present paper does not make any linearizations or approximations. Our methodology is based on the Udwadia–Kalaba equation (UKE) [12,13,14,15,16,17]. The Udwadia–Kalaba equation is obtained by using Gauss’s principle rather than the more commonly used principles of Lagrange, Hamilton, Gibbs, and Appell [18]. This equation takes constraints into account in the dynamical equation and involves the generalized Moore–Penrose inverse [19]. It provides a simple and general explicit equation of motion for constrained mechanical systems without the need for Lagrange multipliers [14,20]. This relatively simple approach allows detailed dynamical analysis of such systems and should improve fundamental understanding of constrained motion in multibody dynamics. Based on UKE, many researchers have used this method to solve problems in the dynamics analysis of constrained systems. Pennestri et al. [21] use UKE to study the slider-crank mechanism and compared the numerical efficiency with the coordinate partitioning (CPE) equation. Chen et al. [22] applied the approach to the trajectory tracking control of the mobile robot, by solving UKE to obtain the control input not make any linearization or approximations. For additional applications using UKE see [23,24,25,26,27,28].

In this paper, we regard the desired trajectory as constraints, by using the UK equation to obtain the control input. Considering the moment of inertial of the robot manipulators are uncertain, a nonlinear controller was adopted to make nonlinear system to track the given trajectory. In the control methodology, this nonlinear controller is augmented by an additional additive controller based on a generalization of the notion of sliding surfaces [15,16,17,29]. It’s important to notice that the control obtained relies on recent advances in analytical dynamics rather than on control theory.

The paper is organized as follows. In Section 2, we introduce the Udwadia–Kalaba equation. In Section 3, we introduce our new approach based on UKE to tracking control of robot manipulator with uncertain dynamics. In Section 4, the proposed method is applied to a redundant SCARA-type manipulator, and illustrative numerical simulation results are presented in Section 3. Finally, this paper is concluded in Section 5.

2. Udwadia–Kalaba Equation

Consider the unconstrained mechanical system, moving under the influence of gravity alone, described by n generalized coordinates $q:={\left[q_1,q_2,\cdots ,q_n\right]}^T$, and with equations of motion expressed in Newtonian or Lagrangian form as

$$M\left(q,t\right)\ddot{q}=Q\left(q,\dot{q},t\right),$$

(1)

with initial conditions

$$q\left(0\right)=q_0,\dot{q}\left(0\right)={\dot{q}}_0.$$

(2)

Here $t\in R$ is the time, M is an $n\times n$ matrix that can be either positive-semidefinite $\left(M\ge 0\right)$ or positive-definite $\left(M>0\right)$ at each instant of time. $\dot{q}$ is the $n\times 1$ velocity vector, $\ddot{q}$ is the $n\times 1$ acceleration vector, and $Q\left(q,\dot{q},t\right)$, called the given force, collects together the normal and Coriolis inertial terms and the applied forces related to q, $\dot{q}$, and t. From (1), when q, $\dot{q}$, and t are known, the acceleration can be obtained as follows:

$$a\left(q,\dot{q},t\right):=M^{-1}\left(q,t\right) Q\left(q,\dot{q},t\right).$$

(3)

Suppose the mechanical system is subject to $l(l<n)$ Paffian (holonomic or non-holonomic) constraints [23]:

$$\sum _{i=1}^n{A}_{li}(q,t){\dot{q}}_i=c_l(q,t),l=1,2,\cdots ,m,$$

(4)

in which $A_{li}(\ast ):R^n\times R\to R$ are both $C^1$, $m\le n$. By differentiating the Equation (4), we can obtain the second-order form constraints as

$$\sum _{i=1}^n(\frac{d}{dt}{A}_{li(q,t)}){\dot{q}}_i+\sum _{i=1}^n(\frac{d}{dt}{A}_{li(q,t)}){\ddot{q}}_i=\frac{d}{dt}{c}_l(q,t),$$

(5)

in which

$$\left\{\begin{array}{c}\frac{d}{dt}{A}_{li}(q,t)={\sum }_{k=1}^n\frac{\partial A_{li}(q,t)}{\partial q_k}{\dot{q}}_k+\frac{\partial A_{li}(q,t)}{\partial t},\hfill \\ \frac{d}{dt}{c}_l(q,t)={\sum }_{k=1}^n\frac{\partial c_l(q,t)}{\partial q_k}{\dot{q}}_k+\frac{\partial c_l(q,t)}{\partial t}.\hfill \end{array}\right.$$

(6)

Let

$$b_l(q,\dot{q},t):=\frac{d}{dt}{c}_l(q,t)-\sum _{i=1}^n(\frac{d}{dt}{A}_{li}(q,t)){\dot{q}}_i.$$

(7)

Then Equation (5) can be written as the matrix form

$$A(q,\dot{q},t)\ddot{q}=b(q,\dot{q},t),$$

(8)

where $b={[b_1,b_2,\cdots ,b_m]}^T$.

When the system is constrained, an additional set of forces act on the manipulator system, and the equation of motion of this constrained manipulator system can then be written as

$$M\left(q,t\right)\ddot{q}=Q\left(q,\dot{q},t\right)+Q^c\left(q,\dot{t},t\right),$$

(9)

where $Q^c\left(q,\dot{q},t\right)$ is an $n\times 1$ vector, which is present because of the additional constraint force and satisfies the constraint conditions.

In Lagrangian mechanics, when the constraints are ideal, $Q^c\left(q,\dot{q},t\right)$ is governed by the usual D’Alembert principle. However, the constraints can also be nonideal, and the constrained system can be subject to both ideal and nonideal constraints at the same time, in which case $Q^c\left(q,\dot{q},t\right)$ can be written as

$$Q^c\left(q,\dot{q},t\right)=Q_{\mathrm{id}}^c\left(q,\dot{q},t\right)+Q_{\mathrm{nid}}^c\left(q,\dot{q},t\right),$$

(10)

where $Q_{\mathrm{id}}^c\left(q,\dot{q},t\right)$ is the ideal constraint force and $Q_{\mathrm{nid}}^c\left(q,\dot{q},t\right)$ the nonideal constraint force.

Assuming that the virtual displacement is $\nu$, the work done by the ideal constraint force is zero, i.e.,

$${\nu }^T{Q}_{\mathrm{id}}^c=0,$$

(11)

while the work done by the nonideal constraint force $Q_{\mathrm{nid}}^c\left(q,\dot{q},t\right)$ is nonzero. i.e.,

$${\nu }^T{Q}_{\mathrm{nid}}^c\ne 0.$$

(12)

Udwadia and Kalaba showed that the ideal constraint force is given by

$$Q_{\mathrm{id}}^c\left(q,\dot{q},t\right)=M^{\frac{1}{2}}{B}^{+}\left(b-A{M}^{-1}Q\right),$$

(13)

and the nonideal constraint force by

$$Q_{\mathrm{nid}}^c\left(q,\dot{q},t\right)=M^{\frac{1}{2}}{B}^{+}\left(I-B^{+}B\right)M^{-\frac{1}{2}}\mathbf{c},$$

(14)

where the matrix $B=A{M}^{-\frac{1}{2}}$ and the superscript “+” indicates the Moore–Penrose inverse matrix. The vector $\mathbf{c}$ is a known vector, which can be obtained experimentally or by observation for a given mechanical system.

From (9), (10), (13), and (14), the general equation describing the dynamics of the constrained system is

$$M\ddot{q}=Q+M^{\frac{1}{2}}{B}^{+}\left(b-A{M}^{-1}Q\right)+M^{\frac{1}{2}}{B}^{+}\left(I-B^{+}B\right)M^{-\frac{1}{2}}\mathbf{c}.$$

(15)

If the work done by constraint forces under virtual displacements is zero, then $Q_{\mathrm{nid}}^c=0$, and the general equation of the constrained system is then simplified to

$$M\ddot{q}=Q+M^{\frac{1}{2}}{B}^{+}\left(b-A{M}^{-1}Q\right)=Q+Q^c.$$

(16)

Premultiplying both sides of Equation (16) with $M^{-1}$, the acceleration of the nominal system that satisfies the constraint in Equation (5)

$$\ddot{q}=a+M^{-1}{Q}^c\left(t\right).$$

(17)

3. Tracking Control of Robot Manipulators with Uncertain Dynamics

We suppose this framework in the joint space of the robot. The task description is given by the end-effector of robot manipulators trajectory as constraint with $h(q,t)=f\left(q\left(t\right)\right)-{\mathbf{x}}_d\left(t\right)=\mathbf{x}\left(t\right)-{\mathbf{x}}_d\left(t\right)=0$, in which $\mathbf{x}=f\left(q\right)$ denotes the forward kinematics [30]. We make use of the differential forward kinematics,

$$\dot{\mathbf{x}}=J\left(q\right)\dot{q},\ddot{\mathbf{x}}=J\left(q\right)\ddot{q}+\dot{J}\dot{q},$$

(18)

and we bring Equation (18) into Equation (8) with

$$A(q,\dot{q},t)=J,b(q,\dot{q},t)={\ddot{\mathbf{x}}}_d-\dot{J}\dot{q}.$$

(19)

We considered the uncertainties in the modeling of the moments of inertia of the robot manipulators by using the concept of a sliding surface [15,16,17]. The equation of motion of the controlled “actual” system is

$$M_a{\ddot{q}}_c=Q_a(q_c,{\dot{q}}_c,t)+Q^c\left(t\right)+Q^u(q_c,{\dot{q}}_c,t),$$

(20)

in which $q_c$ is the generalized coordinate vector of the controlled actual system; the subscript a denotes the actual, real-life system whose knowledge is uncertain, but bounded; the actual “given” force vector is $Q_a$; $Q^c$ is obtained from Equation (16) and $Q^u$ is the additional control force vector that compensates for the actual system.

The tracking errors (position and velocity) between the actual and the nominal system is

$$e_{xa}\left(t\right)={\mathbf{x}}_a\left(t\right)-{\mathbf{x}}_d\left(t\right),{\dot{e}}_{xa}\left(t\right)={\dot{\mathbf{x}}}_a\left(t\right)-{\dot{\mathbf{x}}}_d\left(t\right).$$

(21)

For a sliding surface defined as

$$s\left(t\right)={\dot{e}}_{xa}\left(t\right)+k{e}_{xa}\left(t\right),$$

(22)

when the sliding surface $s=0$, it tracks the desired trajectories of the manipulator system exactly, using a smooth function [15] (instead of the traditional signum function or saturation function), and we can only ensure that the actual system stays within a small region around the origin, $s\in {\mathsf{\Omega }}_{ϵ}$. This region ${\mathsf{\Omega }}_{ϵ}$ defined as

$${\mathsf{\Omega }}_{ϵ}:=\{s\in R^n|\parallel s\parallel \le ϵ\}$$

(23)

can be made arbitrarily small. The method requires the computation of the following estimates:

$$\begin{array}{c}\left(i\right){\lambda }_{min}:=min\left(eigenvaluesofA{M}_a^{-1}{A}^{+}\right),\hfill \\ \left(ii\right)\beta \ge \frac{\parallel A\parallel \parallel \delta \ddot{q}\parallel +(\parallel \dot{A}\parallel +k\parallel A\parallel )\parallel {\dot{e}}_a\parallel }{{\lambda }_{min}},\forall t>0.\hfill \end{array}$$

(24)

in which

$$\delta \ddot{q}=M_a^{-1}(Q_a+Q^c)-M^{-1}(Q+Q^c).$$

(25)

In the above relations, $\parallel \ast \parallel$ means the $L_2$ norm. The additional control force is given as

$$Q^u=-\beta A^{+}(s/ϵ),$$

(26)

where $ϵ$ is a positive number, which can be chosen by the user. The tracking errors boundary is given by (for a Proof, see Ref. [15]):

$$|e_{xa,i}|\le \frac{ϵ}{k},\left|{\dot{e}}_{xa,i}\right|\le 2ϵ,i=1,2,\cdots ,n.$$

(27)

Proof Equation (26).

Define the tracking errors on position as

$$e_{xa}=f\left(q_a\right)-f\left(q\right)=\dot{\mathbf{x}}-{\dot{\mathbf{x}}}_d=A{\dot{e}}_a.$$

(28)

The derivative of Equation (22) is evaluated as

$$\begin{array}{cc}\hfill \dot{s}& ={\ddot{e}}_{xa}+k{\dot{e}}_{xa}={\ddot{\mathbf{x}}}_a-{\ddot{\mathbf{x}}}_d+k({\dot{\mathbf{x}}}_a-{\dot{\mathbf{x}}}_d)=\stackrel{{\ddot{\mathbf{x}}}_a-{\ddot{\mathbf{x}}}_d}{\overbrace{A{\ddot{e}}_a+\dot{A}{\dot{e}}_a}}+\underset{k({\dot{\mathbf{x}}}_a-{\dot{\mathbf{x}}}_d)}{\underbrace{kA{\dot{e}}_a}}\hfill \end{array}.$$

(29)

Upon differentiating the tracking errors $e_a\left(t\right)=q_a\left(t\right)-q_d\left(t\right)$ twice, we have

$${\ddot{e}}_a={\ddot{q}}_a\left(t\right)-{\ddot{q}}_d\left(t\right).$$

(30)

Using Equation (16), and the controlled actual system (Equation (20)), Equation (30) becomes

$${\ddot{e}}_a=M_a^{-1}(Q_a+Q^c)-M^{-1}(Q+Q^c)+M_a^{-1}{Q}^u=\delta \ddot{q}+M_a^{-1}{Q}^u.$$

(31)

Therefore, the time derivative of the sliding manifold by using Equations (31) and (29) can be simplified as:

$$\dot{s}={\ddot{e}}_{xa}+k{\dot{e}}_{xa}=A{\ddot{e}}_a+\dot{A}{\dot{e}}_a+kA{\dot{e}}_a=A(\delta \ddot{q}+M_a^{-1}{Q}^u)+\dot{A}{\dot{e}}_a+kA{\dot{e}}_a.$$

(32)

Considering the Lyapunov function

$$V_a=\frac{1}{2}{s}^{T}s,$$

(33)

suppose $s\in {\mathsf{\Omega }}_{ϵ}$ and compute

$$\begin{array}{cc}\hfill {\dot{V}}_a& =s^T\dot{s}\hfill \\ & =s^T\left(A(\delta \ddot{q}+M_a^{-1}{Q}^u)+\dot{A}{\dot{e}}_a+kA{\dot{e}}_a\right)\hfill \\ & =s^T\left(A(\delta \ddot{q}-M_a^{-1}\beta A^{+}\left(\frac{s}{ϵ}\right))+\dot{A}{\dot{e}}_a+kA{\dot{e}}_a\right)\hfill \\ & =s^T(A\delta \ddot{q}-\beta A{M}_a^{-1}{A}^{+}\left(\frac{s}{ϵ}\right)+\dot{A}{\dot{e}}_a+kA{\dot{e}}_a).\hfill \end{array}$$

(34)

Observing that $s^{T}A{M}_a^{-1}{A}^{+}s\ge {\lambda }_{min}{\parallel s\parallel }^2$, we have

$$\begin{array}{cc}\hfill {\dot{V}}_a\le & \parallel s\parallel (\parallel A\parallel \parallel \delta \ddot{q}\parallel -\beta {\lambda }_{min}\frac{\parallel s\parallel }{ϵ}+\parallel \dot{A}\parallel +k\parallel A\parallel )\parallel {\dot{e}}_a\parallel \hfill \\ & =\parallel s\parallel \parallel A\parallel (\parallel \delta \ddot{q}\parallel -\beta {\lambda }_{min}\frac{\parallel s\parallel }{\parallel A\parallel ϵ}+\frac{\parallel \dot{A}\parallel +k\parallel A\parallel }{\parallel A\parallel }\parallel {\dot{e}}_a\parallel ).\hfill \end{array}$$

(35)

Thus, when $\beta \ge \frac{\parallel A\parallel \parallel \delta \ddot{q}\parallel +(\parallel \dot{A}\parallel +k\parallel A\parallel )\parallel {\dot{e}}_a\parallel }{{\lambda }_{min}},\forall t>0$, Equation (35) is strictly negative. □

4. Application Example: A Redundant SCARA-Type Manipulator

In this section, the proposed Equation (26) is tested on a redundant SCARA-type manipulator [31]. The manipulator is shown in Figure 1.

4.1. Kinematics Analysis

The DH parameters can be used to write the transformations for each link. The general transformation between link $i-1$ and i can be written as:

$${}^{i-1}{T}_i=\left[\begin{array}{cccc}cos{\theta }_i& -sin{\theta }_{i}cos{\alpha }_i& sin{\theta }_{i}sin{\alpha }_i& a_{i}cos{\theta }_i\\ sin{\theta }_i& cos{\theta }_{i}cos{\alpha }_i& -cos{\theta }_{i}sin{\alpha }_i& a_{i}sin{\theta }_i\\ 0& sin{\alpha }_i& cos{\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right].$$

(36)

For the redundant manipulator, the DH parameters are shown in

Table 1. DH parameters of a redundant manipulator.

i	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	0	0	$l_1+d_1$	0
2	$l_2$	0	0	${\theta }_2$
3	$l_3$	0	0	${\theta }_3$
4	$l_4$	$\pi$	0	${\theta }_4$
5	0	0	$l_5+d_5$	0

.

It is straightforward to write the transformation matrix for each link of the manipulator by inserting the DH parameters from Table 1 in Equation (36). Then, the transformation matrix can be easily obtained from

$$T=\left[\begin{array}{cccc}{C}_{234}& S_{234}& 0& l_2{C}_2+l_3{C}_{23}+l_4{C}_{234}\\ S_{234}& -C_{234}& 0& l_2{S}_2+l_3{S}_{23}+l_4{S}_{234}\\ 0& 0& -1& l_1+d_1-l_5-d_5\\ 0& 0& 0& 1\end{array}\right],$$

(37)

where $S_2=sin{\theta }_2$, $S_{23}=sin({\theta }_2+{\theta }_3)$, $S_{234}=sin({\theta }_2+{\theta }_3+{\theta }_4)$, $C_2=cos{\theta }_2$, $C_{23}=cos({\theta }_2+{\theta }_3)$, $C_{234}=cos({\theta }_2+{\theta }_3+{\theta }_4)$.

From Equation (37), we can obtain the position of the end-effector of the manipulator

$$\left\{\begin{array}{c}x=l_2{C}_2+l_3{C}_{23}+l_4{C}_{234},\hfill \\ y=l_2{S}_2+l_3{S}_{23}+l_4{S}_{234},\hfill \\ z=l_1+d_1-l_5-d_5.\hfill \end{array}\right.$$

(38)

Then, the constraint encountered by the manipulator can be written as the following:

$$\left\{\begin{array}{c}x=r_x=\eta cos(t/2)cos\left(t\right)+x_0,\hfill \\ y=r_y=\eta sin(t/2)cos\left(t\right)+y_0,\hfill \\ z=r_z=0.2,\hfill \\ {\theta }_1=0.1+0.1sin\left(t\right),\hfill \\ {\theta }_3=\pi /2.\hfill \end{array}\right.$$

(39)

By differentiating Equation (39) with respect to time t twice and combining with Equation (8), we write the constraints in matrix form:

$$\left\{\begin{array}{c}A=\left[\begin{array}{ccccc}0& -l_2{S}_2-l_3{S}_{23}-l_4{S}_{234}& -l_3{S}_{23}-l_4{S}_{234}& -l_4{S}_{234}& 0\\ 0& C_2{l}_2+l_3{C}_{23}+l_4{C}_{234}& C_{23}{l}_3+l_4{C}_{234}& l_4{C}_{234}& 0\\ 1& 0& 0& 0& -1\\ 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right],\hfill \\ b=\left[\begin{array}{c}{l}_2{C}_2{\dot{\theta }}_2^2+l_3{({\dot{\theta }}_2+{\dot{\theta }}_3)}^2{C}_{23}+l_4{({\dot{\theta }}_2+{\dot{\theta }}_3+{\dot{\theta }}_4)}^2{C}_{234}-\frac{5}{4}\eta cos\frac{t}{2}cost+\eta sin\frac{t}{2}sint\\ l_2{S}_2{\dot{\theta }}_2^2+l_3{({\dot{\theta }}_2+{\dot{\theta }}_3)}^2{S}_{23}+l_4{({\dot{\theta }}_2+{\dot{\theta }}_3+{\dot{\theta }}_4)}^2{S}_{234}-\frac{5}{4}\eta sin\frac{t}{2}cost-\eta cos\frac{t}{2}sint\\ 0\\ -0.1sin\left(t\right)\\ 0\end{array}\right].\hfill \end{array}\right.$$

(40)

4.2. Dynamics Analysis

The dynamics equation of the manipulator can be written as:

$$M\left(q\right)\ddot{q}+C\left(q,\dot{q}\right)+G\left(q,\dot{q}\right)=\tau ,$$

(41)

where $\tau$ represents the generalized forces vector, M is the inertia matrix, C is the centrifugal and Coriolis forces vector, and G is the gravitational force vector.

The inertia matrix is a symmetric positive definite matrix, which means $M_{ij}=M_{ji}$. The inertia matrix for the manipulator system is given as follows:

$$\left\{\begin{array}{c}\begin{array}{c}{M}_{11}=m_1+m_2+m_3+m_4+m_5,M_{15}=-m_5,M_{12}=M_{13}=M_{14}=M_{25}=M_{35}=M_{45}=0,\hfill \\ M_{22}=l_{c2}^2{m}_2+(l_2^2+l_{c3}^2+2l_2{l}_{c3}{C}_3)m_3+(l_2^2+l_3^2+l_{c4}^2+2l_2{l}_3)m_4+2(l_3{l}_{c4}{C}_4+l_2{l}_{c4}{C}_{34})m_4\hfill \\ +(l_2^2+l_3^2+l_4^2+2l_2{l}_3{C}_3)m_5+2(l_3{l}_4{C}_4+l_2{l}_4{C}_{34})m_5+I_{2zz}+I_{3zz}+I_{4zz},\hfill \\ M_{23}=(l_{c3}^2+l_3{l}_{c3}{C}_3)m_3+(l_3^2+l_{c4}^2+l_2{l}_3{C}_3+2l_3{l}_{c4}{C}_4)m_4+l_2{l}_4{C}_{34}{m}_5,\hfill \\ +(l_3^2+l_4^2+l_2{l}_3{C}_3+2l_3{l}_4{C}_4)m_5+I_{3zz}+I_{4zz},\hfill \\ M_{24}=(l_{c4}^2+l_3{l}_{c4}{C}_4+l_2{l}_{c4}{C}_{34})m_4+(l_4^2+l_3{l}_4{C}_4+l_2{l}_4{C}_{34})m_5+I_{4zz},\hfill \\ M_{33}=l_{c3}^2{m}_3+(l_3^2+l_{c4}^2+2l_3{l}_{c4}{C}_4)m_4+(l_3^2+l_4^2+2l_3{l}_4{C}_4)m_5+I_{3zz}+I_{4zz},\hfill \\ M_{34}=(l_{c4}^2+l_3{l}_{c4}{C}_4)m_4+(l_4^2+l_3{l}_4{C}_4)m_5+I_{4zz},\hfill \\ M_{44}=l_{c4}^2{m}_5+l_4^2{m}_5+I_{4zz},M_{55}=m_5.\hfill \end{array}\hfill \end{array}\right.$$

(42)

The components of centrifugal and Coriolis forces C are given as:

$$\left\{\begin{array}{c}\begin{array}{c}{C}_{11}=C_{15}=0,\hfill \\ C_{12}=-(l_2{S}_3{\dot{\theta }}_3^2+2l_2{S}_3{\dot{\theta }}_2{\dot{\theta }}_3)(l_{c3}{m}_3+l_4{m}_4+l_3{m}_5)+(l_2{S}_{34}{\dot{\theta }}_3^2-2(l_3{S}_4+l_2{S}_{34}){\dot{\theta }}_3{\dot{\theta }}_4)(l_{c4}{m}_4+l_4{m}_5),\hfill \\ +(l_2{S}_{34}{\dot{\theta }}_2{\dot{\theta }}_3-({\dot{\theta }}_4^2+2{\dot{\theta }}_2{\dot{\theta }}_4)\left(l_2{S}_{34}\right))(l_{c4}{m}_4+l_4{m}_5),\hfill \\ C_{13}=(l_{c3}{m}_3+l_3{m}_4+l_3{m}_5)l_2{S}_3{\dot{\theta }}_2^2+(l_2{S}_{34}{\dot{\theta }}_2^2-(2({\dot{\theta }}_2+{\dot{\theta }}_3)+{\dot{\theta }}_4)l_3{S}_4{\dot{\theta }}_4)(l_{c4}{m}_4+l_4{m}_5),\hfill \\ C_{14}=(l_{c4}{m}_4+l_4{m}_5)(l_3{S}_4+l_2{S}_{34}){\dot{\theta }}_2^2+(l_3{S}_4{\dot{\theta }}_3^2+2l_3{\dot{\theta }}_2{\dot{\theta }}_3)(l_{c4}{m}_4+l_4{m}_5).\hfill \end{array}\hfill \end{array}\right.$$

(43)

The components of the gravitational force are given by:

$$G={\left[(m_1+m_2+m_3+m_4+m_5)g,0,0,0,-m_{5}g\right]}^T.$$

(44)

According to the UK equation, the constraint force, which represents the inverse dynamics of the manipulator, can be written in the form

$$Q^c=M^{\frac{1}{2}}{(A{M}^{-\frac{1}{2}})}^{+}(b-A{M}^{-1}(-C-G)).$$

(45)

4.3. Numerical Simulation

The numerical values of the parameters used for simulation were: $m_1=0.524$ kg, $m_2=m_3=m_4=1.023$ kg, $m_5=0.14$ kg, $∆m_1=0.1m_{1}sin\left(t\right)$, $∆m_2=0.1m_{2}sin\left(t\right)$, $∆m_3=0.1m_{3}sin\left(t\right)$, $∆m_4=0.1m_{4}sin\left(t\right)$, $∆m_5=0.1m_{5}sin\left(t\right)$, $l_1=0.524$ m, $l_2=l_3=l_4=0.2$ m, $l_5=0.14$ m, $l_{1zz}=l_{2zz}=l_{3zz}=0.0058\mathrm{kg}·{\mathrm{m}}^2$, $l_{c2}=l_{c3}=l_{c4}=0.0229$ m, $\eta =0.1$, $x_0=y_0=0.2$, $g=9.8\mathrm{m}/{\mathrm{s}}^2$. The initial conditions of the system are given as: $q\left(0\right)={\left[0.1,-0.6435,\pi /2,0,0.284\right]}^T$, $\dot{q}\left(0\right)={\left[0.1,0.4,0,-0.5,0.1\right]}^T$. We chose the control parameters as follows: $k=10,\beta =1000,ϵ=0.001$. We can guarantee that the tracking errors of position and velocity as given by Equation (27) were $|e_{xa,i}|\le \frac{ϵ}{k}={10}^{-4},\left|{\dot{e}}_{xa,i}\right|\le 2ϵ=2\times {10}^{-3}$. The equation of the controlled system given by Equation (20) was numerically integrated for 20 s using ODE15s on the MATLAB platform.

Figure 2, Figure 3 and Figure 4 show the numerical simulation results for the tracking control of the SCARA-type manipulator. By using the MATLAB Robotics toolbox [32], the trajectories of the end-effector of the manipulator are shown in Figure 2. From Figure 2, we can see that the end-effector of the manipulator can move on the desired trajectory. Figure 3 shows the errors in tracking the position and the velocity of the end-effector of manipulator of the nominal system. These values are bounded within ${10}^{-4}$ and $2\times {10}^{-3}$. The required servo joint forces are shown in Figure 4, where $Q^c$ denotes the control input, and $Q^u$ denotes the additional control torques for uncertainties of the actual system.

5. Conclusions

In this paper, problems in the dynamic modeling and control of a robot manipulators system with uncertain dynamics were considered. The dynamical equations and the constraint torque forces were obtained from the Udwadia–Kalaba equation, using the additional controller to compensate uncertainties in the nominal system. No linearizations/approximations of the robot manipulators systems were made throughout, and the tracking errors were bounded. A SCARA-type manipulator was used as an application example. Numerical simulation demonstrated the simplicity and ease of implementation.