RNN Based Trajectory Control for Manipulators with Uncertain Kinematic Parameters

Abstract

Among tracking control of redundant manipulators, potential limitations such as model uncertainties and physical limitations may exist. Conventional solutions may fail when model parameters differ from nominal ones. In this chapter, a novel kinematic controller with the capability of self-adaptation is proposed to address this challenging issue. Based on the coordinate feedback, a Jacobian adaption strategy is firstly built by updating kinematic parameters online. Using Karush–Kuhn–Tucker conditions, the redundancy solution problem is then turned into a quadratic optimization one, and a recurrent neural network based controller is designed to derive the optimal solution recurrently. Theoretical analysis demonstrates the global convergence of the tracking error. Compared with existing methods, kinematic model uncertainty of the robot is allowed in this chapter, and the pseudo-inverse of Jacobian matrix is avoided, with the consideration of physical limitation in a joint framework. Numerical experiments based on Kinova JACO$_2$ show the effectiveness of the proposed controller.

2.1 Introduction

With the development of mechanics, electronics and computer technology advance, using robot manipulators is becoming popular in modern manufacturing such as welding, painting, assembling, etc [1,2,3,4]. Among these applications, tracking control of manipulators, focusing on the calculation of control actions to drive the robot to move along the user-defined trajectory in Cartesian space, is always a core problem in robot control, and has been studied intensively by researchers in recent decades.

Redundant manipulators have more degrees of freedom (DOFs) than those required to accomplish a given task [5], and have shown great potentials in enhancing robot flexibility, dexterity, and versatility, avoiding obstacles [6,7,8,9], and optimizing energy consumption [10]. However, the nonlinear function description from the joint to Cartesian space, as well as the redundancy in DOFs, makes it a challenging problem to achieve precise tracking control of redundant manipulators.

In recent decades, some results on resolving the redundancy of manipulators have been reported. In most approaches, the problem is solved at the velocity or acceleration level, namely, to derive the corresponding joint velocity or acceleration according to the trajectory description in Cartesian space. Masayuki et al. [12] propose a redundancy solution method for a S-R-S redundant manipulator at the angle level, in which analytic solutions are firstly derived, and analytical methods for joint avoidance is then considered. However, this method is effective only for robots with a specific configuration and is not scalable to manipulators with a general mechanical structure. To solve the kinematic control problem with general configurations, some controllers are proposed, including adaptive control methods [13, 14], barrier-Lyapunov-function based methods [15, 16], and Jacobian-matrix-pseudo-inverse (JMPI) methods [17,18,19]. In [17], an asymmetric barrier Lyapunov function-based method is introduced to handle the output limitation. This method consists of a full state feedback controller and an output feedback controller. Using the JMPI method, one can get the control signals in joint space according to the desired path and the pseudo-inverse of the Jacobian matrix. For a redundant manipulator, there is a null space for the Jacobian matrix [20], which is helpful to design controllers considering a secondary task. Therefore, JMPI based methods have been widely used in redundancy solutions. Galicki [21] proposes a JMPI based tracking controller, and an alternative method around the singular point is discussed. In [22], a weighted damped least-squares method is developed to calculate pseudo-inverse around the singularity, an appropriate damping factor is derived according to the minimum singular value. In [23], pseudo-inverse of Jacobian is calculated online by a Taylor-type discrete-time neural network, which is composed of T-ZNN-K and T-ZNN-U models. In [24], a special type of nonlinear function based neural network is designed for tracking control of a PA10 manipulator, and the finite-time convergence of tracking error is also analyzed.

Although the above-mentioned methods have achieved great success, those methods are afflicted with several major limitations in scenarios that require higher performance of real-time ability, accuracy, and self-adaptation. Firstly, the precise kinematic parameters are required in existing works. Describing the mapping from the joint movement in joint space to the movement of the end-effector in Cartesian space, the Jacobian matrix contains kinematic characteristics, such as configuration and kinematic parameters. For a specified robot, the configuration can be derived, but it is usually difficult to obtain accurate kinematic parameters. For example, because of the manufacturing error, different operation tools, etc., the DH parameters may differ to the reference ones in official guidebooks [25]. In this case, the Jacobian matrix based on the inaccurate parameters would cause errors in pseudo-inverse calculation and even instability of the system [26]. On the other hand, the calculation of pseudo-inverse operation is time-consuming, which would lead to a huge cost in real applications which requires pseudo-inverse calculation in every control cycle. Additionally, due to mechanical reasons, the robot manipulator is suffered from physical constraints, such as joint angular and speed limitations.

In terms of the kinematic control in the presence of model uncertainties, the real-time feedback of the end-effector enables closed-loop control for researchers. This can be done by high performance measuring devices such as high precision cameras and laser trackers [27]. In [28], based on the parameter linearization property, a robust controller is proposed, which shows semi-global stability in regulation control in fix-point control. As to the tracking control of manipulators, Hou proposes a neural network based control strategy, in which the position/orientation of the robot is described by a unit quaternion, and the network is used to learn the unknown nonlinear part of the system. One main contribution of the research is that singularities associated with three-parameter representation can be avoided. Cheah et al. propose several adaptive controllers for manipulators in different industrial applications, such as visual tracking, force tracking and trajectory tracking [30,31,32,33,34]. In [35], Chen and Zhang design a new adaptive controller in the acceleration level, the basic idea is that the Jacobian matrix is updated in realtime rather than kinematic parameters. One major drawback of the strategy is that the controller requires the actual values of end velocity and acceleration, which may contain noise in actual applications. In order to reduce the influence of noise in sensor feedback, Wang introduces a low-pass filter, and then an adaptive torque calculation controller is designed in the inner loop [36]. Xu develops a modified controller [37], in which the joint command is deigned at the acceleration level. It is verified that the controller does not require measurement of end velocity and joint acceleration. The influence of the control parameters on tracking errors and convergence rate is also discussed. The above methods mainly focus on the uncertain model parameters, and the redundancy of the manipulators is not considered. Despite the pseudo-inverse can be used instead of traditional inverse calculation of Jacobian matrix, the disadvantage of JMPI methods remains unresolved. In order to overcome the limitations based on the JMPI method, researches transform this problem into a quadratic programming problem, with the aim of obtaining an optimal solution with the specified evaluation index under the physical constraint. Physical constraints can be formulated into equation constraints or inequality constraints. Zhang et al. [38] develops a dual neural network to solve quadratic programming problems, and it is shown that this strategy is suitable for redundancy solutions. Based on the idea, a series of research is reported in eliminating the position error accumulation [39], nonconvex optimization [40], acceleration-level compliance [41], parallel robots [42] and multiple robot systems [43].

Inspired by the above literature, in this chapter, we focus on the adaptive tracking problem for redundant manipulators. The remained of this chapter is arranged as below. In Sect. 2.2, fundamental robot kinematics together with useful properties are given, we also show the control objective. In Sect. 2.3, an adaptive Jacobian method is designed by updating kinematic parameters online, and a RNN is used to achieve redundant resolution based on the estimated Jacobian matrix, convergence analysis of the tracking error in Cartesian space is also discussed. In Sect. 2.4, numerical results and comparisons are conducted on a 6DOF robot JACO$_2$. Finally, conclusions are drawn in Sect. 2.5. Before ending the introductory section, we highlight the main contributions of this chapter as below:

• As far as we know, for the first time, this chapter proposes an RNN based controller via kinematic regressing for redundant manipulator subject to model uncertainties.

• Using the Lyapunov theory, the convergence of tracking error is proved in the case of uncertain parameters.

• In this chapter, there is no need to calculate the pseudo-inverse of the Jacobian matrix, which can avoid singularity problems effectively, and also reduce computational load.

• The minimum velocity solution is derived by the proposed control scheme, thus the safety of both humans and robots can be also guaranteed.

2.2 Problem Formulation and Existing Results

2.2.1 Robot Kinematics

Without loss of generality, we consider serial robot manipulators in this chapter. The kinematic model for robot manipulators is described as follows:

$$\begin{aligned} f(\theta (t))=x(t), \end{aligned}$$

(2.1)

where $\theta (t)\in \mathbb {R}^n$ represents the vector of the joint angles at time t, and $x(t)\in \mathbb {R}^m$ represents the Cartesian coordinate vector of the end effector. For a specific robot manipulator, $f(\bullet ): \mathbb {R}^n\rightarrow \mathbb {R}^m$ is used to describe the forward kinematics from joint space to Cartesian space, which is a continuous nonlinear mapping containing kinematic parameters and structure information.

By differentiating x(t) with respect to time t, we can get the relationship between Cartesian velocity $\dot{x}(t)\in \mathbb {R}^m$ and joint velocity (or joint control signal) $\dot{\theta }(t)\in \mathbb {R}^n$ as follows:

$$\begin{aligned} J(\theta (t),a_k)\dot{\theta }(t)=\dot{x}(t), \end{aligned}$$

(2.2)

with $J(\theta (t),a_k)=\partial {f(\theta (t),a_k)/\partial \theta (t)}$ being the Jacobian matrix, and $a_k\in \mathbb {R}^l$ denotes the vector of kinematic parameters.

Once the physical structure of manipulator is determined, its kinematic equation (2.2) satisfies the following linearization property [43], which describes the relationship between the robot’s end velocity and its kinematic parameters:

$$\begin{aligned} J(\theta (t),a_k)\dot{\theta }(t)=Y_k(\theta (t),\dot{\theta }(t))a_k, \end{aligned}$$

(2.3)

where $Y_k(\theta (t),\dot{\theta }(t))\in \mathbb {R}^{m\times l}$ is called kinematic regressor matrix. Remarkable that $Y_k(\theta (t),\dot{\theta }(t))$ is the function of $\theta (t)$ and $\dot{\theta }(t)$, and has no relation with $a_k$.

2.2.2 Control Objective

In this chapter, we consider the task space tracking problem for redundant manipulators, where precise values of kinematic parameters are unavailable. The measurable states are joint angles $\theta (t)$ and the end coordinates x(t). The desired Cartesian path $x_{\text{ d }}(t)\in \mathbb {R}^m$ and its time derivative $\dot{x}_{\text{ d }}(t)$ are accessible, both $x_{\text{ d }}(t)$ and $\dot{x}_{\text{ d }}(t)$ are bounded. The nominal value of the kinematic parameter vector is also available, which is denoted as $a_k^n$.

The control objective is to generate joint velocity command in real-time, e.g., designing $\dot{\theta }(t)$ to drive the end-effector of a redundant robot to track $x_{\text{ d }}(t)$ , in the sense that $f(\theta (t))=x(t)\rightarrow x_{\text{ d }}(t)$. During the whole tracking process, velocity of every joint $\dot{\theta }_i(t)$ should not exceed its limits $\dot{\theta }_{\text{ min }}^i$, $\dot{\theta }_{\text{ max }}^i$.

2.3 Main Results

In this section, we will show the detailed process of the controller design. When controlling a redundant robot, one important problem is to make use of its flexibility, such as avoiding obstacles, optimizing energy consumption, and avoiding Singularities, etc. In this chapter, when the kinematic controller is designed to achieve task-space tracking in the presence of physical uncertainties, we consider the energy-saving problem at speed level. Therefore, the secondary task in set to minimize the velocity norm $u^\text {T}u$. The control strategy consists of three parts: a position controller in the outer-loop, a Jacobian adaption part which is capable of handling kinematic uncertainties online, and an RNN which is used to solve the redundancy resolution problem. The stability of the closed-loop system will be also discussed.

2.3.1 Position Controller

Firstly, a precise measurement of actual coordinate x in real-time t is taken to build the closed-loop system. The difference between the desired path and the corresponding feedback can be defined as

$$\begin{aligned} e(t)=x_{\text{ d }}(t)-x(t). \end{aligned}$$

(2.4)

In order to make e(t) converge to 0, by using the zeroing dynamics [53], the derivative of e(t) is designed as

$$\begin{aligned} \dot{e}(t)=-ke(t), \end{aligned}$$

(2.5)

with $k>0$ being a positive constant scaling the convergence rate of e(t). Combining (2.4) and (2.5) yields

$$\begin{aligned} \dot{x}(t)=\dot{x}_{\text{ d }}(t)+k(x_{\text{ d }}(t)-x(t)). \end{aligned}$$

(2.6)

Let $\dot{\theta }(t)=u(t)$, according to (2.6), if u(t) is properly designed to make the robot’s end-effector move at a speed of $\dot{x}(t)$, in the sense that $\dot{x}(t)=J(\theta (t),a_k)u(t)$, the tracking error e(t) in task-space would convergence to zero exponentially.

Fig. 2.1

Framework of the proposed scheme for redundant manipulators with uncertain kinematics, in which the neural control algorithm includes three interactive modules, i.e., position control module, parameter identification module, and redundancy resolution module

When $a_k$ is unknown, the precise Jacobian matrix described in (2.2) is unavailable. The redundancy solution can not be achieved by using $J(\theta , a_k)$. Therefore, we use the estimate Jacobian $J(\theta (t),\hat{a}_k)$ by replacing the unknown parameters $a_k$ in $J(\theta (t),a_k)$ with its estimate $\hat{a}_k$, and the initial value of $\hat{a}_k$ is set as $\hat{a}_k(0)=a_k^n$, the estimate error is defined as $\tilde{a}_k=\hat{a}_k-a_k$. Using $J(\theta (t),\hat{a}_k)$ and the control signal $\dot{\theta }(t)$, we can estimate the velocity of the end-effector as

$$\begin{aligned} \hat{\dot{x}}(t)=J(\theta (t),\hat{a}_k(t))u(t). \end{aligned}$$

(2.7)

Remarkable that the linearization property described in (2.3) still holds for estimated $\hat{a}_k$:

$$\begin{aligned} J(\theta (t),\hat{a}_k(t))u(t)=Y_k(\theta (t),u(t))\hat{a}_k(t), \end{aligned}$$

(2.8)

this property will be used in the following stability proof. The adaptive Jacobian method by updating its kinematic parameters $\hat{a}_k$ is thus developed as

$$\begin{aligned} \dot{\hat{a}}_k(t)=-\varGamma _1Y_k^{\text{ T }}(\theta (t),u(t))e(t), \end{aligned}$$

(2.9)

where $\varGamma _1\in \mathbb {R}^{l\times l}$ is a diagonal positive definite matrix, e(t) is the tracking error in Cartesian space as defined in (2.4), and u(t) is the bounded joint speed vector satisfying $J(\theta (t),\hat{a}_k)u(t)=\dot{x}(t)$, which will be designed later. Unless otherwise specified, $J(\theta (t),\hat{a}_k)$ is simplified as $\hat{J}$.

Remark 2.1

Figure 2.1 gives a brief framework of the proposed control scheme for redundant manipulators with uncertain kinematic parameters. The desired trajectory of the end-effector is specified by $x_{\text{ d }}(t)$ and $\dot{x}_{\text{ d }}(t)$. The desired trajectory together with feedback x(t) are fed into the position controller (2.6). The tracking error e(t) and joint speed $\dot{\theta }(t)$ are used to learn the kinematic parameters online by identifier (2.9). According to the output of the position controller, identified parameter $\hat{a}_k$, feedback of the manipulator and physical limits, an RNN based controller is used to achieve the redundancy solution problem.

2.3.2 Redundant Solution Using RNN

In this subsection, we focus on redundancy resolution problem based on Jacobian adaption method introduced in Sect. 2.3.1. The main purpose of redundancy resolution is to find an optimal joint speed u(t) to make equation $J(\theta (t),a_k)u(t)=\dot{x}_{\text{ d }}(t)+k|e(t)|^{\rho }\text {sgn}(e(t))$ hold, at the same time, a secondary task can be also achieved. The redundancy resolution problem can be converted into a quadratic optimization one with specified constraints. To minimize the kinetic energy of the robot, we select velocity norm $u^{\text{ T }}u={\dot{\theta }}^{\text{ T }}\dot{\theta }$ as an object function to be optimized, the joint range $\theta _{\text {min}}^i\le \theta _i\le \theta _{\text {max}}^i$ and joint speed limits $\dot{\theta }_{\text {min}}^i\le \dot{\theta }_i\le \dot{\theta }_{\text {max}}^i$ are regarded as inequality constrains. Because $J(\theta ,a_k)$ is unavailable, we use $\hat{J}$ instead of $J(\theta ,a_k)$, and rewrite $\dot{x}=b_0$. Then the redundancy resolution problem is reformulated as the following quadratic optimization formulations:

$$\begin{aligned} \text {min}~~&u^{\text{ T }}u,\end{aligned}$$

(2.10a)

$$\begin{aligned} \text {s.t.}~~~&b_0=\hat{J}u,\end{aligned}$$

(2.10b)

$$\begin{aligned} ~~~~~~&u\in \varOmega , \end{aligned}$$

(2.10c)

where $\varOmega =\{u\in \mathbb {R}^n\arrowvert u_{\text {min}}^i\le u_i\le u_{\text {max}}^i\}$ is a convex set describing the physical constraints, where $u_{\text {min}}^i=\text {max}\{\alpha (\theta _{\text{ min }}^i-\theta _i),\dot{\theta }_{\text {min}}^i\}$, $u_{\text {max}}^i=\text {min}\{\alpha (\theta _{\text{ max }}^i-\theta ),\dot{\theta }_{\text {max}}^i\}$, $\alpha >0$ is a positive constant. The convex set ensures the boundedness of both joint angles and speed [44]. According to the Karush−Kuhn−Tucker condition [45], an equivalent description of the optimal solution to the quadratic optimization as shown in (2.10) is described as

$$\begin{aligned} u&=P_{\varOmega }(u-\partial L/\partial u),\end{aligned}$$

(2.11a)

$$\begin{aligned} b_0&=\hat{J}u, \end{aligned}$$

(2.11b)

where $P_{\varOmega }(\bullet )$ is a projection operation to the set $\varOmega$, $P_{\varOmega }(x)=\text {argmin}_{y\in \varOmega }||y-x||$, and $L=L(u,\lambda )$ is a Lagrange function defined as $L(u,\lambda )=u^{\text{ T }}u/2+\lambda ^{\text{ T }}(b_0-\hat{J}u)$, where $\lambda \in \mathbb {R}^m$ is a Lagrange multiplier corresponding to the equality constraint.

Note that the difference between $\hat{J}$ and J would lead to extra error, which will result in tracking failure. To resolve the quadratic optimization problem (2.11), we are ready to present the RNN based controller together with the updating kinematic parameters online:

$$\begin{aligned} \varepsilon \dot{u}&=-u+P_{\varOmega }(-\hat{J}^{\text{ T }}\lambda ),\end{aligned}$$

(2.12a)

$$\begin{aligned} \varepsilon \dot{\lambda }&=\hat{J}u-b_0,\end{aligned}$$

(2.12b)

$$\begin{aligned} \dot{\hat{a}}_k&=-\varGamma _1Y_k^{\text{ T }}(\theta ,u)e, \end{aligned}$$

(2.12c)

where $\varepsilon$ is a positive factor scaling the convergence of RNN. The proposed control scheme is shown in Algorithm 2.3.2.

Remark 2.2

It is worth pointing that although the proposed RNN in (2.12a) and (2.12b) looks similar to existing ones (e.g., [46, 47]), the modification is very meaningful. The proposed RNN is capable of handling kinematic uncertainties. When the kinematic parameters $a_k$ in known, $\hat{J}$ is equal to J, (2.12a) and (2.12b) have the same expression with traditional ones, which shows that a known parameter case is only a special form of our control scheme, thus the proposed RNN is more general. The proposed control scheme offers an important expansion to model uncertainties, which is of universal significance in engineering applications.

Remark 2.3

Using the proposed RNN based controller, the control command u(t) can be derived according to (2.12a), which is capable of optimizing $u^\text{ T }u$, meanwhile, the projection operation $P_{\omega }(\bullet )$ handles inequality constraints. (2.12b) plays an important role in task-space tracking. By referring to (2.12c), we update the Jacobian indirectly by renewing its kinematic parameters online based on the property (2.3), which is different with other Jacobian adaption mathods (e.g., [48]), where joint acceleration is required. The necessary values of our updating law are joint angle $\theta$, joint speed u and tracking error e, therefore, the proposed control strategy can be realized easily.

2.3.3 Convergence Analysis

In this part, we conduct theoretical analysis on the convergence of tracking error under the RNN based tracking controller (2.12a) and (2.12b) along with the kinematic parameter updating law described in (2.12c).

Firstly, two Lemmas are offered as below, which will be used in the proof process of convergence analysis.

Lemma 2.1

For any closed convex set $\Lambda \in \mathbb {R}^p$, $(x-P_{\Lambda }(x))^{\text{ T }}(P_{\Lambda }(x)-y)\ge 0$, $\forall y\in \Lambda$, $\forall x\in \mathbb {R}^p$, and the equality holds only if $x\in \Lambda$ [49].

Lemma 2.2

For any closed convex set $\Lambda \in \mathbb {R}^p$, $(x-P_{\Lambda }(x))^{\text{ T }}(x-y)\ge 0$, $\forall y\in \Lambda$, $\forall x\in \mathbb {R}^p$, and the equality holds only if $x\in \Lambda$ [47].

Based on Lemma 1 and 2, we can obtain the following theorem about convergence of tracking error under the proposed redundancy resolution scheme (2.12).

Theorem 2.1

The control error e(t) defined in (2.4) for a redundant manipulator globally converges to 0, provided the RNN based redundancy resolution (2.12a) and (2.12b), along with the kinematic adaptation law (2.12c).

Proof: The convergence analysis includes two parts. Firstly, we will prove that the output u of proposed RNN (2.12a), (2.12b) would reach the optimal solution of (2.11). Secondly, we will show the convergence of tracking error e along with the adaptation law (2.12c). Note that the proof bears similarity to that with known parameters, but the extra dynamics on parameter adaptation makes it necessary to analyze the joint stability, which constructs the main difference of this proof from existing works.

Part I. By defining $\xi =[u^{\text{ T }},\lambda ^{\text{ T }}]^{\text{ T }}$, controller (2.12a), (2.12b) can be reformulated as

$$\begin{aligned} \epsilon \dot{\xi }=-\xi +P_{\bar{\varOmega }}(\xi -R(\xi )), \end{aligned}$$

(2.13)

where $\bar{\varOmega }=\{(u,\lambda )|u\in \varOmega ,\lambda \in \mathbb {R}^m\}$, and $R(\xi )=[u-{\hat{J}}^{\text{ T }}\lambda , -b_0+\hat{J}u]^{\text{ T }}$. Define $\nabla R=\partial R(\xi )/\partial \xi$, we have

$$\begin{aligned} \nabla R=\begin{bmatrix} I&{}&{}-{\hat{J}}^{\text{ T }}\\ {\hat{J}}&{}&{}0 \end{bmatrix}, \end{aligned}$$

where I is a n-dimensional identity matrix, and $\nabla R\in \mathbb {R}^{(m+n)\times (m+n)}$ is a skew symmetric matrix. The transpose matrix of $\nabla R$ is defined as $\nabla ^{\text{ T }}R$. Remarkable that $\nabla R$ satisfies the following positive semi-definite property:

$$\begin{aligned} y^{\text{ T }}\nabla Ry=y^{\text{ T }}(\nabla R+\nabla ^{\text{ T }} R)y/2\ge 0,~\forall y\in \mathbb {R}^{m+n}. \end{aligned}$$

(2.14)

This property will be used later. Define the following Lyapunov function candidate as

$$\begin{aligned} V_1=||\xi -P_{\bar{\varOmega }}(\xi )||_2^2/2. \end{aligned}$$

(2.15)

It is obvious that $V_1=0$ if and only if $\xi \in \bar{\varOmega }$. According to the result of reference in E.9, $\partial ||\xi -P_{\bar{\varOmega }}(\xi )||_2^2/\partial \xi =2(\xi -P_{\bar{\varOmega }}(\xi ))$. Differentiating $V_1$ with respect to time and substituting (2.13) yield:

$$\begin{aligned} \dot{V}_1=&(\xi -P_{\bar{\varOmega }}(\xi ))^{\text{ T }}\dot{\xi }\nonumber \\ =&-(\xi -P_{\bar{\varOmega }}(\xi ))^{\text{ T }}(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))/\epsilon . \end{aligned}$$

(2.16)

There is no doubt that $P_{\bar{\varOmega }}(\xi -R(\xi ))\in \bar{\varOmega }$, according to Lemma 2, $\forall \xi \in \mathbb {R}^{m+n}$ satisfies the inequality $(\xi -P_{\bar{\varOmega }}(\xi ))^{\text{ T }}(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))\ge 0$. Then we have $\dot{V}_1\le 0$ because $\epsilon >0$, and $\dot{V}_1=0$ only if $\xi \in \bar{\varOmega }$. Based on to LaSalle’s invariance principle [50], it can be proved that $\xi$ gradually converges into $\bar{\varOmega }$, which indicates u converges into $\varOmega$ , the boundedness of joint angles and speed is thus guaranteed. Note that the equilibrium point $\xi ^*$ satisfies

$$\begin{aligned} \xi ^*=P_{\bar{\varOmega }}(\xi ^*-R(\xi ^*)). \end{aligned}$$

(2.17)

According to definition 1 and Lemma 1 in [51], $\xi ^*$ satisfies the following property

$$\begin{aligned} (y-\xi ^*)^{\text{ T }}R(\xi ^*)\ge 0,~~\forall y\in \bar{\varOmega }. \end{aligned}$$

(2.18)

Define function $V_2$ as

$$\begin{aligned} V_2=&(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}R(\xi )+\Vert \xi -\xi ^*\Vert _2^2/2\nonumber \\&-||\xi -P_{\bar{\varOmega }}(\xi -R(\xi ))||_2^2/2+V_1. \end{aligned}$$

(2.19)

Some mathematical calculations on the first and third items of the definition (2.19) give

$$\begin{aligned}&(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}R(\xi )-\Vert \xi -P_{\bar{\varOmega }}(\xi -R(\xi ))\Vert _2^2/2~~~~\nonumber \\ \ge&(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}R(\xi )-\Vert \xi -P_{\bar{\varOmega }}(\xi -R(\xi ))\Vert _2^2~~~~\nonumber \\ =&(\xi -R(\xi )-P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}(P_{\bar{\varOmega }}(\xi -R(\xi ))-\xi ). \end{aligned}$$

(2.20)

Noticing that $\xi$ would gradually converge into the convex set $\bar{\varOmega }$, then we get $\xi \in \bar{\varOmega }$. According to Lemma 1, inequality $(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\cdot (P_{\bar{\varOmega }}(\xi -R(\xi ))-\xi )\ge 0$ holds for any $\xi -R(\xi )\in \mathbb {R}^{m+n}$. Recalling the definition of $V_2$, we have

$$\begin{aligned} V_2\ge ||\xi -\xi ^*||_2^2/2+V_1. \end{aligned}$$

(2.21)

Thus $V_2$ is a Lyapunov function candidate. Differentiating $V_2$ with respect to time and combining (2.13) yields:

$$\begin{aligned} \dot{V}_2=&(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\nabla R\dot{\xi }+{\dot{\xi }}^{\text{ T }}R(\xi )+(\xi -\xi ^*)^{\text{ T }}\dot{\xi }\nonumber \\&-(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\dot{\xi }+\dot{V}_1\nonumber \\ =&-(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\nabla R(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))/\epsilon \nonumber \\&-(\xi -R(\xi )-P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}(P_{\bar{\varOmega }}(\xi -R(\xi ))-\xi ^*)/\epsilon \nonumber \\&-(\xi -\xi ^*)^{\text{ T }}R(\xi )/\epsilon +\dot{V}_1. \end{aligned}$$

(2.22)

Remarkable that $\xi ^*\in \bar{\varOmega }$ , according to Lemma 1, inequality $(\xi -R(\xi )-P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}(P_{\bar{\varOmega }}(\xi -R(\xi ))-\xi ^*)\ge 0$ holds for any $\xi -R(\xi )\in \mathbb {R}^{m+n}$. Using (2.14), we have $-(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\nabla R(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))/\epsilon \le 0$ since $\epsilon >0$. According to mean value theorem, we have

$$\begin{aligned} R(\xi )-R(\xi ^*)=\nabla R(\xi ')(\xi -\xi ^*), \end{aligned}$$

(2.23)

where $\xi '\in [\xi ,\xi ^*]$. After some mathematical calculations on the following polynomials and substituting (2.23), we have

$$\begin{aligned}&(\xi -\xi ^*)^{\text{ T }}R(\xi )\nonumber \\ =&(\xi -\xi ^*)^{\text{ T }}\nabla R(\xi ')(\xi -\xi ^*)+(\xi -\xi ^*)^{\text{ T }}R(\xi ^*). \end{aligned}$$

(2.24)

Using the properties (2.14) and (2.18), we have $(\xi -\xi ^*)^{\text{ T }}\nabla R(\xi ')(\xi -\xi ^*)\ge 0$ and $(\xi -\xi ^*)^{\text{ T }}R(\xi ^*)\ge 0$, then

$$\begin{aligned} (\xi -\xi ^*)^{\text{ T }}R(\xi )\ge 0. \end{aligned}$$

(2.25)

Combining inequalities (2.16), (2.24) and (2.25) yields $\dot{V}_2\le 0$, and $\dot{V}_2=0$ only if $\xi \in \bar{\varOmega }$, which indicates $(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}\nabla R(\xi -P_{\bar{\varOmega }}(\xi -R(\xi )))=0$, $(\xi -R(\xi )-P_{\bar{\varOmega }}(\xi -R(\xi )))^{\text{ T }}(P_{\bar{\varOmega }}(\xi -R(\xi ))-\xi ^*)=0$ and $(\xi -\xi ^*)^{\text{ T }}R(\xi )=0$. From (2.24), we get $(\xi -\xi ^*)^{\text{ T }}\nabla R(\xi ')(\xi -\xi ^*)$ and $(\xi -\xi ^*)^{\text{ T }}R(\xi ^*)=0$. Notable that $\xi =\xi ^*$ is the solution of the above equations. Based on LaSalle’s invariance principle, we arrive at a conclusion that $\xi$ would gradually reach its equilibrium point $\xi ^*$, i.e., u(t) would converge to its optimal solution of redundancy resolution problem (2.10).

Part II. Consider the Lyapunov function candidate

$$\begin{aligned} V_3=e^{\text{ T }}e/2+{\tilde{a}_k}^{\text{ T }}\varGamma _1^{-1}\tilde{a}_k/2. \end{aligned}$$

(2.26)

Differentiating $V_3$ with respect to time and substituting (2.4), (2.8) and (2.9), we have

$$\begin{aligned} \dot{V}_3=&e^{\text{ T }}(\dot{x}_{\text{ d }}-\dot{x})+{\tilde{a}_k}^{\text{ T }}\varGamma _1^{-1}\dot{\tilde{a}}_k\nonumber \\ =&e^{\text{ T }}(\dot{x}_{\text{ d }}-J(\theta ,a_k)u+(1-1)k|e|^{\rho }\text {sgn}(e)+{\tilde{a}_k}^{\text{ T }}\varGamma _1^{-1}\dot{\hat{a}}_k\nonumber \\ =&e^{\text{ T }}(b_0-Y_k(\theta ,u)(\hat{a}_k-\tilde{a}_k)-k|e|^{\rho }\text {sgn}(e))-{\tilde{a}_k}^{\text{ T }}Y_k^{\text{ T }}(\theta ,u)e\nonumber \\ =&e^{\text{ T }}(b_0-\hat{J}u+Y_k(\theta ,u)\tilde{a}_k)-k|e|^{\rho +1}-{\tilde{a}_k}^{\text{ T }}Y_k^{\text{ T }}(\theta ,u)e. \end{aligned}$$

(2.27)

As proved above, using the neural network (2.12), $u^{\text{ T }}u$ will be minimized under the constraints $b_0=\hat{J}u$ and $u\in \varOmega$. Notable that ${\tilde{a}_k}^{\text{ T }}Y_k^{\text{ T }}(\theta ,u)e$ is a scalar value, we have ${\tilde{a}_k}^{\text{ T }}Y_k^{\text{ T }}(\theta ,u)e=e^{\text{ T }}Y_k(\theta ,u)\tilde{a}_k$. Then (2.27) can be rewritten as

$$\begin{aligned} \dot{V}_3=&e^{\text{ T }}Y_k(\theta ,u)\tilde{a}_k-k|e|^{\rho +1}-{\tilde{a}_k}^{\text{ T }}Y_k^{\text{ T }}(\theta ,u)e\nonumber \\ =&-k|e|^{\rho +1}\le 0. \end{aligned}$$

(2.28)

Then we have $e=x_{\text{ d }}-x$ is bounded. Taking the time derivative of $\dot{V}_3$, we have:

$$\begin{aligned} \ddot{V}_3=&-k(\rho +1)|e|^{\rho }\text {sgn}(e)\dot{e}\nonumber \\ =&-k(\rho +1)|e|^{\rho }\text {sgn}(e)(\dot{x}_{\text{ d }}-J(\theta ,a_k)u). \end{aligned}$$

(2.29)

Since $J(\theta ,a_k)$ is composed of trigonometric functions of $\theta$ and kinematic parameters $a_k$, $J(\theta ,a_k)$ is bounded. $\dot{x}_{\text{ d }}$ is also bounded. As illustrated in part I, u is bounded, thus $\ddot{V}_3$ is guaranteed to be bounded. Using Barbalat’s lemma [52], we have $\dot{V}_3\rightarrow 0$ as $t\rightarrow \infty$. Then $e\rightarrow 0$ as $t\rightarrow \infty$. This completes the Proof.$\Box$

Remark 2.4

The convergence analysis shows the stability of the proposed control strategy. The tracking error would globally convergence to 0. The proof also illustrates that the control command u(t) is ensured $u(t)\in \varOmega$, $\forall t\ge 0$, provided $u(0)\in \varOmega$, the boundedness of joint speed is thus guaranteed all the time.

2.4 Illustrative Examples

Fig. 2.2

The physical structure of JACO$_2$

Table 2.1 DH parameters of the Kinova JACO$_2$ robot manipulator

Fig. 2.3

Results of regulation control on JACO$_2$ to a fixed point $[0.3, 0.4, 0.4]\text{ m }$ in the Cartesian space. a Motion trajectory of end effector (red curve) and the corresponding incremental configurations of JACO$_2$. b Error-time curve along three directions. c Angle-time curve of 6 joints. d Command-time curve of joint velocity u

Fig. 2.4

Results when JACO$_2$ tracks a given circle in Cartesian space. a Motion trajectory of end effector (red curve) and the corresponding incremental configurations of JACO$_2$. b Error-time curve along three directions. c Angle-time curve of 6 joints. d Command-time curve of joint velocity u. e The first Cartesian velocity input $b_0$(x-axis direction) described by (2.6) and the corresponding output $\hat{J}u(1)$. f The second Cartesian velocity input $b_0$(y-axis direction) described by (2.6) and the corresponding output $\hat{J}u(2)$. g The third Cartesian velocity input $b_0$(z-axis direction) described by (2.6) and the corresponding output $\hat{J}u(3)$. h The Euclidean norm of the manipulator’s joint velocity

2.4.1 Numerical Setup

We consider the position tracking problem in task space, then JACO$_2$ can thus be regarded as a functional redundant manipulator. The architecture of JACO$_2$ is shown in Fig. 2.2, and the DH parameters are shown in Table 2.1. Noticing that the last 3 joints of JACO$_2$ do not intersect at a single point, these joints cannot be simplified as spherical joint, therefore the configuration of JACO$_2$ is more general than other 6DOF manipulators, e.g., the PUMA 560. The initial state of joint position vector $\theta (0)$ is randomly set as $[0.5,0,1.5,0,0,0]^{\text{ T }}$rad, and the initial joint speed u(0) is selected to be zero. The nominal values of kinematic parameters are selected as $\hat{a}_k(0)=[0.25,0.2,0,-0.2,-0.1,-0.2]^{\text{ T }}\text{ m }$. The set $\varOmega$ describing joint speed limits are set to be $[-2,2]^6\text {rad/s}$. The control gain k is set to be 8, and the gain matrix $\varGamma _1$ is selected as 0.5I, where I is a 6-dimensional identity matrix (Fig. 2.4).

2.4.2 Fixed-point Regulation

In order to verify the proposed tracking strategy, a set value adjustment experiment is carried out. Set the desired fixed position of the end-effector to $[0.3,0.4,0.4]^{\text{ T }}\text{ m }$, select the zoom factor to be $\epsilon =0.008$. The simulation results are shown in Fig. 2.3. The adjustment error converges to zero, and the convergence time is about $0.5\text{ s }$, as shown in Fig. 2.3b. Therefore, the joint angle $\theta$ reaches a set of constant values, as shown in Fig. 2.3c. The combined velocity u(t)reaches the limit at the beginning of the simulation, making the end-effector move toward the target as fast as possible, and slowing down rapidly when the end-effector approaches the target. During the whole simulation process, u is guaranteed not to exceed its limit, as shown in Fig. 2.3d. Finally, as shown in Fig. 2.3a, the robot successfully reaches the fixed point under the proposed control scheme.

2.4.3 Circular Trajectory

In this section, tracking of a smooth circular trajectory using the control scheme is carried out. The end effector of JACO$_2$ is expected to move at an angular speed of $0.5\text{ rad/s }$ along a circular trajectory. The desired circle is centered at $[0.3,0.3,0.3]^{\text{ T }}$ with a radius of $0.1732\text{ m }$, and has a revolute angle of $45^{\circ }$ around the x-axis. The scaling coefficient is selected as $\epsilon =0.008$ The convergence time is about $0.5\text{ s }$, which is similar to the regulation case. As shown in Fig. 2.4d, when the simulation begins, the robot moves at the maximum speed when the tracking error is big (Fig. 2.3a), which makes the end effector move close to the desired circle. Then the robot moves at a low speed periodically, and correspondingly, the joint angle $\theta$ changes with the same frequency (Fig. 2.4b), meanwhile, the tracking error is already close to zero (Fig. 2.4a), which means that the robot has successfully tracked the desired circular trajectory with time. According to (2.6), the reference speed vector of the end-effector $b_0$ can be derived, and its components along $x-$, $y-$, and $z-$ directions are shown as blue lines in Fig. 2.4e–g, in which red lines represent the corresponding values of $\hat{J}u$. The red lines quickly converge to blue ones, demonstrating that the proposed control strategy is able to track the given trajectory under kinematic uncertainties. The joint velocity norm $||u||_2^2$ is shown in Fig. 2.4h.

Fig. 2.5

Results when JACO$_2$ tracks a square trajectory in Cartesian space. a Motion trajectory of end effector (red curve) and the corresponding incremental configurations of JACO$_2$. b Error-time curve along three directions. c Angle-time curve of 6 joints. d Command-time curve of joint velocity u. e The first Cartesian velocity input $b_0$(x-axis direction) described by (2.6) and the corresponding output $\hat{J}u(1)$. f The second Cartesian velocity input $b_0$ (y-axis direction) described by (2.6) and the corresponding output $\hat{J}u(2)$. g The third Cartesian velocity input $b_0$(z-axis direction) described by (2.6) and the corresponding output $\hat{J}u(3)$. h The Euclidean norm of the manipulator’s joint velocity

2.4.4 Square Trajectory

In this section, the JACO$_2$ is used to track a square trajectory. The corners of the desired square in the Cartesian space are set to be $[0.3,0.4,0.4]^{\text{ T }}$, $[0.4,0.3,0.4]^{\text{ T }}$, $[0.3,0.2,0.2]^{\text{ T }}$,and $[0.2,0.3,0.3]^{\text{ T }}$. The motion period is $12.56\text{ s }$. The velocity norm of desired path $||\dot{x}_{\text{ d }}(t)||$ keeps constant, which means that the expected velocity of end effector $\dot{x}_{\text{ d }}(t)$ remains constant between two adjacent vertices, while $\dot{x}_{\text{ d }}(t)$ changes discontinuously at the four corners. The scaling coefficient is selected as $\epsilon =0.008$. Numerical results are shown in Fig. 2.5. In the initial stage, the tracking error approaches zero over time after a short transient state, at the same time, the joint speed remains within the set $\varOmega$ at all times (Fig. 2.5d). The output of the position controller (2.6) and the resulting responses under the proposed control scheme along the $x-$, $y-$, and $z-$ directions are shown in Fig. 2.5e–g. The red lines converge to blue ones quickly both at the beginning of simulation and after discontinuous change of the desired velocity, and the joint speed also switches at these moments, as shown in Fig. 2.5d. As a result, there exist vibration on the error curve at time $t=3.14$, 6.28, 9.24, 12.56, 15.7, 18.8 s, with the maximum value of $[4\times 10^{-3},2\times 10^{-3},1.5\times 10^{-3}]^{\text{ T }}\text{ m }$. The joint velocity norm $||u||_2^2$ is shown in Fig. 2.5h (Table 2.2).

Table 2.2 Comparisons among different tracking controllers on manipulators

2.4.5 Comparison

In this section, we compare the proposed method with the performance of the existing redundant robot tracking controller as shown in Table 2.2. JMPI [28, 35] and RNN policy-based tracking controller [39, 40, 61, 62]. In the reference [28, 35] and our study, the exact kinematic model of the robot is not needed, which can be used to solve the kinematic uncertainty problem.The controllers proposed [28, 35, 40, 61] are calculated according to the speed level, while the controllers proposed [39, 62] are designed according to the acceleration level.In this chapter, we develop a speed-level controller. The controller obtains the control command in [28, 35] by computing the pseudo-inverse of the jacobian. These strategies cannot be used when the robot is in a singular configuration. Although DLS [22] and other improved methods are introduced, the convergence of tracking error of singular configuration cannot be guaranteed, and its physical limitations are not considered. In addition to referencing [62], the initial position of the end-effector can be set randomly in the controller, which needs to be on the desired path when referencing [62]. The controllers kin [39, 40, 62] based on RNN can guarantee the boundedness of the control command. The three controllers can track the time-varying trajectory, but the position adjustment fails in [62]. In summary, our controller can achieve stable tracking under both regulation and path tracking, and it does not need the accurate kinematic model and pseudo-inverse calculation of the Jacobian matrix, so it has good flexibility and adaptability to the uncertain environment.

2.5 Summary

This chapter studies the kinematic control of redundant robots with uncertain kinematics. A dynamic neural network is proposed to solve the redundancy problem by using an adaptive motion identifier to learn motion parameters online. The interaction between the adaptive online identifier and the neural controller makes it a nonlinear coupled system. The global convergence of tracking error is verified by the Lyapunov theory. Numerical experiments and comparisons based on JACO$_2$ robot arm demonstrate the effectiveness of the algorithm and its superiority over existing algorithms. This method is combined with RNN to realize static and dynamic task space tracking. The pseudo-inverse computation of the Jacobian matrix is avoided and the real-time performance of the controller is guaranteed. The boundedness of joint speed can also protect the robot and improve safety performance. Before concluding this chapter, it is worth pointing out that this is the first dynamic neural model of motion control for a manipulator with adaptive redundancy based on kinematic regression, with demonstrable convergence and guaranteed performance limits.

Appendix

According to (2.3), the analytical expression of JACO$_2'$s kinematic regressor matrix $Y_k\in \mathbb {R}^{3\times 6}$ is given as follows.

$Y_{11}=0$,

$Y_{12}=-\dot{\theta }_2c_1c_2+\dot{\theta }_1s_1s_2$,

$Y_{13}=-\dot{\theta }_1c_1$,

$Y_{14}=\dot{\theta }_2(c_1c_2c_3+c_1s_2s_3)+\dot{\theta }_1(c_2s_1s_3-c_3s_1s_2)-\dot{\theta }_3(c_1c_2c_3+c_1s_2s_3)$,

$Y_{15}=(\dot{\theta }_1((\sqrt{3}c_1c_4)/2+(\sqrt{3}s_4(s_1s_2s_3+c_2c_3s_1))/2+(c_2s_1s_3)/2-(c_3s_1s_2)/2)-\dot{\theta }_4((\sqrt{3}s_1s_4)/2{+}(\sqrt{3}c_4(c_1c_2c_3{+}c_1s_2s_3))/2){+}\dot{\theta }_2((c_1c_2c_3+c_1s_2s_3)/2){-}(\sqrt{3}s_4(c_1c_2s_3-c_1c_3s_2))/2+(c_1s_2s_3)/2) -\dot{\theta }_3((c_1c_2c_3)/2-(\sqrt{3}s_4(c_1c_2s_3-c_1c_3s_2))/2)$,

$Y_{16}=(\dot{\theta }_5((\sqrt{3}c_5(s_1s_4+c_4(c_1c_2c_3+c_1s_2s_3)))/2+(\sqrt{3}s_5((c_4s_1)/2-(s_4(c_1c_2c_3+c_1s_2s_3))/2+(\sqrt{3}(c_1c_2s_3-c_1c_3s_2))/2))/2)-\dot{\theta }_4((\sqrt{3}s_1s_4)/4-(\sqrt{3}c_5((s_1s_4)/2+(c_4(c_1c_2c_3+c_1s_2s_3))/2))/2-(\sqrt{3}s_5(c_4s_1-s_4(c_1c_2c_3+c_1s_2s_3)))/2+(\sqrt{3}c_4(c_1c_2c_3+c_1s_2s_3))/4)+\dot{\theta }_1((\sqrt{3}c_1c_4)/4+ (\sqrt{3}s_5(c_1s_4-c_4(s_1s_2s_3+c_2c_3s_1)))/2-(\sqrt{3}c_5((c_1c_4)/2-(\sqrt{3}(c_2s_1s_3-c_3s_1s_2))/2+(s_4(s_1s_2s_3+c_2c_3s_1))/2))/2+(\sqrt{3}s_4(s_1s_2s_3+c_2c_3s_1))/4+(c_2s_1s_3)/4- (c_3s_1s_2)/4)+\dot{\theta }_2(+(\sqrt{3}(c_1c_2c_3+c_1s_2s_3))/2))/2(\sqrt{3}c_5((s_4\cdot (c_1c_2s_3-c_1c_3s_2))/2-(\sqrt{3}s_4(c_1c_2s_3-c_1c_3s_2))/4+(c_1c_2c_3)/4+(c_1s_2s_3)/4 +(\sqrt{3}c_4s_5(c_1c_2s_3-c_1c_3s_2))/2)-\dot{\theta }_3((\sqrt{3}c_5((s_4(c_1c_2s_3-c_1c_3s_2))/2+(\sqrt{3}(c_1c_2c_3+c_1s_2s_3))/2))/2-(\sqrt{3}s_4(c_1c_2s_3-c_1c_3s_2))/4 +(c_1c_2c_3)/4+(c_1s_2s_3)/4+(\sqrt{3}c_4s_5(c_1c_2s_3-c_1c_3s_2))/2))$,

$Y_{21}=0$,

$Y_{22}=-\dot{\theta }_1c_1s_2-\dot{\theta }_2c_2s_1$,

$Y_{23}=-\dot{\theta }_1s_1$,

$Y_{24}=\dot{\theta }_2(s_1s_2s_3+c_2c_3s_1)-\dot{\theta }_3(s_1s_2s_3+c_2c_3s_1)-\dot{\theta }_1(c_1c_2s_3 - c_1c_3s_2)$,

$Y_{25}=\dot{\theta }_2((s_1s_2s_3)/2+(c_2c_3s_1)/2-(\sqrt{3}s_4(c_2s_1s_3-c_3s_1s_2))/2)-\dot{\theta }_3((s_1s_2s_3)/2+(c_2c_3s_1)/2-(\sqrt{3}s_4(c_2s_1s_3-c_3s_1s_2))/2)+\dot{\theta }_1((\sqrt{3}c_4s_1)/2-(\sqrt{3}s_4(c_1c_2c_3 +c_1s_2s_3))/2-(c_1c_2s_3)/2+(c_1c_3s_2)/2)+\dot{\theta }_4((\sqrt{3}c_1s_4)/2-(\sqrt{3}c_4(s_1s_2s_3+c_2c_3s_1))/2)$,

$Y_{26}=\dot{\theta }_1((\sqrt{3}c_4s_1)/4-(\sqrt{3}c_5((c_4s_1)/2-(s_4(c_1c_2c_3+c_1s_2s_3))/2+(\sqrt{3}(c_1c_2s_3-c_1c_3s_2))/2))/2+(\sqrt{3}s_5(s_1s_4+c_4(c_1c_2c_3+c_1s_2s_3)))/2 -(\sqrt{3}s_4(c_1c_2c_3+c_1s_2s_3))/4-(c_1c_2s_3)/4+(c_1c_3s_2)/4)-\dot{\theta }_5((\sqrt{3}c_5(c_1s_4-c_4(s_1s_2s_3+c_2c_3s_1)))/2+(\sqrt{3}s_5((c_1c_4)/2-(\sqrt{3}(c_2s_1s_3 -c_3s_1s_2))/2+(s_4(s_1s_2s_3+c_2c_3s_1))/2))/2)+\dot{\theta }_2((s_1s_2s_3)/4+(\sqrt{3}c_5((\sqrt{3}(s_1s_2s_3+c_2c_3s_1))/2+(s_4(c_2s_1s_3-c_3s_1s_2))/2))/2+(c_2c_3s_1)/4 -(\sqrt{3}s_4(c_2s_1s_3-c_3s_1s_2))/4+(\sqrt{3}c_4s_5(c_2s_1s_3-c_3s_1s_2))/2)-\dot{\theta }_3((s_1s_2s_3)/4+(\sqrt{3}c_5((\sqrt{3}(s_1s_2s_3+c_2c_3s_1))/2+(s_4(c_2s_1s_3 -c_3s_1s_2))/2))/2+(c_2c_3s_1)/4-(\sqrt{3}s_4(c_2s_1s_3-c_3s_1s_2))/4+(\sqrt{3}c_4s_5(c_2s_1s_3-c_3s_1s_2))/2)-\dot{\theta }_4((\sqrt{3}c_5((c_1s_4)/2-(c_4(s_1s_2s_3+c_2c_3s_1))/2))/2 -(\sqrt{3}c_1s_4)/4+(\sqrt{3}s_5(c_1c_4+s_4(s_1s_2s_3+c_2c_3s_1)))/2+(\sqrt{3}c_4(s_1s_2s_3+c_2c_3s_1))/4)$,

$Y_{31}=0$,

$Y_{32}=-\dot{\theta }_2s_2$,

$Y_{33}=0$,

$Y_{34}=\dot{\theta }_3(c_2s_3 - c_3s_2) - \dot{\theta }_2(c_2s_3 - c_3s_2)$,

$Y_{35}=\dot{\theta }_3((c_2s_3)/2-(c_3s_2)/2+(\sqrt{3}s_4(c_2c_3+s_2s_3))/2)-\dot{\theta }_2((c_2s_3)/2-(c_3s_2)/2+(\sqrt{3}s_4(c_2c_3+s_2s_3))/2)+(\sqrt{3}\dot{\theta }_4c_4(c_2s_3-c_3s_2))/2$,

$Y_{36}=\dot{\theta }_5((\sqrt{3}s_5((\sqrt{3}(c_2c_3+s_2s_3))/2+(s_4(c_2s_3-c_3s_2))/2))/2-(\sqrt{3}c_4c_5(c_2s_3-c_3s_2))/2)+\dot{\theta }_4((\sqrt{3}c_4(c_2s_3-c_3s_2))/4+ (\sqrt{3}s_4s_5(c_2s_3-c_3s_2))/2-(\sqrt{3}c_4c_5(c_2s_3-c_3s_2))/4)-\dot{\theta }_2((c_2s_3)/4-(c_3s_2)/4+(\sqrt{3}c_5((\sqrt{3}(c_2s_3-c_3s_2))/2-(s_4(c_2c_3+s_2s_3))/2))/2 +(\sqrt{3}s_4(c_2c_3+s_2s_3))/4-(\sqrt{3}c_4s_5(c_2c_3+s_2s_3))/2)+\dot{\theta }_3((c_2s_3)/4-(c_3s_2)/4+(\sqrt{3}c_5((\sqrt{3}(c_2s_3-c_3s_2))/2-(s_4(c_2c_3+s_2s_3))/2))/2 +(\sqrt{3}s_4(c_2c_3+s_2s_3))/4-(\sqrt{3}c_4s_5(c_2c_3+s_2s_3))/2)$.