Parametric Approach to Trajectory Tracking Control of Robot Manipulators

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.

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 𝐵-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) where 𝑞,𝑞,̇𝑞̈∈ℝ^𝑛 are the vectors of the generalized joint coordinates, velocity, and acceleration; 𝑀(𝑞)∈ℝ^𝑛×𝑛 denotes a symmetric positive definite inertia matrix; 𝐶(𝑞,𝑞)̇∈ℝ^𝑛×𝑛 stands for the Coriolis and centrifugal torques; 𝑔(𝑞)∈ℝ^𝑛 models the gravity forces; and 𝑢(𝑡)∈ℝ^𝑛 is the torque input. Some useful properties of robot dynamic are as follows.

Property 1. Matrix 𝑀(𝑞) is symmetric and positive definite.

Property 2. Matrix 𝑀̇(𝑞)−2𝐶(𝑞,𝑞)̇ is skew symmetric and satisfies that 𝑞̇^𝑇𝑀̇𝑞−2𝐶𝑞,𝑞̇𝑞̇=0. (2)

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 Figure 1. The masses of both links and actuators are denoted by 𝑚₁ and 𝑚₂ with 𝐼₁ and 𝐼₂ as mass moment of inertia. 𝑎₁ and 𝑎₂ denote the length; 𝑢₁ and 𝑢₂ are joints torques. The joints positions of the two links are defined by 𝜃₁ and 𝜃₂.

Figure 1 

Two-link (RR) robot manipulator.

The dynamic equations of 2-link RR robot are written in state space form as 𝑥̇=𝑓(𝑥)+𝐵(𝑥)𝑢(𝑡), (3) where 𝑥=[𝑞^𝑇,𝑞̇^𝑇]^𝑇 is the system state, 𝑞=[𝜃₁,𝜃₂]^𝑇, and 𝑓(𝑥)=𝑞̇−𝑀⁻¹𝑞𝐶𝑞̇̇𝑞,𝐵(𝑥)=0𝑀𝑞,𝑞+𝑔₋₁𝑞, (4) where 𝑀𝑞=𝑐₁₁𝑐₁₂𝑐₂₁𝑐₂₂,𝑐₁₁=𝑚₁+𝑚₂𝑎²₁+𝑚₂𝑎²₂+2𝑚₂𝑎₁𝑎₂cos𝜃₂,𝑐₁₂=𝑐₂₁=𝑚₂𝑎²₂+𝑚₂𝑎₁𝑎₂cos𝜃₂,𝑐₂₂=𝑚₂𝑎²₂,𝐶𝑞,𝑞̇𝑞̇=−𝑚₂𝑎₁𝑎₂2𝜃̇₁𝜃̇₂+𝜃̇²₂sin𝜃₂𝑚₂𝑎₁𝑎₂𝜃̇²₁sin𝜃₂,𝑔𝑞=𝑚₁+𝑚₂𝑔𝑎₁cos𝜃₁+𝑚₂𝑔𝑎₂cos𝜃₁+𝜃₂𝑚₂𝑔𝑎₂cos𝜃₁+𝜃₂. (5) Define 𝑁𝑞̇𝑞̇̇𝑞=𝑁𝑞,=𝐶𝑞,𝑞+𝑔¹𝑞̇𝑁𝑞,₂𝑞̇,𝑀𝑞,₋₁𝑞=𝑀₁₁𝑀₁₂𝑀₂₁𝑀₂₂; (6) then, 𝑓(𝑥)=𝜃₂𝜃̇₂Θ𝜃̇₁Ξ𝜃₁(𝑥)=𝑀,𝐵0000₁₁𝑀₁₂𝑀₂₁𝑀₂₂; (7) here 𝑀Θ=²²−𝑁₂𝑞,𝑞̇+𝑀₁₂−𝑁₁𝑞,𝑞̇𝑥₁(𝑡),𝑀Ξ=²²−𝑁₂𝑞,𝑞̇+𝑀₁₁−𝑁₁𝑞,𝑞̇𝑥₂(𝑡). (8)

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 𝑢(𝑡)∈ℝ^𝑛 such that the cost function 𝐽=𝛼₁Φ₀(𝑥(𝑇))+𝛼₂^𝑇₀𝑢^𝑇𝑅𝑢𝑑𝑡 (9) is minimized, where Φ₀(𝑥(𝑇))=(𝑥(𝑇)−𝑥_𝑑)^𝑇𝑄(𝑥(𝑇)−𝑥_𝑑), 𝑇 is the free terminal time, 𝑥_𝑑 is the desired trajectory, 𝛼₁ and 𝛼₂ are the weighting parameters, and 𝑄∈ℝ^2𝑛×2𝑛 and 𝑅∈ℝ^𝑛×𝑛 are symmetric positive semidefinite and symmetric positive definite weighting matrices, respectively.

We refer to the above problem as problem (𝑃). 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,𝑇] be partitioned into 𝑝 subintervals as follows: 0=𝑡₀≤𝑡₁≤⋯≤𝑡_𝑝=𝑇. (10) The switching times 𝑡_𝑖,1≤𝑖≤𝑝, 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 𝜃_𝑖=𝑖/𝑝,𝑖=1,…,𝑝, on a new time horizon [0,1]. Then the following differential equation is achieved: 𝑑𝑡(𝑠)𝑑𝑠=𝑣^𝑝(𝑠),𝑠∈[0,1], (11) where 𝑣^𝑝(𝑠)=^𝑝_𝑖=1𝜉_𝑖𝜒_{[𝜃𝑖−1,𝜃𝑖]}(𝑠), (12) where 𝜒_𝐼(𝑠) denotes the indicator function of 𝐼 defined by 𝜒_𝐼(𝑠)=1,𝑠∈𝐼,0,elsewhere, (13) and 𝜉_𝑖≥0,∑^𝑝_𝑖=1𝜉_𝑖=𝑇.

For 𝑠∈[𝜃_𝑙−1,𝜃_𝑙], we have 𝑡(𝑠)=^𝑙−1_𝑖=1𝜉_𝑖+𝜉_𝑙𝑠−𝜃_𝑙−1𝑝, (14) where 𝑙=1,…,𝑝. Clearly, 𝑡(1)=^𝑝_𝑖=1𝜉_𝑖=𝑇. (15)

Then after the time scaling transform, system (3) can be converted into the following form: 𝑥(𝑠)=𝑣^𝑝(𝑠)𝑓(𝑥(𝑠))+𝐵(𝑥(𝑠),𝑠)𝑢(𝑠), (16) where 𝑥(𝑠)=[𝑥(𝑠)^𝑇,𝑡(𝑠)]^𝑇,𝑥(𝑠)=𝑥(𝑡(𝑠)), and 𝑢(𝑠)=𝑢(𝑡(𝑠)).

Now we apply the control parameterization technique to approximate the control 𝑢(𝑠) as follows: 𝑢^𝑝_𝑖(𝑠)=^𝑝+1_𝑘=−1𝜎^𝑖_𝑘Ω1𝑝𝑠−𝑘,𝑖=1,…,𝑛, (17) where Ω(𝜅)=|𝜅|−10,>2,6|𝜅|³+𝜅²|𝜅|+4−23|𝜅|1,1≤≤2,2|𝜅|³−𝜅²+23,|𝜅|<2, (18) is the cubic spline basis function.

Define 𝜎^𝑖=[𝜎^𝑖₋₁,…,𝜎^𝑖_𝑝+1]^𝑇, 𝑖=1,…,𝑛, and 𝜎=[(𝜎¹)^𝑇,…,(𝜎^𝑛)^𝑇]^𝑇; let Π denote the set containing all 𝜎. Then 𝑢^𝑝(𝑠)=[𝑢^𝑝₁(𝑠),…,𝑢^𝑝_𝑛(𝑠)]^𝑇 is determined uniquely by the switching vector 𝜎∈Π. Thus, it can be written as 𝑢^𝑝(⋅∣𝜎). Now the optimal parameterization selection problem, which is an approximation of problem (𝑃), can be stated as follows.

Problem (𝑄). Given system (16), find a combined vector (𝜎,𝜉), such that the cost function 𝐽(𝜎)=𝛼₁Φ₀(𝑥(1∣𝜎))+𝛼₂¹₀𝑣^𝑝(𝑠∣𝜉)𝑢^𝑝(𝑠∣𝜎)^𝑇𝑅𝑢^𝑝(𝑠∣𝜎)𝑑𝑠 (19) is minimized, where Φ₀(𝑥(1∣𝜎))=(𝑥(1∣𝜎)−𝑥_𝑑)^𝑇𝑆(𝑥(1∣𝜎)−𝑥_𝑑) and 𝑥_𝑑 is the desired trajectory.

Now problem (𝑃) 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 (𝑄) can be solved easily by the use of the optimal control software package MISER 3.3 [21].

Suppose that (𝑢^𝑝∗,𝑥^∗) is the optimal solution of problem (𝑄). Then it follows that the optimal solution to problem (𝑃) is (𝑢^∗,𝑥^∗,𝑇^∗), where 𝑢^∗ is the optimal open loop control, 𝑥^∗ is the corresponding optimal state vector, and 𝑇^∗ 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 control  𝑢^∗ for problem (𝑃) is given by 𝑢^∗(𝑡)=12𝛼₂𝑅⁻¹𝐵^𝑇𝐾(𝑡)𝑓𝑥^∗(𝑡),𝑡, (20) where 𝑥^∗ is the optimal state and 𝐾(𝑡) is the solution of the following Riccati-like differential equation 𝐾̇+𝐾𝐹+𝐹^𝑇𝐾+12𝐾𝐹𝐵𝑅⁻¹𝐵^𝑇𝐾𝑓+𝐾𝐷=0, (21) where 𝐹=𝜕𝑓𝜕𝑥,𝐷=𝜕𝑓,𝐾(𝑇)𝑓(𝑥(𝑇))𝜕𝑡,𝑇=𝛼₁𝜕Φ₀(𝑥(𝑇))𝜕𝑥(𝑇)=2𝛼₁𝑥(𝑇)−𝑥_𝑑𝑆. (22)

The proof is similar to that given for Theorem  3.1 in [22]. Details can refer to this literature.

Problem (𝑅). Subject to the dynamical system (1), with 𝑢 given by Theorem 1, find a 𝐾(𝑡) such that the cost function (17) also with 𝑢 is minimized.

By Theorem 1, although the form of the optimal closed loop control law is given, the matrix function 𝐾(𝑡) 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 (𝑅) could be solved well.

In [15], an alternative approach was proposed to construct an approximate optimal matrix function 𝐾^∗(𝑡) without having to solve this complicated optimal control problem (𝑅). The basic idea is explained as follows. Suppose that 𝑢^∗ is an optimal open loop control of problem (𝑃) and that 𝑥^∗ is the corresponding optimal state. We now consider problem (𝑃) with 𝑥=𝑥^∗, that is, along the optimal open loop path, and our task is to find a 𝐾^∗(𝑡) such that 𝑢̆^∗=(1/2𝛼₂)𝑅⁻¹𝐵^𝑇𝐾^∗(𝑡)𝑓(𝑥^∗(𝑡),𝑡) best approximates the control 𝑢^∗ in the mean square sense. Then 𝑢̆^∗ can be regarded as a good approximate optimal feedback control for problem (𝑃).

The calculation steps of solving 𝐾^∗(𝑡) are as follows.

Step 1. The time horizon [0,𝑇^∗] is partitioned into 𝑝 equal subintervals: 0=𝑡₀≤𝑡₁≤⋯≤𝑡_𝑝≤𝑡_𝑝+1=𝑇^∗. (23)

Step 2. Let 𝐾(𝑡)_𝑖,𝑗≈^𝑝+1_𝑘=−1𝑐_{𝑖,𝑗,𝑘}Ω𝑇^∗𝑝𝑡−𝑘, (24) where 𝑐_{𝑖,𝑗,𝑘},𝑖,𝑗=1,2,…,𝑛,𝑘=−1,0,…,𝑝+1, are real constant coefficients that are to be determined. 𝑝 is the number of equality subintervals on [0,𝑇^∗], and 𝑝+3 is the total number of cubic spline basis functions used in the approximation of each [𝐾(𝑡)_𝑖,𝑗].

Step 3. Let Υ(𝐾)=^𝑇∗₀𝑢^∗(𝑡)−𝑢̆(𝑡)𝑑𝑡, (25) where 𝑢̆(𝑡)=12𝛼₂𝑅⁻¹𝐵^𝑇𝐾(𝑡)𝑓𝑥^∗(𝑡),𝑡. (26)

Step 4. Find coefficients 𝑐_{𝑖,𝑗,𝑘} such that the cost function (25) is minimized. These optimal coefficients can be obtained by solving the following optimality conditions: (𝐾)Λ=𝜕Υ𝜕𝑐_{𝑖,𝑗,𝑘}=0. (27) 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: 𝑚₁𝑚=1Kg,₂𝑎=1Kg,₁𝑎=1m,₂=1m,𝑔=9.8m/s²,𝑥₀=[1,1]^𝑇,𝑥̇₀=[1,1]^𝑇. (28)

The control objective is to track the desired trajectory given by 𝑞_1𝑑(),𝑞=1+0.2cos𝜋𝑡_2𝑑=1+0.2sin(𝜋𝑡). (29)

The evolution of tracking errors are as follows 𝑒=𝑒₁𝑒₂^𝑇=𝑞₁−𝑞_1𝑑𝑞₂−𝑞_2𝑑^𝑇. (30)

In the simulation, the time horizon [0,𝑇] is partitioned into 20 subintervals. 𝛼₁=3,𝛼₂=1, and 𝑆 and 𝑅 are unit matrices of proper dimension. We first use the time scaling transform and the control parameterization method to construct the corresponding approximated problem (𝑄). 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 𝐾^∗(𝑡) is obtained by the above calculation steps.

Simulation results are shown in Figures 2 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.

(a)

(b)

Figure 2 

Position tracking of the end joint.

(a)

(b)

Figure 3 

Velocity tracking of the end joint.

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.

Acknowledgments

This work was supported by the National Natural Science Foundation of China (61075083), the Natural Science Foundation of Henan Education Department (13A120204), the Technology Innovation Talents Plan of Henan University of Technology (2012CXRC14), and the Science Foundation of Henan University of Technology (2012BS043).