# Parametric Approach to Trajectory Tracking Control of Robot Manipulators

Authors: Shijie Zhang; Yi Ning
Journal: Journal of Applied Mathematics
 (2013)

---

## Abstract

The mathematic description of the trajectory of robot manipulators with the optimal trajectory tracking problem is formulated as an optimal control problem, and a parametric approach is proposed for the optimal trajectory tracking control problem. The optimal control problem is first solved as an open loop optimal control problem by using a time scaling transform and the control parameterization method. Then, by virtue of the relationship between the optimal open loop control and the optimal closed loop control along the optimal trajectory, a practical method is presented to calculate an approximate optimal feedback gain matrix, without having to solve an optimal control problem involving the complex Riccati-like matrix differential equation coupled with the original system dynamics. Simulation results of 2-link robot manipulator are presented to show the effectiveness of the proposed method.


---

## Body

## 1. Introduction

Trajectory tracking problem is the most significant and fundamental task in control of robotic manipulator. Motivated by requirements such as a high degree of automation and fast speed operation from industry, various control methods are used such as PID control, adaptive control, variable structure control, neural networks control, and fuzzy control [1–5].In the past two decades, the optimal control schemes for manipulator arms have been actively researched because the optimal motions that minimize energy consumption, error trajectories, or motion time yield high productivity, efficiency, smooth motion, durability of machine parts, and so forth [6–11]. Various types of methods have been developed to solve the robotic manipulator optimal control schemes.By the application of the optimal control theory, Pontryagin’s maximum principle leads to a two-point boundary value problem. Although this theory and its solutions are rigorous, it has been used to solve equations for the motions of 2-link or at most 3-link planar manipulators due to the complexity and the nonlinearity of the manipulator dynamics [6]. Approximation methods have been studied to obtain the solutions for three or more DOF spatial manipulators. However, the solutions obtained have not been proved to be optimal. These approximation methods are roughly divided into two groups depending on whether or not they utilize gradients [11]. Recently, the applications of intelligent control techniques (such as fuzzy control or neural network control) with optimal algorithm to the motion control of robot manipulators have received considerable attention [12–17]. But sometimes these methods take quite a long time to find a coefficient that satisfies the requirement of the controlling task. In addition, lack of theoretical analysis and stability security makes industrialists wary of using the results in real industrial environments.This paper is concerned with the nonlinear optimal feedback control of robot manipulator trajectory tracking. The energy consumption and error trajectories are minimized as performance index in the optimal control problem. An optimal open loop control is first obtained by using a time scaling transform [18] and the control parameterization technique [19]. Then, we derive the form of the optimal closed loop control law, which involves a feedback gain matrix, for the optimal control problem. The optimal feedback gain matrix is required to satisfy a Riccati-like matrix differential equation. Then, the third order B-spline function, which has been proved to be very efficient for solving optimal approximation and optimal control problems, is employed to construct the components of the feedback gain matrix. By virtue of the relationship between the optimal open loop control and the optimal closed loop control along the optimal trajectory, a practical computational method is presented for finding an approximate optimal feedback gain matrix, without having to solve an optimal control problem involving the complex Riccati-like matrix differential equation coupled with the original system dynamics [20].


## 2. Robot Manipulators Dynamics

### 2.1. Models of Robot Dynamics

Consider the dynamic equation of a robot manipulator(1)M(q)q¨+C(q,q˙)q˙+g(q)=u(t),
where q,q˙,q¨∈ℝn are the vectors of the generalized joint coordinates, velocity, and acceleration; M(q)∈ℝn×n denotes a symmetric positive definite inertia matrix; C(q,q˙)∈ℝn×n stands for the Coriolis and centrifugal torques; g(q)∈ℝn models the gravity forces; and u(t)∈ℝn is the torque input. Some useful properties of robot dynamic are as follows.Property 1.
MatrixM(q) is symmetric and positive definite.Property 2.
MatrixM˙(q)-2C(q,q˙) is skew symmetric and satisfies that
(2)q˙T[M˙(q)-2C(q,q˙)]q˙=0.Property 3.
The robot dynamics are passive in open loop, from torque input to velocity output, with the Hamiltonian as its storage function. If viscous friction was considered, the energy dissipates and the system is strictly passive.The two-link revolute (RR) robot manipulator is shown in Figure1. The masses of both links and actuators are denoted by m1 and m2 with I1 and I2 as mass moment of inertia. a1 and a2 denote the length; u1 and u2 are joints torques. The joints positions of the two links are defined by θ1 and θ2.Figure 1
Two-link (RR) robot manipulator.The dynamic equations of 2-link RR robot are written in state space form as(3)x˙=f(x)+B(x)u(t),
where x=[qT,q˙T]T is the system state, q=[θ1,θ2]T, and
(4)f(x)=[q˙-M-1(q)(C(q,q˙)q˙+g(q))],B(x)=[0M-1(q)],
where
(5)M(q)=[c11c12c21c22],c11=(m1+m2)a12+m2a22+2m2a1a2cosθ2,c12=c21=m2a22+m2a1a2cosθ2,c22=m2a22,C(q,q˙)q˙=[-m2a1a2(2θ˙1θ˙2+θ˙22)sinθ2m2a1a2θ˙12sinθ2],g(q)=[(m1+m2)ga1cosθ1+m2ga2cos(θ1+θ2)m2ga2cos(θ1+θ2)].
Define
(6)N(q,q˙)=C(q,q˙)q˙+g(q)=[N1(q,q˙)N2(q,q˙)],M-1(q)=[M11M12M21M22];
then,
(7)f(x)=[θ2θ˙2Θθ˙1Ξθ1],B(x)=[0000M11M12M21M22];
here
(8)Θ=M22(-N2(q,q˙))+M12(-N1(q,q˙))x1(t),Ξ=M22(-N2(q,q˙))+M11(-N1(q,q˙))x2(t).


### 2.2. Problem Statement

The purpose of control is to determine an optimal closed loop control signal so that the robot manipulator tracks the desired trajectory with minimal energy consumption, The optimal control problem can be formulated as follows.Given system (3), find a closed loop control u(t)∈ℝn such that the cost function
(9)J=α1Φ0(x(T))+α2∫0T‍uTRudt
is minimized, where Φ0(x(T))=(x(T)-xd)TQ(x(T)-xd), T is the free terminal time, xd is the desired trajectory, α1 and α2 are the weighting parameters, and Q∈ℝ2n×2n and R∈ℝn×n are symmetric positive semidefinite and symmetric positive definite weighting matrices, respectively.We refer to the above problem as problem(P). This optimal close loop control problem is very difficult to be solved directly. In this paper, we derive the form of the optimal closed loop control law after obtaining an optimal open loop control by using a time scaling transform and the control parameterization technique. Then the difficulty of the problem is transformed to find a feedback gain matrix which is involved in the optimal closed loop control law. A practical computational method is presented in [20] for finding an approximate optimal feedback gain matrix, without having to solve an optimal control problem involving the complex Riccati-like matrix differential equation coupled with the original system dynamics.


## 2.1. Models of Robot Dynamics

Consider the dynamic equation of a robot manipulator(1)M(q)q¨+C(q,q˙)q˙+g(q)=u(t),
where q,q˙,q¨∈ℝn are the vectors of the generalized joint coordinates, velocity, and acceleration; M(q)∈ℝn×n denotes a symmetric positive definite inertia matrix; C(q,q˙)∈ℝn×n stands for the Coriolis and centrifugal torques; g(q)∈ℝn models the gravity forces; and u(t)∈ℝn is the torque input. Some useful properties of robot dynamic are as follows.Property 1.
MatrixM(q) is symmetric and positive definite.Property 2.
MatrixM˙(q)-2C(q,q˙) is skew symmetric and satisfies that
(2)q˙T[M˙(q)-2C(q,q˙)]q˙=0.Property 3.
The robot dynamics are passive in open loop, from torque input to velocity output, with the Hamiltonian as its storage function. If viscous friction was considered, the energy dissipates and the system is strictly passive.The two-link revolute (RR) robot manipulator is shown in Figure1. The masses of both links and actuators are denoted by m1 and m2 with I1 and I2 as mass moment of inertia. a1 and a2 denote the length; u1 and u2 are joints torques. The joints positions of the two links are defined by θ1 and θ2.Figure 1
Two-link (RR) robot manipulator.The dynamic equations of 2-link RR robot are written in state space form as(3)x˙=f(x)+B(x)u(t),
where x=[qT,q˙T]T is the system state, q=[θ1,θ2]T, and
(4)f(x)=[q˙-M-1(q)(C(q,q˙)q˙+g(q))],B(x)=[0M-1(q)],
where
(5)M(q)=[c11c12c21c22],c11=(m1+m2)a12+m2a22+2m2a1a2cosθ2,c12=c21=m2a22+m2a1a2cosθ2,c22=m2a22,C(q,q˙)q˙=[-m2a1a2(2θ˙1θ˙2+θ˙22)sinθ2m2a1a2θ˙12sinθ2],g(q)=[(m1+m2)ga1cosθ1+m2ga2cos(θ1+θ2)m2ga2cos(θ1+θ2)].
Define
(6)N(q,q˙)=C(q,q˙)q˙+g(q)=[N1(q,q˙)N2(q,q˙)],M-1(q)=[M11M12M21M22];
then,
(7)f(x)=[θ2θ˙2Θθ˙1Ξθ1],B(x)=[0000M11M12M21M22];
here
(8)Θ=M22(-N2(q,q˙))+M12(-N1(q,q˙))x1(t),Ξ=M22(-N2(q,q˙))+M11(-N1(q,q˙))x2(t).


## 2.2. Problem Statement

The purpose of control is to determine an optimal closed loop control signal so that the robot manipulator tracks the desired trajectory with minimal energy consumption, The optimal control problem can be formulated as follows.Given system (3), find a closed loop control u(t)∈ℝn such that the cost function
(9)J=α1Φ0(x(T))+α2∫0T‍uTRudt
is minimized, where Φ0(x(T))=(x(T)-xd)TQ(x(T)-xd), T is the free terminal time, xd is the desired trajectory, α1 and α2 are the weighting parameters, and Q∈ℝ2n×2n and R∈ℝn×n are symmetric positive semidefinite and symmetric positive definite weighting matrices, respectively.We refer to the above problem as problem(P). This optimal close loop control problem is very difficult to be solved directly. In this paper, we derive the form of the optimal closed loop control law after obtaining an optimal open loop control by using a time scaling transform and the control parameterization technique. Then the difficulty of the problem is transformed to find a feedback gain matrix which is involved in the optimal closed loop control law. A practical computational method is presented in [20] for finding an approximate optimal feedback gain matrix, without having to solve an optimal control problem involving the complex Riccati-like matrix differential equation coupled with the original system dynamics.


## 3. Parametric Approach to the Optimal Controller Design

By using a time scaling transform and the control parameterization technique, the above problem is solved as an optimal open loop control problem firstly. An optimal open loop control and the corresponding optimal trajectory will be provided.Let the time horizon[0,T] be partitioned into p subintervals as follows:
(10)0=t0≤t1≤⋯≤tp=T.
The switching times ti,1≤i≤p, are regarded as decision variables. Employing the time scaling transform introduced in [19] to map these switching times into a set of fixed time points θi=i/p,i=1,…,p, on a new time horizon [0,1]. Then the following differential equation is achieved:
(11)dt(s)ds=vp(s),s∈[0,1],
where
(12)vp(s)=∑i=1p‍ξiχ[θi-1,θi](s),
where χI(s) denotes the indicator function of I defined by
(13)χI(s)={1,s∈I,0,elsewhere,
and ξi≥0,∑i=1p‍ξi=T.Fors∈[θl-1,θl], we have
(14)t(s)=∑i=1l-1‍ξi+ξl(s-θl-1)p,
where l=1,…,p. Clearly,
(15)t(1)=∑i=1p‍ξi=T.Then after the time scaling transform, system (3) can be converted into the following form:
(16)x^(s)=vp(s)[f(x^(s))+B(x^(s),s)u~(s)],
where x^(s)=[x~(s)T,t(s)]T,x~(s)=x(t(s)), and u~(s)=u(t(s)).Now we apply the control parameterization technique to approximate the controlu~(s) as follows:
(17)u~ip(s)=∑k=-1p+1σkiΩ((1p)s-k),i=1,…,n,
where
(18)Ω(κ)={0,|κ|>2,-16|κ|3+κ2-2|κ|+43,1≤|κ|≤2,12|κ|3-κ2+23,|κ|<2,
is the cubic spline basis function.Defineσi=[σ-1i,…,σp+1i]T, i=1,…,n, and σ=[(σ1)T,…,(σn)T]T; let Π denote the set containing all σ. Then u~p(s)=[u~1p(s),…,u~np(s)]T is determined uniquely by the switching vector σ∈Π. Thus, it can be written as u~p(·∣σ). Now the optimal parameterization selection problem, which is an approximation of problem (P), can be stated as follows.Problem(Q). Given system (16), find a combined vector (σ,ξ), such that the cost function
(19)J(σ)=α1Φ^0(x^(1∣σ))+α2∫01‍vp(s∣ξ)u~p(s∣σ)TRu~p(s∣σ)ds
is minimized, where Φ^0(x^(1∣σ))=(x^(1∣σ)-x^d)TS^(x^(1∣σ)-x^d) and x^d is the desired trajectory.Now problem(P) is approximated by a sequence of optimal parameter selection problems, each of which can be viewed as a mathematical programming problem and hence can be solved by existing gradient-based optimization methods. Here, our controls are approximated in terms of cubic spline basis functions, and thus they are smooth. problem (Q) can be solved easily by the use of the optimal control software package MISER 3.3 [21].Suppose that(u~p*,x^*) is the optimal solution of problem (Q). Then it follows that the optimal solution to problem (P) is (u*,x*,T*), where u* is the optimal open loop control, x* is the corresponding optimal state vector, and T* is the optimal terminal time. For the computation of the optimal closed loop control problem, we have the following theorem.Theorem 1.
The optimal closed loop controlu-* for problem (P) is given by
(20)u-*(t)=12α2R-1BTK(t)f(x*(t),t),
where x* is the optimal state and K(t) is the solution of the following Riccati-like differential equation
(21)(K˙+KF+FTK+12KFBR-1BTK)f+KD=0,
where
(22)F=∂f∂x,D=∂f∂t,K(T)f(x(T),T)=α1∂Φ0(x(T))∂x(T)=2α1(x(T)-xd)S.The proof is similar to that given for Theorem  3.1 in [22]. Details can refer to this literature.Problem(R). Subject to the dynamical system (1), with u- given by Theorem 1, find a K(t) such that the cost function (17) also with u- is minimized.By Theorem1, although the form of the optimal closed loop control law is given, the matrix function K(t) is still required to be obtained. The solving process involves solving a new optimal control problem denoted as follow. Using the method proposed in [20], problem (R) could be solved well.In [15], an alternative approach was proposed to construct an approximate optimal matrix function K*(t) without having to solve this complicated optimal control problem (R). The basic idea is explained as follows. Suppose that u* is an optimal open loop control of problem (P) and that x* is the corresponding optimal state. We now consider problem (P) with x=x*, that is, along the optimal open loop path, and our task is to find a K*(t) such that u˘*=(1/2α2)R-1BTK*(t)f(x*(t),t) best approximates the control u-* in the mean square sense. Then u˘* can be regarded as a good approximate optimal feedback control for problem (P).The calculation steps of solvingK*(t) are as follows.Step 1.
The time horizon[0,T*] is partitioned into p equal subintervals:
(23)0=t0≤t1≤⋯≤tp≤tp+1=T*.Step 2.
Let(24)[K(t)i,j]≈∑k=-1p+1‍(ci,j,k)Ω((T*p)t-k),
where ci,j,k,i,j=1,2,…,n,k=-1,0,…,p+1, are real constant coefficients that are to be determined. p is the number of equality subintervals on [0,T*], and p+3 is the total number of cubic spline basis functions used in the approximation of each [K(t)i,j].Step 3.
Let(25)Υ(K)=∫0T*‍∥u*(t)-u˘(t)∥dt,
where
(26)u˘(t)=12α2R-1BTK(t)f(x*(t),t).Step 4.
Find coefficientsci,j,k such that the cost function (25) is minimized. These optimal coefficients can be obtained by solving the following optimality conditions:
(27)Λ=∂Υ(K)∂ci,j,k=0.
We can see that these are linear equations and hence are easy to solve.


## 4. Simulation

In this section, the simulations of the nonlinear optimal control for the 2-link RR-robot manipulator are performed to show the efficiency of the proposed method.Assuming that the friction is negligible, two-link robot manipulators is simulated with following parameters:(28)m1=1Kg,m2=1Kg,a1=1m,a2=1m,g=9.8m/s2,x0=[1,1]T,x˙0=[1,1]T.The control objective is to track the desired trajectory given by(29)q1d=1+0.2cos(πt),q2d=1+0.2sin(πt).The evolution of tracking errors are as follows(30)e=[e1e2]T=[q1-q1dq2-q2d]T.In the simulation, the time horizon[0,T] is partitioned into 20 subintervals. α1=3,α2=1, and S and R are unit matrices of proper dimension. We first use the time scaling transform and the control parameterization method to construct the corresponding approximated problem (Q). Then, MISER 3.3 is utilized to solve it, giving rise to an optimal open loop control and the corresponding optimal trajectory. Then the feedback gain matrix K*(t) is obtained by the above calculation steps.Simulation results are shown in Figures2 to 4. The position tracking and the velocity tracking of the end joint are shown in Figures 2 and 3, and the control input of the end joint is shown in Figure 4.Position tracking of the end joint.
(a)
(b)Velocity tracking of the end joint.
(a)
(b)Figure 4
The control inputs of the end joint.


## 5. Conclusions

A parametric approach to trajectory tracking control of robot manipulators is studied in this paper, in which an optimal open loop control is obtained firstly by using the control parametrization method and the time scaling transform. Then, we obtained the form of the optimal closed loop control law, where the feedback gain matrix is required to satisfy a Riccati-like matrix differential equation, and a practical method was proposed to calculate the feedback gain matrix. The simulation results demonstrate the validity of the proposed method.



---

*Source: 958597-2013-10-31.xml*