Time-Optimal Trajectory Planning of Six-Axis Manipulators Based on the Improved Direct Collocation Method with FMU

Abstract

The trajectory planning method with dynamics is the key to improving the motion performance of manipulators. The optimal control method (OCM) is a key technology to solve optimal problems with dynamics. There are direct and indirect methods in OCM; indirect methods are difficult to apply to engineering applications, and so direct methods are widely applied instead. The direct collocation method (DCM) is a technology in OCM to transform an optimal control problem (OCP) to a nonlinear problem (NLP), so that plenty of solvers can be used directly. However, the general DCM, for which it has been found that the explicit form of the right-hand-side (RHS) functions of state equations of the complex system in the OCP is hard to derive, is limited to solving the OCP of three-axis manipulators. This paper proposes an improved DCM to solve the OCP of six-axis manipulators, which can find the solution of the time-optimal trajectory for the motion of six-axis manipulators based on the improved DCM. The proposed method derives the RHS equations implicitly by introducing a Functional Mock-up Unit (FMU), which simplifies the representation of the RHS equations as a black-box model, so that the DCM can be applied to the OCP of six-axis manipulators. A simulation case of a three-axis manipulator accomplished in a related study works as a reference compared with our improved method to verify the solution consistence between the DCM using the explicit RHS equations or using the implicit RHS equations, and the loss of computational efficiency is acceptable. In the meantime, a simulation solution and an experiment of six-axis manipulators, which is a novel advancement, are presented to validate the proposed method.

1. Introduction

Industrial robotic manipulators constitute important modern manufacturing automatic equipment and play an important role in manufacturing. The trajectory planning method is the key to the manipulators’ motion performance, and makes the control stable and agile by considering the robotic dynamics behavior [1]. The targets of trajectory planning are mainly time, energy, and jerk. They can increase productivity, save energy, and smooth the motion [2]. This paper talks about minimizing the time of trajectory planning of industrial robotic manipulators. There are some related research studies about this topic presented in this section.

A group of researchers used heuristics methods to solve these problems; for example, Buhai Shi and Jiaxiang Xu proposed an improved particle swarm optimization that transformed the problem into a parameter optimization problem of B-Spline [3]. G. Li et al. proposed an optimal trajectory planning method using a cubic polynomial curve [4], while H. Wang et al. applied a multi-segment high-order polynomial curve in the smooth trajectory planning of industrial manipulators [5]. H. Yu et al. applied an improved adaptive genetic algorithm with quintic polynomial function to realize the time-optimal trajectory planning problem [6]. B. Shi and H. Zeng applied an improved Hybrid-PSO algorithm with NURBS in Cartesian and cubic B-Spline in joint space to generate a smooth and time-optimal trajectory [7]. E. Barnett and C. Gosselin introduced the bisection algorithm to solve the trajectory planning problem of parallel manipulators along specified paths [8]. Another group of researchers solved the problem with optimal control methods; for example, Tie Zhang et al. used the method of J. E. Bobrow [9] to obtain the minimum-time trajectory along a specified path of the manipulators; moreover, they designed an input shaping algorithm to confirm the precision of the trajectory tracking [10].

However, all the above studies achieved the optimal trajectories in the state space of the system. The state space variables, such as joint positions, joint velocities, and joint accelerations of manipulators, cannot be applied as input to the real system directly. The inverse dynamics method, which usually refers to the feedback system, should be applied to transform the optimized states into the controls, such as joint torques, actuator voltage and so on. However, the controls of the system rely on the feedback system, which depends on the sample period, which may cause the system delay. We prefer the traditional optimal control method that can find the desired controls, and the result can be input to the system as feed-forward to obtain the expected motion of the system, and the performance of the system is going to be better in this way. The optimal control method (OCM) is a powerful technique for solving this kind of problem with dynamics. Related research about the optimal control is given in the following paragraphs.

J. Harzer et al. studied the OCM, solving the problem of high-oscillatory systems [11]. J. Wang et al. worked on the semi-linear parabolic optimal control problems [12]. M. Leomanni et al. worked on the time-optimal control problem for a chain of multidimensional integrators [13]. Meanwhile, K. Prag et al. reviewed the reinforcement learning approaches for adaptive data-driven optimal control frameworks, enabling the OCM with data-driven dynamics [14]. J. Zhao, Z. Yuan and S. Manna applied the OCM with data-driven dynamics in different fields, and obtained great results [15,16,17]. For further research, Z. Mei et al. proposed a feed-forward control method based on the hybrid inverse dynamic model that couples the data-driven model with the analytic model [18]. M. Schappler et al. realized the model-based joint impedance control on the arms of the Atlas robot [19]. J. Richalet et al. proposed a novel control strategy called model predictive control (MPC), and show its robustness [20]. C. E. Garcia et al. applied MPC in nonlinear systems, and proved it is highly effective by comparing it with the traditional feedback control method [21]. K. Czerwinski and M. Lawrynczuk proved that the dynamic matrix control algorithm, which belongs to MPC, can be used in the fast embedded system, with MPC satisfying the real-time requirements, and can achieve rapid response [22]. G. Ortiz-Torres et al. and F. D. J. Sorcia-Vazquez et al. presented a decentralized model predictive controller for different nonlinear systems [23,24]. All these studies offer a framework of control strategies based on OCM. Some studies are discussed below that use OCM to solve the trajectory planning problem of manipulators.

M. E. Kahn and B. Roth firstly worked on the minimum-time optimal control problem (OCP) for manipulators in 1971; their research [25] linearized the motion equations for the system so that they could simplify the highly nonlinear dynamics system and used the Pontryagin minimum principle to obtain an approximation of the minimum-time control.

J. E. Bobrow solved the minimum-time manipulator control problem, the path of which is specified, and the actuator torque limitations of which are known. This method finds the bang-bang switching structure and finds the optimal open-loop torques [9].

In 1988, M. Otter gave a non-academic, highly nonlinear model of a commercially available robot [26]. O Von Stryk used this model with a hybrid approach that combines a direct collocation method (DCM) and an indirect multiple shooting method to solve optimal point-to-point trajectory planning problems of robotic manipulators in the mid-1990s [27]. At this time, the optimal control method was firstly applied in three-axis robotic manipulators.

In 2010, Victor M. Becerra developed the DCM and published an open-source solver PSOPT [28]. The work of Stryk in 1988 was included in the example set of PSOPT. M. Kelly published an introduction to help others solve trajectory planning problems using DCM in 2016 [29].

It is said in [27] that the most difficult part in describing an OCP is to give the expression of the state equations of a complex dynamic system, which are generally called the right-hand-side (RHS) equations and written as:

$$\dot{\mathit{x}}=\mathit{f}(\mathit{x},\mathit{u},t),$$

(1)

where x and u refer to the states and the controls in a dynamic system; function f generally refers to the forward dynamics of the system.

As has been said in [27], the dynamics behavior of the manipulator is given either explicitly by the output of a symbolic computation system, or in an efficient implicit form of the RHS equations by the subroutine R3M2SI [30].

However, the RHS equations become more and more difficult to express as the degree of freedom of robotic manipulators increases. The work of [26] derived the explicit RHS equations of a three-axis manipulator; the work of [30] presents a general method to describe the RHS equations of manipulators wit ha tree structure in an implicit way, and the subroutine R3M2SI is an instance of this method applied to a three-axis manipulator. However, the RHS equations of six-axis manipulators are still tough to work out, and there is no research solving the OCP with the RHS equations of six-axis manipulators.

This research introduces Functional Mock-up Unit (FMU) to overcome the difficulty. Modelica is a modeling language, which builds models that can instead use the RHS equations. The Modelica model is defined as an FMU which can be regarded as a black-box function. Moreover, a standard interface criterion FMI (Functional Mock-up Interface) [31] is applied to call any specified variables in the Modelica models. Therefore, we can use the Modelica model to construct the dynamics of manipulators and call the model with FMI in an implicit form of the RHS equations. In this way, the OCP described in PSOPT with the RHS equations expressed by FMU can be solved. The solutions of the time-optimal trajectory planning problem of both three-axis and six-axis robotic manipulators are given in this paper.

The related paper flow is shown in Figure 1. The paper is organized as follows: The problem description of OCP is formulated in Section 2 for general manipulators. Section 3 introduces the improved DCM, and shows how to transcribe a continuous OCP into a finite-dimensional problem that can be solved by using an NLP algorithm. In Section 4, the simulation results of three-axis robotic manipulators named Manutec R3 model 2 and six-axis robotic manipulators named Manutec R3 model 1 in [26] are presented. Section 5 gives the experiments solutions, and Section 6 sums up the full paper.

2. Problem Statement

2.1. General Problem Formulation of Optimal Control

The minimum-time trajectory planning problem of manipulators can be described as an OCP. The general OCP formulation can be defined to find the control trajectories $\mathit{u}\left(t\right), t\in [t_0,t_f]$ that minimize the following Bolza performance index:

$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+{\int }_{t_0}^{t_f}L(\mathit{x}\left(t\right),\mathit{u}\left(t\right),t)dt,$$

(2)

subjected to the state equations (the RHS equations):

$$\dot{\mathit{x}}\left(t\right)=\mathit{f}(\mathit{x},\mathit{u},t), t\in [t_0,t_f],$$

(3)

the boundary conditions:

$$\mathit{\varphi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)=0,$$

(4)

and the inequality constraints:

$$\mathit{C}\left(\mathit{x}\right(t),\mathit{u}(t),t)\le 0,$$

(5)

where J is the index, which is the function we aim to optimize. $\mathsf{\Phi }$ and L are the boundary cost function and process cost function. The performance index of the minimum-time trajectory planning problem discussed in this paper belongs to a boundary cost function, and the the process cost function equals zero. $t_0$ and $t_f$ are the start time and the end time, respectively. u represents the controls in the system, and refers to the output torque of the joint actuators in the robotic dynamic system. x represents the states in the system, and refers to joint angles and joint velocities in the robotic dynamic system. $\dot{\mathit{x}}$ represents the differential states, and refers to joint velocities and joint accelerations in the robotic dynamic system [27].

The minimum-time trajectory planning problem of robotic manipulators discussed in this paper is solved with the optimal control solver; the problem description is given in terms of the general OCP, as shown in the following section.

2.2. Minimum-Time Trajectory Planning Problem Description of Robotic Manipulators

For the minimum-time trajectory problem of industrial robots, the performance index is expressed as below:

$$J=t_f\to min.$$

(6)

The RHS equations can be derived from the dynamics of the system. The general dynamics equation of manipulators is expressed on this function:

$$\mathit{\tau }=M\left(\mathit{q}\right)\ddot{q}+\mathit{v}(\mathit{q},\dot{\mathit{q}})+\mathit{g}\left(\mathit{q}\right),$$

(7)

where q indicates the states representing the relative angle between adjacent arms, $\mathit{\tau }$ represents the controls representing the output torque of the joint actuators, $M\left(\mathit{q}\right)$ is the positive definite and symmetric matrix of the moment of inertia, $\mathit{v}(\mathit{q},\dot{\mathit{q}})$ is the moment caused by Coriolis and centrifugal forces, and $\mathit{g}\left(\mathit{q}\right)$ is the moment caused by gravitational forces.

After several transposition operations on (7), the RHS equations become:

$$\ddot{\mathit{q}}=M^{-1}\left(\mathit{q}\right)[\mathit{\tau }-v(\mathit{q},\dot{\mathit{q}})-g\left(\mathit{q}\right)].$$

(8)

It is quite important to derive the RHS equations, because it helps to predict the states of the system. Finding methods to instead the explicit expression of (8) is also the main difficulty in solving the OCP.

As a result, the state and the state equations of robotic manipulators can be written as below:

$$\mathit{x}=\left(\begin{array}{c}\mathit{q}\\ \dot{\mathit{q}}\end{array}\right), \dot{\mathit{x}}=\left(\begin{array}{c}\dot{\mathit{q}}\\ M^{-1}\left(\mathit{q}\right)[\mathit{\tau }-v(\mathit{q},\dot{\mathit{q}})-g\left(\mathit{q}\right)]\end{array}\right).$$

(9)

The boundary conditions $\mathit{\varphi }$ are taken into consideration as below:

$$\mathit{q}\left(t_0\right)={\mathit{q}}_0, \mathit{q}\left(t_f\right)={\mathit{q}}_f,$$

(10)

$$\dot{\mathit{q}}\left(t_0\right)={\dot{\mathit{q}}}_0, \mathit{q}\left(t_f\right)={\dot{\mathit{q}}}_f.$$

(11)

Generally, ${\mathit{q}}_0$ and ${\mathit{q}}_f$ refer to the start and the end, and they are both constant and can be defined by users. The stationary boundary conditions are taken into consideration, i.e., ${\dot{\mathit{q}}}_0={\dot{\mathit{q}}}_f=0$.

The inequality constraints can be categorized into the following two kinds:

• The control constraints on the torque voltage;

  $$\mathit{\mid }{u}_i\left(t\right)\mid \le {\mathit{u}}_{i,max}, i=1\dots n.$$

  (12)
• The state constraints on the angles and the angular velocities.

  $$\mid {\mathit{q}}_i\left(t\right)\mid \le {\mathit{q}}_{i,max}, i=1\dots n.$$

  (13)

  $$\mid {\dot{\mathit{q}}}_i\left(t\right)\mid \le {\dot{\mathit{q}}}_{i,max}, i=1\dots n.$$

  (14)

  where i refers to the number of the robotic link, and n refers to the max number of the robotic link, which is equal to the degrees of freedom of robotic manipulators.

3. DCM with FMU

The OCP proposed in the last section is a kind of variational problem; the Pontryagin minimum principle offers an explicit solving method. However, when it comes to the engineering application, the related OCPs are too difficult to solve in this way. Therefore, the DCM is adopted here to transcribe a continuous OCP into a discrete NLP problem, so that a series of NLP solvers are available to be applied to solve the problem.

However, as said in last section, the RHS equations are still troublesome. In this section, a new method is proposed, where the FMU is applied to improve the DCM. The target DCM makes it possible to solve the problem with an NLP solver; the FMU can be applied to express the RHS equations of the complex system, such as six-axis robotic manipulators.

The collocation methods are divided into local and global methods according to the information used to approximate the RHS equations, which are the derivatives of the states in essence. While there are a lot of numerical methods using the local information to obtain the derivatives of a function, including the Euler method, trapezoidal method, Hermite–Simpson method and classical Runge–Kutta method, pseudospectral methods use, in contrast, the global information over samples of the whole domain of the function to approximate their derivatives at chosen points. Generally, the global pseudospectral methods offers a faster convergence. There are three kinds of pseudospectral methods, including the Legendre Pseudospectral Method (LPM), Gauss Pseudospectral Method (GPM) and Radau Pseudospectral Method (RPM). When applied to an OCP, the terminal of which is not completely free, the Legendre Pseudospectral Method allows better convergence [32,33,34,35]. As a result, this work applies the Legendre Pseudospectral Method.

The processes of the improved LPM method with FMU are shown in the following sections.

3.1. Original LPM Method Processes

This part shows how the LPM method transforms an OCP to an NLP problem.

3.1.1. Time Domain Transformation

The LPM method discretizes the states and the controls on the Legendre–Gauss–Lobatto (LGL) points (note that $P_N\left(T\right)$ is an N-order Legendre polynomial, and ${\dot{L}}_N\left(T\right)$ is the derivative of $L_N\left(T\right)$). Because the definition domain of Legendre polynomials is interval $[-1,1]$, the LGL points can be given as

$$T_0=-1, T_1, T_2, \dots , T_{N-1}, T_N=1,$$

(15)

where $T_1, T_2, \dots , T_{N-1}$ are the roots of ${\dot{L}}_N\left(T\right)$. The transformation function between the real time interval $[t_0,t_f]$ and the definition interval $[-1,1]$ are shown as follows:

$$T=\frac{2t-t_f-t_0}{t_f-t_0}, T\in [-1,1].$$

(16)

3.1.2. Collocation and Discretization

The collocation points are equivalent to the LGL points. Discretizing the states and the controls in every point to generate the discrete states $X_0, X_1, \dots , X_N$ and the discrete controls $U_0, U_1, \dots , U_N$. (N is the number of the collocation points). According to the Lagrange interpolation method, the approximations of the states and the controls are determined as below:

$$\mathit{x}\left(T\right)\approx \mathit{X}=\sum _{i=0}^N{L}_i\left(T\right){\mathit{X}}_i,$$

(17)

$$\mathit{u}\left(T\right)\approx \mathit{U}=\sum _{i=0}^N{L}_i\left(T\right){\mathit{U}}_i,$$

(18)

$L_i\left(T\right)$ is the basis function of Lagrange interpolation function, and is shown as below:

$$L_i\left(T\right)=\prod _{j=0,j\ne i}^N\frac{T-T_j}{T_i-T_j}.$$

(19)

In this way, differential operations in the state equations and the integral operation in the performance index are transformed into an algebraic operation, shown in the next step.

3.1.3. The Transformation of RHS Equations

Once the estimation of the states comes out, the differential values of the states can be estimated as below:

$$\dot{\mathit{x}}\left(T_k\right)\approx {\dot{\mathit{X}}}_k=\sum _{i=0}^N{\dot{L}}_i\left(T_k\right){\mathit{X}}_i=\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i,$$

(20)

where $k=0,1,\dots ,N$ and the differential matrix $D_{ki}$ represent the differential values of every basis function of the Lagrange interpolation function in every LGL point, as shown below:

$$D_{ki}=\left\{\begin{array}{cc}\frac{P_N\left(T_k\right)}{P_N\left(T_i\right)(T_k-T_i)},& i\ne k;\\ -\frac{N(N+1)}{4},& i=k=0;\\ \frac{N(4N+1)}{4},& i=k=N;\\ 0,& otherwise;\end{array}\right.$$

(21)

Now, the RHS equations can be written as:

$$\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i-\frac{t_f-t_0}{2}\mathit{f}({\mathit{X}}_k,{\mathit{U}}_k,{\tau }_k)=0, k=0, 1, \dots , N.$$

(22)

3.1.4. Integral Equations Transformation

The performance index after transformation using the Gauss–Lobatto integral method is shown as below:

$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+\frac{t_f-t_0}{2}\sum _{i=0}^N{\omega }_{i}L({\mathit{X}}_i,{\mathit{U}}_i,T_i),$$

(23)

where ${\omega }_i$ is the integral weight, defined as:

$${\omega }_i=\frac{2}{N(N+1)P_N^2\left(T_i\right)}.$$

(24)

3.1.5. The Constraints Transformation

The boundary conditions and the inequality constraints are discretized as below:

$$\mathit{\Phi }({\mathit{X}}_0,{\mathit{X}}_N,t_0,t_f)\le \mathit{\delta },$$

(25)

$$\mathit{C}({\mathit{X}}_k,{\mathit{U}}_k,T_k)\le \mathit{0}, k=0, 1, \dots , N,$$

(26)

There is no absolute equality between any values in a numerical calculation; therefore, the slack $\mathit{\delta }$ is introduced to avoid system instability.

To sum up, the original OCP (2)–(5) transform into (23), (22), (25), and (26), respectively. However, Equation (22) still includes the right side of the RHS function $\mathit{f}({\mathit{X}}_k,{\mathit{U}}_k,{\tau }_k)$. Therefore, FMU is introduced to substitute the RHS equations in the next step.

3.2. The Substitution of RHS Equations with FMU

This step aims to rebuild (22) with an FMU as a substitution of the RHS equations. Modelica is used here to build FMU; FMI offers supports the interface to use FMU as the RHS equations.

Modelica is a language for physical modeling. The design approach builds on non-casual modeling with true equations and the use of object-oriented constructs to facilitate the reuse of modeling knowledge. As a result, it is convenient to write the dynamics equations in a freer form of which you do not have to guarantee there is one and only one variable on the left-hand side. With this property, the RHS equations can be derived implicitly by constructing the FMU of the robotic dynamics in Newton–Euler form.

The robot dynamics model is constructed by a Modelica based software Mworks supporting drag-and-drop modeling [36]. As a result, the dynamics in the Newton–Euler form that have clear physical significance can be expressed easily in Mworks. The model is shown in Figure 2.

Where the components tau1–tau6 represent the output torques of motors, the components inertia1–inertia6 represent the inertias of motors; the components r1–r6 represent the revolute joints in manipulators which shows the relationship of joint coordinates fixed on adjacent links; the components b1–b6 represent the link bodies in the manipulators which contain the iterative Newton–Euler dynamic formulation [37] of adjacent links. The outputs of components inertia 1–inertia6 are the output torques of gears, which are the input of components r1–r6.

The dynamics in Newton–Euler form are constructed by operations of dragging–dropping function blocks. The components’ revolute contains information of kinematic forward derivation in the Newton–Euler method, and the component link body contains physical properties of the robotic body and information of dynamic backward derivation in the Newton–Euler Method.

The Modelica model can be transformed into FMU with FMI. The structure chart of FMU is shown in Figure 3.

FMU is a black-box model used here to replace the analytic RHS equations; the input and output flows are presented in Figure 3. According to Figure 3, the FMU model, where the inputs are the present time t, the present controls u, and the present states x, can derive the present differential states $\dot{\mathit{x}}$ by the internal solver. Therefore, it can be applied to the OCP solver as the RHS equations, and the difficulty in expressing the RHS equations is overcome.

As a result, two black-box functions are used to express the dynamics of manipulators; i.e., Equations (7)–(9) are rewritten in the FMU form below:

$$\mathit{\tau }={\mathrm{FMU}}_{inverse}(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}},t),$$

(27)

$$\ddot{\mathit{q}}={\mathrm{FMU}}_{forward}(\mathit{\tau },\mathit{q},\dot{\mathit{q}},t),$$

(28)

$$\mathit{x}=\left(\begin{array}{c}\mathit{q}\\ \dot{\mathit{q}}\end{array}\right), {\dot{\mathit{x}}}_{\mathrm{FMU}}=\left(\begin{array}{c}\dot{\mathit{q}}\\ {\mathrm{FMU}}_{forward}(\mathit{\tau },\mathit{q},\dot{\mathit{q}},t)\end{array}\right).$$

(29)

By discretizing (29), we have:

$$\left\{\begin{array}{c}{\mathit{X}}_i=\left(\begin{array}{c}\mathit{q}\left(T_i\right)\\ \dot{\mathit{q}}\left(T_i\right)\end{array}\right),\\ {\dot{\mathit{X}}}_{\mathrm{FMU},i}=\left(\begin{array}{c}\dot{\mathit{q}}\left(T_i\right)\\ {\mathrm{FMU}}_{forward}(\mathit{\tau }\left(T_i\right),\mathit{q}\left(T_i\right),\dot{\mathit{q}}\left(T_i\right),T_i)\end{array}\right).\end{array}\right.$$

(30)

Then, the state equations are given by substituting (30) into (22); that is,

$${∥\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i-\frac{t_f-t_0}{2}{\dot{\mathit{X}}}_{\mathrm{FMU},i}∥}_{\infty }\le \mathit{\delta }, k=0, 1, \dots , N.$$

(31)

All in all, the FMU model of manipulators, which is built by using the Newton–Euler method, can be applied in the DCM to determine the value of the RHS equations in (22). ${\dot{\mathit{X}}}_{\mathrm{FMU},i}$ in (30), which is derived in the FMU, is used to obtain the values of the RHS equations in (22), which are the value of $\mathit{f}({\mathit{X}}_k,{\mathit{U}}_k,{\mathit{\tau }}_k), k=0, 1, \dots , N$.

3.3. Final NLP Problem Description

After adding FMU into the LPM method, the final NLP problem formula of the time-optimal trajectory planning problem in terms of (2)–(5) is presented below:

Minimize:

$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+\frac{t_f-t_0}{2}\sum _{i=0}^N{\omega }_{i}L({\mathit{X}}_i,{\mathit{U}}_i,T_i).$$

(32)

This is subjected to:

$${∥\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i-\frac{t_f-t_0}{2}{\dot{\mathit{X}}}_{\mathrm{FMU},i}∥}_{\infty }\le \mathit{\delta },$$

(33)

$$\mathit{\Phi }({\mathit{X}}_0,t_0,{\mathit{X}}_N,t_f)\le \mathit{\delta },$$

(34)

$$\mathit{C}({\mathit{X}}_k,{\mathit{U}}_k,T_k)\le 0, k=0, 1, \dots , N.$$

(35)

This final formula of the problem statement makes it possible to solve the transformed OCP by NLP solvers. IPOPT is the NLP solver applied here.

The problem formula, which is transformed from the original OCP to the NLP problem with FMU, is listed in

Table A1. Problem Formulas List.

Problem Formula
OCP	Performance Index	$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+{\int }_{t_0}^{t_f}L(\mathit{x}\left(t\right),\mathit{u}\left(t\right),t)dt,$$
	The State Equations	$$\dot{\mathit{x}}\left(t\right)=f(\mathit{x}\left(t\right),\mathit{u}\left(t\right),t),t\in [t_0,t_f],$$
	Boundary Conditions	$$\mathit{\varphi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)=0,$$
	Inequality Constraints	$$\mathit{C}\left(\mathit{x}\right(t),\mathit{u}(t),t)\le 0.$$
NLP	Performance Index	$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+{\displaystyle \frac{t_f-t_0}{2}}\sum _{i=0}^N{\omega }_{i}L({\mathit{X}}_i,{\mathit{U}}_i,T_i),$$
	The State Equations	$${∥\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i-{\displaystyle \frac{t_f-t_0}{2}}f({\mathit{X}}_k,{\mathit{U}}_k,{\tau }_k)∥}_{\infty }\le \mathit{\delta }, k=0, 1, \dots , N,$$
	Boundary Conditions	$$\mathit{\varphi }({\mathit{X}}_0,{\mathit{X}}_N,t_0,t_f)\le \mathit{\delta },$$
	Inequality Constraints	$$\mathit{C}({\mathit{X}}_k,{\mathit{U}}_k,T_k)\le 0, k=0, 1, \dots , N,$$
NLP with FMU	Performance Index	$$J=\mathsf{\Phi }(\mathit{x}\left(t_0\right),t_0,\mathit{x}\left(t_f\right),t_f)+{\displaystyle \frac{t_f-t_0}{2}}\sum _{i=0}^N{\omega }_{i}L({\mathit{X}}_i,{\mathit{U}}_i,T_i),$$
	The State Equations	$${∥\sum _{i=0}^N{D}_{ki}{\mathit{X}}_i-{\displaystyle \frac{t_f-t_0}{2}}{\dot{\mathit{X}}}_{\mathrm{FMU},i}∥}_{\infty }\le \mathit{\delta }, k=0, 1, \dots , N,$$
	Boundary Conditions	$$\mathit{\varphi }({\mathit{X}}_0,{\mathit{X}}_N,t_0,t_f)\le \mathit{\delta },$$
	Inequality Constraints	$$\mathit{C}({\mathit{X}}_k,{\mathit{U}}_k,T_k)\le 0, k=0, 1, \dots , N,$$

.

4. Simulation Results

This section gives two cases using the improved method proposed in the last section. The three-axis case results allow us to validate the improved method using the same case in the reference research [27]. Then, the six-axis case results are displayed.

4.1. The Manutec R3 Model 2

Manutec R3 is an industrial robot with 6 degrees of freedom; it is said that the first 3 d.o.f are responsible for the position and the last 3 d.o.f for the orientation of the tool center point frame [27]. For simplicity, the Manutec R3 model 2 (R3M2) focused on the first 3 d.o.f case. The relative physical parameters are displayed in [26], the robotic manipulator with no load is taken into consideration, and the terminal time $t_f$ is set as a free variable to find the shortest working time. There are some conditions for this, as follows:

The initial state conditions, which give the positions of the start point and the end point and angular velocities on both points, are:

$$\mathit{q}\left(0\right)=\left(\begin{array}{c}0.00\\ -1.50\\ 0.00\end{array}\right), \mathit{q}\left(t_f\right)=\left(\begin{array}{c}1.00\\ -1.95\\ 1.00\end{array}\right),$$

(36)

$$\dot{\mathit{q}}\left(0\right)=0, \dot{\mathit{q}}\left(t_f\right)=0.$$

(37)

The boundary conditions give constant boundaries of the controls and the states. It is noted that the motors in the robot are modeled as ideal voltage–torque converters without any dynamic effects; in other words, there is a linear relationship between voltage and torque. The linear converter coefficient $\mathit{L}\mathit{C}$ can be found in [26].

The controls boundaries of voltage ${\mathit{u}}_{max}$, and the linear converter coefficient $\mathit{L}\mathit{C}$ are shown as

$$u_{i,max}=7.5(\mathrm{V}), i=1, 2, 3.$$

(38)

$$LC\left[3\right]={[-126,252,72]}^T(\mathrm{N}·\mathrm{m}/\mathrm{V}).$$

(39)

The torques boundaries ${\mathit{\tau }}_{max}$ can be calculated by

$${\tau }_{i,max}=LC\left[i\right]\times u_{i,max}(\mathrm{N}·\mathrm{m}), i=1, 2, 3.$$

(40)

The states’ boundary conditions including the boundaries of positions ${\mathit{q}}_{max}$ and angular velocities ${\dot{\mathit{q}}}_{max}$ are as follows:

$$\left\{\begin{array}{cc}{q}_{1,max}=2.97\left(\mathrm{rad}\right), \hfill & {\dot{q}}_{1,max}=3.00\left(\mathrm{rad}/\mathrm{s}\right),\hfill \\ q_{2,max}=2.01\left(\mathrm{rad}\right), \hfill & {\dot{q}}_{2,max}=1.50\left(\mathrm{rad}/\mathrm{s}\right),\hfill \\ q_{3,max}=2.86\left(\mathrm{rad}\right), \hfill & {\dot{q}}_{3,max}=5.20\left(\mathrm{rad}/\mathrm{s}\right),\hfill \end{array}\right.$$

(41)

The LPM method is initialized with 20 and 100 nodes, and the initialization of the state ${\tilde{\mathit{q}}}_n$, ${\dot{\tilde{\mathit{q}}}}_n$ on each node is set as:

$${\tilde{\mathit{q}}}_n=\mathit{q}\left(0\right)+{\displaystyle \frac{n-1}{N-1}}(\mathit{q}\left(t_f\right)-\mathit{q}\left(t_0\right)),$$

(42)

$${\dot{\tilde{\mathit{q}}}}_n=\dot{\mathit{q}\left(0\right)}+{\displaystyle \frac{n-1}{N-1}}(\dot{\mathit{q}}\left(t_f\right)-\dot{\mathit{q}}\left(t_0\right)).$$

(43)

The initial control ${\tilde{\mathit{u}}}_n$ on each node is defined as zero,

$${\tilde{\mathit{u}}}_n\left(t\right)=\mathit{0}.$$

(44)

where n represents the sequence number of the node, N represents the number of all nodes, ${\tilde{\mathit{q}}}_n\left(t\right)$ represents the initial angular position on the node n, and ${\dot{\tilde{\mathit{q}}}}_n\left(t\right)$ represents the initial angular velocity on the node n.

After setting the initialized conditions of the problem, the result solutions are shown in Figure 4 and

Table 1. Comparison of results of the improved method with FMU and the reference research.

Model(R3M2)	Nodes of LPM Method	Total Cost Time of CPU (s)	Performance Index (Terminal Time ${\mathit{t}}_{\mathit{f}}$) (s)
REF	20	2.221	0.498
	100	196.933	0.495
FMU	20	2.144	0.498
	100	125.136	0.495

. It should be noted that the REF model of R3M2 uses the analytic RHS equations, and the FMU model of R3M2 uses the black-box model of FMU to replace the analytic RHS equations.

Figure 4 displays the solution of the trajectory planning problem; the figures of desire actuator current, joint position and joint velocity are given in the time domain. A different number of time nodes are considered. From Figure 4, the results of the improved method with FMU coincide with the results of the reference research completely when the LPM method with 20 nodes is applied. When it comes to applying the LPM method with 100 nodes, the results of both are still highly consistent. What is more, from Table 1, the optimized performance indexes are all the same. Although the proposed method needs extra time to analyze FMU, which is a black-box function, so that the value of FMU can be applied in the DCM as the RHS equation, the total cost time of the CPU of the proposed method and the reference research is of the same order of magnitude. Furthermore, the decrease in nodes in the LPM method has little impact on the result of performance index, but can greatly improve the efficiency of calculation. This property can help us to obtain good initial values or analyze the structure of results quickly.

To sum up, the proposed method in this work is not only validated, but also proved to be highly efficient.

4.2. The Manutec R3 Model 1

The Manutec r3 model 1 (R3M1) is a kind of six-axis robotic manipulator constructed by Otter and Türk. However, the dynamics of the model are not presented in the literature. The proposed method in this work fills this gap. The exact boundary conditions and constraints are displayed in

Table A2. The OCP constraint conditions of the Manutec R3 Model 1.

	Num	Joint 1	Joint 2	Joint 3	Joint 4	Joint 5	Joint 6
Vars
$\mathit{q}\left(t_0\right)$		0.00	−1.50	0.00	0.00	0.00	0.00
$\mathit{q}\left(t_f\right)$		1.00	−1.95	1.00	0.50	1.00	0.50
$\dot{\mathit{q}}\left(t_0\right)$		0.00	0.00	0.00	0.00	0.00	0.00
$\dot{\mathit{q}}\left(t_f\right)$		0.00	0.00	0.00	0.00	0.00	0.00
${\mathit{u}}_{max}$		7.50	7.50	7.50	7.50	7.50	7.50
${\mathit{q}}_{max}$		2.97	2.01	2.86	3.63	2.27	3.15
${\dot{\mathit{q}}}_{max}$		3.00	1.50	5.20	3.40	4.30	3.70
$\mathit{L}\mathit{C}$		−126	252	72	−24.8	21.4	−8.6

to solve this problem.

The terminal time $t_f$, the number of nodes, the discrete initial states and the discrete initial controls are set as usual. The loads are set to 0 kg, 7 kg, and 15 kg, respectively. The results of the proposed method are displayed in Figure 5 and

Table 2. Comparison of results of the improved method with different loads.

Loads of R3M1	Nodes of LPM Method	Total Cost Time of CPU (s)	Performance Index (Terminal Time) (s)
0 kg	20	16.644	0.476
	100	468.226	0.475
7 kg	20	8.144	0.497
	100	383.118	0.496
15 kg	20	15.319	0.522
	100	469.191	0.521

, and the impact of different loads is analyzed.

Figure 5 not only has the same content as Figure 4, but also displays the results of the R3M1 manipulators with different loads. It is found that, with the increase in the load, the optimized terminal time $t_f$ increases. Joints 4 and 5 are influenced by loads greatly.

What is more, the oscillating behavior of the controls caused by the discrete property is common for a collocation method. Therefore, a better approximation of the controls can be found by applying a huge number of nodes in the LPM method, or by taking the switching structure into account to transform the original one-stage problem to a multi-stage problem [38]. According to Table 2, the calculation time of the first plan increases exponentially. However, the switching structure can be generally figured out even with the LPM method with fewer nodes. Therefore, the second plan has broader application prospects.

5. Experiment

ROCR6, a type of six-axis robotic manipulator from the company Si Valley in China, is applied in the experiment. The related MDH parameters and kinetic parameters of the robotic manipulators are listed in the

Table 3. MDH parameters.

Link Number	Link Length a (m)	Link Offset D (m)	Link Twist $\mathit{\alpha }$ (rad)	Joint Angle $\mathit{\theta }$ (Rad)
1	0.1223	0	0	0
2	0	0	$\mathsf{\pi }$/2	−$\mathsf{\pi }$/2
3	0	−0.270	0	0
4	0.1233	−0.253	0	−$\mathsf{\pi }$/2
5	0.1071	0	$\mathsf{\pi }$/2	0
6	0.0991	0	−$\mathsf{\pi }$/2	0

and

Table A3. Kinetic parameters.

	Num	Link 1	Link 2	Link 3	Link 4	Link 5	Link 6
Vars
Mass (kg)		1.6895	3.3798	1.4040	0.7111	0.7111	0.1043
Mass Center w.r.t. MDH Coordinates (m)	X	0.0001	−0.2324	−0.1710	0.0001	0.0001	−0.0003
	Y	−0.0005	0.0000	0.0001	−0.0142	−0.0241	0.0046
	Z	−0.0132	−0.2331	0.0226	−0.0081	−0.0081	−0.0535
Inertia Tensor w.r.t. Mass Center (kg·m2)	$I11$	0.0053	−0.1045	0.0028	0.0033	0.0011	−0.0002
	$I12$	0.0000	0.0001	0.0000	0.0000	0.0000	0.0000
	$I13$	0.0000	0.1125	−0.0209	0.0000	0.0000	0.0000
	$I22$	0.0051	−0.1732	0.0803	0.0002	0.0022	−0.0002
	$I23$	0.0000	0.0000	0.0000	0.0001	0.0002	0.0000
	$I33$	0.0018	−0.0544	0.0584	0.0013	0.0011	0.0003

.

In the next step, the improved DCM is applied to solve the problem of the time-optimal trajectory of the manipulator. The OCP description is given as

Table A4. The OCP Description.

Performance Index			$\mathit{J}={\mathit{t}}_{\mathit{f}},$
The State Equations			${∥{\displaystyle \sum _{\mathit{i}=\mathit{0}}^{\mathbf{N}}}{\mathit{D}}_{\mathit{k}\mathit{i}}{\mathbf{X}}_i-{\displaystyle \frac{{\mathit{t}}_{\mathit{f}}-{\mathit{t}}_{\mathit{0}}}{2}}{\dot{\mathit{X}}}_{\mathit{F}\mathit{M}\mathit{U},\mathit{i}}∥}_{\infty }\le \mathit{\delta }, \mathit{k}=\mathit{0}, \mathit{1}, \dots , \mathbf{N},$
Constraint Conditions		Num	Joint 1	Joint 2	Joint 3	Joint 4	Joint 5	Joint 6
	Vars
	$\mathit{q}\left(t_0\left(\mathrm{rad}\right)\right)$		0.00	0.00	0.00	0.00	0.00	0.00
	$\mathit{q}\left(t_f\right)\left(\mathrm{rad}\right)$		1.07	0.26	0.52	0.78	1.57	0.00
	$\dot{\mathit{q}}\left(t_0\right)\left(\mathrm{rad}/\mathrm{s}\right)$		0.00
	$\dot{\mathit{q}}\left(t_f\right)\left(\mathrm{rad}/\mathrm{s}\right)$		0.00
	${\mathit{u}}_{max}\left(\mathrm{A}\right)$		0.21
	${\mathit{q}}_{max}\left(\mathrm{rad}\right)$		2.67
	${\dot{\mathit{q}}}_{max}\left(\mathrm{rad}/\mathrm{s}\right)$		1.74
	$\mathit{L}\mathit{C}(\mathrm{N}·\mathrm{m}/\mathrm{A})$		101

.

It is noted that the state equations referring to (30) can be expressed after constructing the FMU of ROCR6. The unit of q is rad, the unit of $\dot{\mathit{q}}$ is rad/s, and the unit of u is A. The output torques of actuators are proportional to the current u, where the proportionality coefficient is 101 N·m/A.

The resulting minimum time is 0.903 s. The resulting trajectory of the improved PSOPT is input into ROCR6, which was compared with another two cases in this experiment to prove the validity of the result. One is inputting the default trajectory built into the manipulator and another is inputting the same path with a shorter time. The comparison results are shown in the next section.

5.1. Comparison with the Default Trajectory

The default trajectory applies the maximum velocity trajectory planning method. As Figure 6 shows, the joint position of each joint is given, and the right side of the figure identifies the joint number. The different maximum velocity trajectories of each joint are shown as red lines in Figure 6. The experiment shows that the maximum velocity can differ from 0.163 rad/s to 1.501 rad/s. The lower bound of the maximum velocity is not fixed, but there is an interruption caused by self-collision in the upper bound case, which means a bigger upper bound of more than 1.501 rad/s is not allowed in the system. However, the OCP case achieves a maximum velocity more than 1.501 rad/s. This shows that the dynamics of the system are taken into consideration properly, and realizes better dynamic performance.

5.2. Comparison with the OCP Path with Shorter DTT

Several cases with a trajectory Desired Terminal Time (DTT) shorter than or equal to 0.903 s are taken into consideration in the experiment, including 0.451 s, 0.541 s and 0.903 s. The experiments are executed three times for each case. The Actual Terminal Time (ATT) is shown in

Table 4. Table of DTT and ATT in the experiments.

DTT (s)	Order	ATT (s)	Rate
0.903	1	1140	26.25%	31.79%
	2	1180	30.68%
	3	1250	38.43%
0.541	1	0.840	55.27%	64.51%
	2	0.930	71.90%
	3	0.900	66.36%
0.903	1	0.820	81.82%	81.08%
	2	0.790	75.17%
	3	0.840	86.25%

. The executive time increased by 81.08%, 64.51% and 31.79%. Because the model is not exactly the same as the experimental robotic manipulator, ATT is always longer than DTT. As shown in Figure 7a, the joint position of each joint in each experiment is given, and we find that the shorter the time, the higher the slope of the trajectory. What is more, when DTT = 0.451 s, the trajectory of joint 1 is interrupted half way because of self-collision, which means the trajectory with a time shorter than 0.451 s is not allowed.

The current of each joint actuator is shown in Figure 7b, and the current over 0.21 A is marked in Figure 7c. It is found that the current of the standard OCP trajectory is stable; only four sampling points in joint 1 are over the rated maximum. As a result, the motion is quick and smooth without too much vibration. However, the current of the OCP path with a shorter DTT becomes not stable any more, and the period of current over rated maximum increases greatly, which causes violent vibration. It was found that the case with the same path and shorter time requires higher voltage to achieve the trajectory, which may go beyond the nominal voltage and cause a vibration. The default case takes a much longer time to finish the motion, and the voltage curve is more smooth, which means the capabilities of the manipulator are not fully utilized. As the result, the validity of the proposed method is proved.

6. Conclusions

An improved method based on the traditional collocation method with FMU is proposed in this paper. This proposed method is validated by comparing the results to the simulation accomplished in PSOPT, and is proved to be highly efficient. At the same time, it is demonstrated that this method is available to solve the six-axis robotic manipulator problem, which was not solved with the traditional method before. By considering different loads in the six-axis case, there is an obvious impact on joints 4 and 5, which makes the input voltage of the actuator shift between the minimum and maximum values more frequently. A motors’ life might therefore be affected. The experiments show that the OCP trajectory can stimulate the performance of the robotic manipulator by achieving a larger maximum of velocity, and extend the motors’ life by controlling the currents under the rated maximum. Moreover, the calculation time increases exponentially when applying the LPM method with more nodes. Therefore, applying the switching structure, each stage of which uses the LPM method with fewer nodes, is more useful to eliminate the oscillating behavior of the controls.

For further research, we can study the effects of other different numerical integral methods besides LPM in the DCM on the solution, and study how to improve the proposed method so that it can be applied for real-time implementations. These studies may have great value.