Rojas Quintero Villalobos Chin Santibañez Optimal Control of Robotic System Using FE For Time Integration Covariant Control Equations

Received July 13, 2021, accepted July 19, 2021, date of publication July 26, 2021, date of current
version August 2, 2021.

Digital Object Identifier 10.1109/ACCESS.2021.3099131
Optimal Control of Robotic Systems Using Finite

Elements for Time Integration of Covariant
Control Equations
JUAN ANTONIO ROJAS-QUINTERO 1 , JORGE VILLALOBOS-CHIN 2,
AND VICTOR SANTIBANEZ 2 , (Member, IEEE)

1 CONACYT/Tecnológico Nacional de México/I. T. Ensenada, Ensenada 22780, México
2 Tecnológico Nacional de México/I. T. La Laguna, Torreón 27000, México
Corresponding author: Juan Antonio Rojas-Quintero ([email protected])
This work was supported by in part by the Secretaría de Educación Pública (SEP) and Consejo Nacional de Ciencia y Tecnología
(CONACYT) under Grant A1-S-29824, and in part by the Tecnológico Nacional de México (TecNM) projects.
ABSTRACT We used an optimal control method involving covariant control equations as optimality
conditions, to command the actuators of robot manipulators. These form a coupled system of second order
nonlinear ordinary differential equations when associated with the robot motion equations. By solving this
system, the control action required to take the robot from an initial to a final state is optimized in a prescribed
time. However, the target set of equations exhibited stiffness. Therefore, an adequate solution could only be
found for short trajectory durations with readily available numerical methods. We examined a time dis-
cretization procedure based on cubic and quintic Hermite finite elements which exhibited superconvergence
properties for interpolation. This motivated us to develop a time integration algorithm based on Hermite’s
technique, where motion and control equations were perturbed to solve the optimal control problem. The
optimal motion of a robotic manipulator was simulated using this algorithm. Our method was compared with
a commercial differential equations solver on the basis of specific indicators. It outperformed the commercial
solver by effectively solving the stiff set of equations for longer trajectory durations, with the cubic elements
performing better than the quintic ones in this sense. The convergence analysis of our method confirmed that
the quintic elements are more precise at the cost of increased computational burden, but converge at a lower
rate than expected. Controlled motion experiments on a robotic manipulator validated our methodology.
Trajectories were smoothly tracked and results exposed further methodology improvements.
INDEX TERMS Differential equations, finite element methods, nonlinear dynamical systems, numerical
simulation, optimal control, robot control.
I. INTRODUCTION simpler subdomains that are called finite elements to achieve

The finite element method (FEM) has become a favored spatial discretization. The solution of a particular differen-
technique for solving engineering problems relating to the tial equation is then approximated over each finite element,
estimation of stress, strain or wear in elastic materials (see ref- regarded as a domain for which boundary values should be
erences [1]–[5] for some examples). Its origins can be linked met. The whole system is modeled by assembling all of the
to the works of Richard Courant back in 1943 [6]. However, finite elements into a system of algebraic equations.
it has been advocated that the FEM can easily be used as The FEM has proved to be computationally accurate, effi-
a general approximation method for solving boundary-value cient and stable [7]–[11]. It is therefore natural that the FEM
problems that arise in science and engineering applica- has found applications in the areas of systems control and
tions [7], [8]. The basic idea lies in subdividing a sys- robotics. In recent years, FEM has been widely used in
tem, regarded as the domain, into smaller and geometrically the modeling and control of distributed parameter systems.
The method became important in this field, where systems
The associate editor coordinating the review of this manuscript and can be modeled using partial differential equations (PDE),
approving it for publication was Okyay Kaynak . and the FEM provides a numerical solution that is useful
This work is licensed under a Creative Commons Attribution-NonCommercial-NoDerivatives 4.0 License.
104980 For more information, see https://creativecommons.org/licenses/by-nc-nd/4.0/ VOLUME 9, 2021
J. A. Rojas-Quintero et al.: Optimal Control of Robotic Systems Using FEs for Time Integration
for, among other things, evaluating the performance of the rigid multibody systems commonly involves nonlinear prob-
controllers. For example, the finite element (FE) formulation lem formulations. In this context, shooting procedures are
presented in [1], is used to achieve the active bending and instead commonly used [11], [18]. However, a finite element
torsional control of functionally gradient material plates that description has proven to be computationally advantageous
are subject to a thermal gradient. In this work, the bending and for the direct integration of the dynamic equations governing
torsional vibration suppression of the plates are accomplished multibody systems motions. For example, finite elements
by using a feedback algorithm. Another good example may have previously been used in [11], [19] for the simulation
be [12], where an iterative procedure is proposed to update of multibody systems motion, to solve nonlinear differential
the FE model of a system. The poles and zeros of a frequency equations arising in biomechanics and robotics. It was shown
response reduced order model are determined from a mea- that the required number of steps and CPU-time are lower
sured frequency response function. The obtained model is for such a FEM-based algorithm when compared to popular
applicable to the determination and control of unmeasurable Runge-Kutta (fourth order) and Newmark methods. One of
performance variables, which is relevant in the context of the advantages of the FEM is that boundary configurations are
model based control. The robust output regulation of linear exactly satisfied in the solution. This property is particularly
boundary control systems is studied in [13]. Linear finite relevant in robotics applications. These promising results led
elements were used to approximate solutions to sets of PDE us to develop a time integration method based in the Hermite
such as convection and beam equations. The study held finite elements piecewise basis functions, to solve the OCP
in [14] presents a simulation framework that uses the output of a robot manipulator.
signals retrieved from a FEM-based solution of heat flow Optimal control methodologies are aimed at finding sys-
equations. This would allow for the testing of different control tem controls that optimize a predefined performance index
strategies on an additive manufacturing process in a virtual (usually also called objective function) according to specific
environment. restrictions. Traditionally speaking, solutions to OCP tend
Soft robotics is a field where the FEM has found appli- to fall into two main frameworks: Pontryagin’s Maximum
cations for the analysis and control of flexible link manip- Principle (PMP) and the Bellman approach which led to
ulators. For example, a numerical integration strategy based Dynamic Programming (DP). Generally, PMP-related meth-
on a FEM with a numerical optimization based on Lagrange ods involve ordinary differential equations (ODE) whereas
multipliers was conducted in [3]. This was done to obtain the DP-based methods involve PDE. PMP-based methods are
forward and inverse kinematic models of soft manipulators therefore at a numerical advantage and are often easier to
in order to perform position control. The FEM proved useful use than DP-based methods in order to solve OCP [20].
since the formulation of the problem heavily relied in the use With these two methodologies, it is possible to perform
of continuum mechanics. A nonlinear finite element-based optimal path planning where the optimal controls, according
procedure was studied in [2] and applied to the control of to a specific performance index, are obtained. Alternatively,
a single-link flexible manipulator with hydraulic actuation. the Linear Quadratic Regulator (LQR), which defines a class
This was achieved by using what the authors called Virtual of optimal control methods, operates differently by finding
Decomposition Control which enabled the handling of the the controller parameters (usually gains) that will optimize a
dynamics and control of the hydraulic actuator and the flexi- specified performance index [21]. It should be noted however
ble link separately. As a more traditional application, the FEM that the LQR methodology is specifically designed for linear
was also used to assist the design of soft robots, for example, systems because optimal controller parameters are obtained
to model their dynamics and predict the controllability of through the solution of a linear Riccati equation. Therefore,
the points of interest [15]. In contrast with the approach a much more general approach can be found in what is known
presented in this document, in all of these previous examples as State-Dependent Riccati Equation (SDRE) control. SDRE
the FEM is employed to discretize space as opposed to time, control also requires the solution of a linear Riccati equa-
using beam-like models to estimate link deformation during tion to find optimal controller parameters. However, nonlin-
motion. earities are fully captured and dealt with through extended
An early development involving the FEM for time dis- linearization [22], [23].
cretization in the context of optimal control was proposed by In this work, we propose a FE-based method which can be
Cavin and Tandon [16] where heat PDE subject to boundary applied to obtain the solutions to OCP in which stiff ODE
conditions were solved using a space-time finite element arise, which is usually the case for PMP-related develop-
formulation. Later, a space-time finite element approximation ments [24]. Due to the FE nature of our method, it is not
scheme for the optimal control problem (OCP) of systems excluded that our algorithm can be adapted to solve PDE that
governed by fractional order PDE was proposed in [17]. arise in DP. We will focus in the ODE case of the optimal
The approach considers piecewise constant discontinuous control methodology described in [25] which involves covari-
Galerkin time discretization and linear finite elements for ant control equations. This methodology is similar to PMP,
spatial discretization of the state equation. Nonetheless, only however, the resulting set of nonlinear ODE are of the sec-
a few works make use of the FEM to discretize time in the ond order instead, but also exhibit stiffness. The covariant
context of multibody systems dynamics. Control of general control equations involved, act as control restrictions that
VOLUME 9, 2021 104981

bound their intensities. It has been shown that these covariant The robot velocity field is a linear function of the variables
control equations define adjoint states in the framework of qi time derivatives q̇i (where a superposed dot shall denote
PMP [26] thus rendering them compatible with a wide range a time differential). The kinetic energy function K (q, q̇) is
of optimal control solution methods. Our proposed FE-based quadratic and strictly convex. It is defined with the positive
algorithm should also be of interest for solving ODE that definite Hessian M (q), commonly called mass tensor, which
arise in other optimal path planning and kinematics control is regarded as a Riemannian metric with components Mij .
strategies [27], [28]. It expresses as
The organization of the paper is the following. First, 1
we will establish the selected optimal control methodology in K = Mij q̇i q̇j . (1)
2
section II. This methodology results in a two-point boundary-
A gravitational potential V (q), defined as the product
value problem (TPBVP) and involves covariant control equa-
between mass, local gravitational field intensity, and center
tions which are conjugate to the robot motion equations,
of mass height models gravity actions. A torque ui is exerted
composing a system of second order ODE (see section II).
between two consecutive links by the ith actuator. For any vir-
The solution of this TPBVP provides the optimal path along
tual variation δqi , the actuators virtual work ui δqi is invariant,
with optimal torques required by such path. We will then
therefore ui are the covariant components of a torque tensor.
present a time discretization procedure based on Hermite
Robot motion is governed by the n Euler-Lagrange equa-
finite elements piecewise basis functions in section III. There,
tions
two simple examples will illustrate the technique benefits and
d ∂K ∂K ∂V

accuracy for our purposes. A time integration procedure will − i = ui − i ,
also be proposed, followed by a one dimensional nonlinear dt ∂ q̇ i ∂q ∂q
example. Next, we carry out optimal motion simulations which upon using (1), lead to the explicit expression
in section IV. For this purpose, we begin by proposing an
Mij q̈j + 0ikl q̇k q̇l + ∇i V = ui . (2)
optimization procedure in the form of an algorithm, which
∂V
involves the perturbation of motion and control equations to where ∇i V = ∂qi
is the i th covariant derivative component
find a suitable finite element-based approximation. Optimal of the potential V and 0ikl are Christoffel symbols of the first
motion simulation examples are conducted to compare our kind which derive from the Mij components by
method with a commercial ODE solver. It is shown that our
1 ∂Mik ∂Mil ∂Mkl

method outperforms this solver by being able to produce 0ikl = + − . (3)
longer simulations. We conclude section IV by performing 2 ∂ql ∂qk ∂qi
a convergence analysis of our proposed method. Our simu- Let us introduce the coefficients of the inverse metric ten-
lations are then followed by controlled motion experiments sor M ij , which lead to the Christoffel symbols of the second
conducted on a robotic serial manipulator in section V. kind when multiplied by (3)
We present our control scheme in this section. Additionally, j
we carry out a robustness analysis of our control strategy. The 0 kl = M ji 0ikl . (4)
conducted experiments verify the tractability of our approach Invoking the above, equations (2) transform into
and allow us to identify ways to improve our optimal control
j

methodology. Mij q̈j + 0 kl q̇k q̇l + M jk ∇k V = ui ,
where the term M jk ∇k V = ∇ j V is the j th contravariant

II. OPTIMAL CONTROL METHODOLOGY derivative component of potential V . Upon renaming indices
We have followed the optimal control methodology presented in the above equation, the contravariant torque tensor compo-
in [25]. Therefore, this section summarizes the main ideas of nents reveal
the cited methodology. The reader is referred to the previously
q̈i + 0jk q̇ q̇ + ∇ i V = ui .
i j k
(5)
mentioned document for more details. First, the dynamics of
a robotic manipulator are derived. The optimal control of such Note that these contravariant torque components were
a system then consists in minimizing an integral functional of retrieved by following the rules of tensor calculus in a Rie-
the joint torques. This procedure leads to the establishment of mannian manifold, where ui = M ik uk .
covariant control equations regarded as optimality conditions.
B. OPTIMAL CONTROL
The required torque intensities to bring the robotic system
A. SYSTEM DYNAMICS
from an initial state x0 = x(0), to a final state x1 = x(T )
We consider serial robotic manipulators with n degrees of
in a prescribed time T , can be constrained by minimizing an
freedom (DOF) which are described by n configuration
integral functional of the type:
parameters qi , and operated by n actuators placed between
two consecutive segments. In the following, tensor nota- ZT
tion and the Einstein summation convention on repeated J (u) = γ (u(t)) dt.
indices [29] will be used to establish the robot dynamics. 0
104982 VOLUME 9, 2021

This integral functional is regarded as the performance index. nonlinear ODE in the dual variables (qi , ui ) for resolving the
The integrand γ is commonly called cost function and is optimal trajectories and torques:
generally chosen to be convex so that the integral functional J
is reasonable to minimize. It has been noticed that γ is often Mij q̈j + 0ikl q̇k q̇l + ∇i V − ui = 0 (10a)
chosen so that d̂2 ui
+ Rkjil q̇j q̇l uk + ∇i ∇l V ul = 0. (10b)
1 dt 2
γ = Aij ui uj ,
2 Solving the system of equations (10) minimizes the perfor-
where Aij is a diagonal matrix with constant coefficients, mance index (6). Note that qi and ui are independent vari-
usually unitary [11], [30]–[33]. However such a cost func- ables and that 4n boundary conditions are required. These
tion is not invariant under a change of coordinates. Instead, are generally known values of initial and final positions and
as proposed in [25], we select the invariant cost function velocities: qi (0), qi (T ), q̇i (0) and q̇i (T ), where T denotes
γ = 12 ui ui = 12 M ij ui uj so that the performance index the prescribed trajectory duration. Using these boundary
becomes constraints to solve the ODE system (10), the initial OCP
ZT transforms into a TPBVP. Note also that no boundary con-
1
J (u) = ui ui dt, (6) ditions are imposed on ui parameters, which are regarded
2 as conjugate parameters that directly provide motion torque
0
requirements.
ui
where ui and are respectively expressed by equations (2)
and (5) and are functions of parameters qi , q̇i and q̈i . Note III. TIME INTEGRATION METHOD
that γ is selected to be a convex and invariant cost function, In order to solve the nonlinear ODE system (10) in an interval
involving the inverse mass tensor components M ij , to which [0, T ], we perform a time finite element discretization. This
the Euler-Lagrange equations can be applied as means that the solution domain is discretized and represented
d2 ∂γ d ∂γ ∂γ as a mesh of time elements. The unknown variables (con-

− + i = 0. (7) figurations and controls in our case) behavior is approxi-
dt 2 ∂ q̈i dt ∂ q̇ i ∂q
mated over each element by continuous functions expressed
By applying the rules of tensor calculus, equations (7) in terms of the nodal values of the unknown and its time
transform into the covariant control equations derivatives [16] (more on this along the section). Hermite
d̂2 ui piecewise functions [9] are defined over each time finite
+ Rkjil q̇j q̇l uk + ∇i ∇l V ul = 0. (8) element. The collection of these interpolation functions for
dt 2
the whole domain provides a piecewise approximation to the
The reader is referred to [25], where the complete analysis
sought variables.
of how equations (7) lead to (8), is conducted. These control
equations are nonlinear second order ODE that restrict the
A. PIECEWISE HERMITE FINITE ELEMENTS
torques during motion. They are regarded as optimality con-
INTERPOLATION
ditions, and it has been shown that these equations define the
adjoint states in the framework of PMP [26]. The left hand Hermite finite elements (HFE) are compact piecewise con-
side of these equations involve: the second covariant time tinuous functions that can be connected in a continuously
derivative of the covariant torque tensor components ui in the differentiable way. Therefore, the values of their derivatives
first term (see equation (2)); the Riemann-Christoffel curva- are also used to characterize the functions that they approxi-
ture tensor in the second term; the second covariant derivative mate [9]. This property is particularly useful for our approach
of potential V in the third term; and contravariant torque because the targeted optimal control methodology requires a
tensor components ul in the third term (see equation (5)). continuously differentiable solution. Cubic HFE (CHFE) are
Definitions for these objects can be found in [25], [29] and formed by functions φ and ψ, shown in Figure 1a, and defined
have the following expressions: by
(1 − |t|)2 (1 + 2 |t|) ∀ |t| ≤ 1

∂0ijk
!
d̂2 ui φ(t) =
= üi − − 0ji 0ml uk q̇j q̇l
m k
0 ∀ |t| > 1;
dt 2 ∂ql
t (1 − |t|)2 ∀ |t| ≤ 1

ψ(t) =

− 0ijk uk q̈j + 2u̇k q̇j ; 0 ∀ |t| > 1.
∂0jlk ∂0jik One can construct higher order HFE functions by follow-
Rkjil −= + 0jlm 0mi
k
− 0jim 0ml
k
;
∂qi ∂ql ing prescriptions found in [10]. For example, quintic HFE
∂ 2V ∂V (QHFE, using fifth degree polynomials) are formed by func-
∇i ∇l V = l i − 0ilm m . (9) tions φ, ψ and χ, shown in Figure 1b, and defined by
∂q ∂q ∂q
(1 − |t|)3 1 + 3|t| + 6t 2

Control equations (8) can be associated to the motion equa- ∀ |t| ≤ 1
φ(t) =
tions (2), thus providing a coupled system of 2n second order 0 ∀ |t| > 1;
VOLUME 9, 2021 104983

FIGURE 1. Hermite finite elements basis functions: (a) cubic; (b) quintic.
t (1 − |t|)3 (1 + 3|t|) ∀ |t| ≤ 1

ψ(t) = Let us remark for now that coefficients aip , bip and cip
0 ∀ |t| > 1; respectively correspond to the values of parameters qi , their
(1
t 2 (1 − |t|)3 ∀ |t| ≤ 1 time derivatives q̇i , and their second time derivatives q̈i at
χ(t) = 2 time instance t = p h. Analogously, coefficients xpi , yip and zip
0 ∀ |t| > 1. correspond to the values of parameters ui , their time deriva-
Considering the prescribed time T in the functional inte- tives u̇i , and their second time derivatives üi at time instance
gral (6), let us construct a uniform time mesh by dividing the t = p h.
domain [0, T ] into N equal pieces such that the size of each
time finite element is of h = T /N , by the instances tp = p h. B. INTERPOLATIONS ACCURACY
We define the basis functions for 0 ≤ p ≤ N as Let us now consider a one dimensional example in the
approximation of the circular configuration parameter q(t) =

t
φp (t) = φ −p sin t. After following the piecewise HFE interpolation pro-
h

t
cedure presented in III-A, the error e(t) = qh (t) − sin t of
ψp (t) = ψ −p the interpolations and their convergence rates are measured
h as suggested by [9]. Let us introduce the norms

t
χp (t) = χ −p , (11)
s
Z T
h
kek3 = (e(t))2 + h2 (ė(t))2 dt
where χp (t) is used only for the QHFE case. The number of 0
basis functions corresponds to 2N +2 for the CHFE case, and for the CHFE case, and
to 3N + 3 for the QHFE case. Basis functions that overflow s
Z T
the domain [0, T ] must be truncated on the left for p = 0 or
on the right for p = N . Each configuration parameter qi and kek5 = (e(t))2 + h2 (ė(t))2 + h4 (ë(t))2 dt
0
each control parameter ui can then be approximated by the
piecewise CHFE for the QHFE case. Errors for varied values of h are plotted
in the logarithmic scale. This allows to visualize convergence
N
X rates which correspond to the slope of the error logarithm,
qih = aip φp (t) + h bip ψp (t)
as a function of the logarithm of h. As shown by Figure 2a,
p=0
the norm of the error e(t) = qh (t) − sin t reveals to be in h4
N
X for the CHFE case and in h6 for the QHFE interpolation case.
uhi = xpi φp (t) + h yip ψp (t) , (12) These convergence rates are one degree higher than the order
p=0
of the chosen HFE interpolation and is a phenomenon known
or alternatively by the QHFE as superconvergence. This is due to the mesh uniformity and
N
X the choice of the error norm [9].
qih = aip φp (t) + h bip ψp (t) + h2 cip χp (t)
p=0 C. TIME INTEGRATION PROCEDURE
N
X When the torque history ui(t) is known, the system state
uhi = xpi φp (t) + h yip ψp (t) + h2 zip χp (t) . (13) vector x i (t) = qi (t), q̇i (t) can be predicted at any time
p=0 instance t after its initial value x i (0) = qi (0), q̇i (0) by
104984 VOLUME 9, 2021

FIGURE 2. Accuracy of Hermite finite elements interpolations. (a) Interpolations superconvergence: cubic elements convergence is in h4 and quintic
elements convergence is in h6 , where h is the step size. (b) Analytical solution q versus CHFE approximation qh of the nonlinear pendulum motion:
the CHFE approximation is as accurate as the analytical solution.
numerically solving the system motion equation (2). How- Steps (i) and (ii) of the above procedure constitute the HFE
ever, when an optimal trajectory is desired, the ODE sys- time discretization Algorithm 1 shown below. Note that the
tem (10) may be solved instead between boundary values above procedure works, as presented, with linear models.
x i (0) and x i (T ). Motion simulations may be conducted by In order to account for nonlinearities, a perturbation method
following the procedure: will be detailed in the next subsection III-D.
(i) approximate each configuration parameter qi by its Recall that v contains the values of parameters
interpolation qih , and each control parameter ui by its (qi , q̇i , q̈i , ui , u̇i , üi ) at time instances t = p h (last paragraph
interpolation uhi (12) for the CHFE case or (13) for the of III-A). Because of this important property, reconstruc-
QHFE case; tions (12) or (13) may not be needed if only the values of
(ii) express motion and control equations (10) at 2n(N + 1) these parameters, at nodes, suffice. As a consequence, other
time instances for the CHFE case or 3n(N + 1) time interpolations may be carried out in step (iv) if desired.
instances for the QHFE case (n is the number of system In the examples of IV-B and IV-C, boundary conditions
DOF and N is the desired nodes quantity): provide with the values of ai0 = qi (0), bi0 = q̇i (0), aiN =
j qi (T ), and biN = q̇i (T ).
Mij q̈h + 0ikl q̇kh q̇lh + ∇i V − uhi = 0
d̂2 uhi j D. ONE DIMENSIONAL NONLINEAR EXAMPLE
+ Rkjil q̇h q̇lh uhk + ∇i ∇l V ulh = 0;
dt 2 In order to verify the accuracy of our simulation method,
after some development, the previous system becomes we now study the periodicity and the precision over the period
an algebraic system with unknowns aip , bip , xpi , yip (for of the classical one-dimensional nonlinear pendulum. How-
CHFE discretization), cip and zip (for QHFE discretiza- ever, this system motion equation is nonlinear, which is also
tion) generally true for robotic systems. Therefore, a perturbation
method [35] is conducted in order to implement the above
Kv = r time integration algorithm. This is an iterative process where
where K is a square band matrix containing the basis increments 1q and 1u are respectively added to approximate
functions (11) evaluated at each node and scaled by the solutions q and u at each step. Upon perturbation, motion
system parameters, v is the vector of unknowns, and equations (2) develop in:
r contains the residual terms given by the boundary ∂M
ij
conditions; Mij q̈j + 1q̈j + 1qk q̈j
∂qk
(iii) solve the algebraic system Kv = r using generalized
matrix inversion techniques; + 0ikl q̇k q̇l + q̇l 1q̇k + q̇k 1q̇l
(iv) build the approximations qih and uhi ((12) for CHFE ∂0ikl k l m
discretization or (13) for QHFE discretization). + q̇ q̇ 1q
∂qm
In the above procedure, steps (ii) and (iii) are inspired ∂ 2V
from the inertial parameters identification method presented + ∇i V + j i 1qj = ui + 1ui , (14)
∂q ∂q
in [34]. Steps (ii) and (iii) show that our FE-based procedure
determines the whole trajectory at once by solving Kv = r. where squared ‘‘1’’ terms vanish.
VOLUME 9, 2021 104985

Algorithm 1 HFE Time Discretization Algorithm

1: procedure HFE(q, u)
2: for i ← 1, n do F n is the system number of DOF
3: if CHFE then
4: for m ← 0, 2N do F N is the desired nodes quantity
N
X
5: qi (tm ) ← qih (tm ) = aip φp (tm ) + h bip ψp (tm )
p=0
XN
6: ui (tm ) ← uhi (tm ) = xpj φp (tm ) + h yjp ψp (tm ) F Using equation (12)
p=0
Insert qih (tm ), uhi (tm ) into the target ODE

7:
8: end for
9: else if QHFE then
10: for m ← 0, 3N do F N is the desired nodes quantity
XN
11: qi (tm ) ← qih (tm ) = aip φp (tm ) + h bip ψp (tm ) + h2 cip χp (tm )
p=0
XN
12: ui (tm ) ← uhj (tm ) = xpi φp (tm ) + h yip ψp (tm ) + h2 zip χp (tm ) F Using equation (13)
p=0
Insert qih (tm ), uhi (tm ) into the target ODE

13:
14: end for
15: end if
16: end for
17: Establish the resulting algebraic system Kv = r subject to the required boundary values
18: end procedure
For the present case of a simple pendulum, the motion function sn [36], [37], also shown by Figure 2b. With our
equation is q̈ + ω2 sin q = 0 where only initial values for time methodology, the identified oscillations period was of
q(0) = π6 rad and q̇(0) = 0 rad s−1 are given as boundary 2.061 08 s, and is −3 × 10−7 s away from the period obtained
conditions. Note that no torques ui appear in this equation with the incomplete elliptic integral of the first kind F [38],
and thus, no 1u increment is introduced because it vanishes. Z π
4 dα 4 π π
Note also that the initial position implies a π3 rad amplitude
2
τ= = F , sin2 .
ω 0 ω
q
motion (see Figure 2b). For the present one-dimensional π
1 − sin2 12

sin2 α 2 12
case, the mass tensor has a unique constant component M ,
embedded in the ω parameter. Pendulum length and mass are Naturally, the above reported error can be decreased by fur-
such that ω = 3.102 s−1 . ther increasing the number of interpolation nodes or by using
For this example, CHFE discretization is used QHFE instead. The norm k k3 of the difference between the
(see III-A) in the time integration procedure proposed in III-C theoretical solution and our numerical solution reveals to be
with N = 100 interpolation nodes. The implemented pertur- in h4 , again illustrating the superconvergence of the method
bation method then leads to the following procedure: (the resulting graph of ln kek3 versus ln khk is similar to the
one shown by Figure 2a). This superconvergence is a conse-
(1) approximate the linear pendulum motion by solving the quence of Hermite’s technique and will also occur for more
equation q̈ + ω2 q = 0 with the same initial condi- complex single ODE models [9]. This example validates our
tions as the nonlinear pendulum, and let that solution be approach for solving second order nonlinear ODE.
called q1 ;
(2) according to the perturbed motion equation (14) approx- IV. OPTIMAL CONTROL OF ROBOTIC MANIPULATORS
imate the solution 1q1 of the linear ODE 1q̈1 + A. OPTIMIZATION PROCEDURE
ω2 cos(q1 )1q1 = −q̈1 − ω2 sin(q1 ) with vanishing Let us consider the nonlinear ODE system (10). Its solu-
initial conditions; tion provides optimal torques and trajectories. Recall that 4n
(3) improve the approximation q1 by setting q2 = q1 +1q1 ; boundary conditions qi (0), qi (T ), q̇i (0) and q̇i (T ) are usually
(4) iterate this process until k1qi k3 < 10−5 rad. given, thus establishing a TPBVP. Instead of solving the
A periodic solution qh was found after three iterations and is 2n second order ODE (10), we can insert the CHFE or QHFE
shown by Figure 2b. This numerical solution can be compared approximations (respectively given by equations (12) or (13))
to the analytical solution q expressed with Jacobi’s elliptic of q and u in (10), and conduct the time integration procedure
104986 VOLUME 9, 2021

proposed in III-C with the perturbation method introduced obtained (see last comment in III-A). The same applies
in III-D. Recall that the motion equation (10a) is perturbed for optimal torques ui (t) and their first and second time
as in equation (14). Analogously, control equation (10b) is derivatives. Algorithm 2 presents our HFE/Perturbation
perturbed according to equation (15): method that combines the HFE time discretization procedure
(Algorithm 1) with the perturbation method presented
dˆ2 ui dˆ2 1ui
+ above.
dt 2 dt 2
∂0ijk
!

− − 0 0
m k
1q̇ j l
q̇ u + q̇ j
1q̇ l
u Algorithm 2 Time Integration Algorithm Using HFE Time
k k
∂ql ji ml
Discretization and Perturbation Method
1: procedure HFE/Perturbation(q, u)
∂ 2 0ijk ∂0jim k
!
m ∂0ml
k
− − 0 − 0ji q̇j q̇l uk 1qp 2: Execute procedure HFE(q, u) to discretize the lin-
∂qp ∂ql ∂qp ml ∂qp earized system (10), with the same boundary values as
∂0 k for the nonlinear system
ij
− 0ijk 1q̈ uk + 21q̇ u̇k −
j j
1q l
q̈j
u k + 2q̇ j
u̇k 3: Solve the resulting algebraic system Kv = r to obtain
∂ql
solutions (qi1 , u1i )
+ Rjil q̇ q̇ uk + 1q̇ q̇ uk + q̇j 1q̇l uk + q̇j q̇l 1uk
k j l j l
4: for s ← 1, w do F w is sufficiently large
5: Insert (qis , usi ) into the perturbed motion equa-
∂Rkjil
+ q̇j q̇l uk 1qp + ∇i ∇l V (ul + 1ul ) tions (14) and perturbed control equations (15)
∂qp 6: Execute procedure HFE(1qis , 1usi ) to discretize
∂ 3V ∂0ilm ∂V m ∂ V
2

the perturbed motion equations (14) and perturbed con-
+ − − 0 ul 1qp
∂qp ∂ql ∂qi ∂qp ∂qm il
∂qp ∂qm trol equations (15) with null boundary values
∂M ls 7: Solve the resulting algebraic system Kv = r to
+ ∇i ∇l V us 1qp = 0, (15) obtain solutions (1qis , 1usi )
∂qp
8: qis+1 ← qis + 1qis
where squared ‘‘1’’ terms vanish.
9: us+1
i ← usi + 1usi
Therefore, our optimal control procedure consists in adapt-
10: if k1qs k ≤ then
ing the perturbation method proposed in section III-D, to the
11: Break
solution of the nonlinear ODE system (10):
12: end if
(1) approximate the solution of the linearized system (10) 13: end for F Where is chosen accordingly to the
with the same boundary values as for the nonlinear case, desired accuracy
and let the solutions be called (qi1 , u1i ); 14: end procedure
(2) approximate the solution of the ODE system composed
of the perturbed motion equations (14) and the perturbed
control equations (15) with vanishing boundary values,
and let the solutions be called (1qi1 , 1u1i ); B. OPTIMAL CONTROL OF A 1-DOF ROBOTIC
(3) improve the approximations (qi1 , u1i ) by setting qi2 = MANIPULATOR
qi1 + 1qi1 and u2i = u1i + 1u1i ; In order to illustrate and validate our optimization procedure,
(4) iterate this process until k1qi k3 < (for CHFE dis- we shall consider a simple robotic manipulator with one DOF.
cretization) or k1qi k5 < (for QHFE discretization), To resolve optimal trajectories and torques, the solution of the
where is chosen accordingly to the desired accuracy governing motion and control equations must be obtained:
(10−3 is a reasonable value).
We will refer to this general procedure as HFE/Perturbation,
(
M (q̈ + ω2 sin(q)) = u
which denotes two variants: CHFE/Perturbation for CHFE (16)
ü + ω2 u cos(q) = 0,
time discretization, or QHFE/Perturbation for QHFE time
discretization. Note that in step (3), covariant torque compo-
nents ui are being calculated instead of the contravariant com- where M = 1.384 kg m2 is the unique component of the
ponents ui . Let us emphasize that when conducting motion mass tensor; system link length and mass are such that
control simulations and experiments with HFE discretization, ω = 2.609 s−1 . The prescribed trajectory duration is set to
the ODE system (10) now depends solely on coefficients aip , T = 1.0 s. Boundary values are given by
bip , xpi and yip for the CHFE case, or aip , bip , cip , xpi , yip and zip for π
the QHFE case. Either way, recall that aip and bip are known q(0) = 0, q̇(0) = 0, q(T ) = , q̇(T ) = 0. (17)
3
for p = 0 and p = N :
According to the optimization procedure proposed in
ai0 = qi (0), bi0 = q̇i (0), aiN = qi (T ), and biN = q̇i (T ).
the previous section, the solution to the above second
Upon obtaining an approximation of qi (t), recall that approx- order nonlinear ODE system must first be approximated
imations of q̇i (t) and q̈i (t) are also automatically being by solving the following linear ODE system (step (1),
VOLUME 9, 2021 104987

FIGURE 3. Optimal trajectory provided by Mathematica’s NDSolve and the proposed HFE/Perturbation algorithm approximation, for the optimal motion
simulation of a 1-DOF robotic manipulator: (a) position and velocity; (b) torque.
lines 2 and 3 of Algorithm 2) TABLE 1. Solver performance for the solution of equations (16) for the
optimal motion simulation of a 1-DOF robotic manipulator subject to
( boundary conditions (17) where T = 1 s. Lower values of performance
M (q̈ + ω2 q)−u = 0 index (PI, equation (6)) and error norms indicate better performance.
ü + ω2 u = 0,
subject to the above mentioned boundary conditions, where

CHFE approximations of q and u are inserted. This provides
with solutions (q1 , u1 ) that are used as a starting point to
further solve the perturbation of (16). This perturbation is
composed of equations (14) and (15) (step (2)), which for
instance reduce to successfully satisfy position and velocity boundary values
that lead to the desired motion (Figure 3a).
M (1q̈1 + ω2 cos(q1 )1q1 ) − 1u1


 Recalling that the initial goal is to minimize the perfor-
+ M (q̈ + ω2 sin(q )) − u1 = 0

mance index (6), by solving the set of equations (10) for
1 1
1ü 1 + ω2 cos(q )1u1 − ω2 u1 sin(q )1q robotic manipulators, the performance of our algorithm can
 1 1 1
be assessed by evaluating the performance index value for the


+ ü + ω2 cos(q1 )u1 = 0.
 1
calculated trajectory. Additionally, let us define kf m k as the
Following steps (3) and (4) of the optimization procedure error norm of the left hand side of equation (10a):
proposed in IV-A (lines 5 to 9 of Algorithm 2) leads to
s
Z T
2
approximate the solution of the initial nonlinear problem (16). m
kf k = Mij q̈j + 0ikl q̇k q̇l + ∇i V − ui dt. (18)
Note that Algorithm 1 implies an approximation of the above 0
systems of ODE with rectangular matrix equations. The latter Analogously, let us define kf c k as the error norm of the left
is solved by using generalized matrix inversion. Note also that hand side of equation (10b):
this procedure minimizes the performance index J (uh ) (see v
uZ !2
equation (6)). u T d̂2 u
i
Algorithms 1 and 2 were implemented in Wolfram kf c k = t + Rkjil q̇j q̇l uk + ∇i ∇l V ul dt. (19)
0 dt 2
Mathematica R using N = 50 nodes for both the
CHFE/Perturbation and QHFE/Perturbation time integration These error norms evaluate the accuracy of our proposed
algorithms, and led to an acceptable solution after three method to solve the ODE system (10). In the following, our
iterations of Algorithm 2. For reference, the nonlinear ODE algorithm will be compared with Mathematica’s NDSolve
system (16) was also solved using the built-in NDSolve ODE solver on these terms.
solver, set up to operate with shooting methods. Solutions As shown by Table 1, when NDSolve is set up to operate
obtained with our algorithm and solutions obtained with with shooting methods, error norms kf m k and kf c k evaluate
NDSolve are displayed in Figures 3a and 3b. to fairly large values as compared to the other methods.
The optimal trajectory positions and velocities provided Therefore, it was alternatively set up to use a Runge-Kutta
by our HFE/Perturbation solvers and NDSolve are indis- (RK) method for time integration in order to increase its accu-
tinguishable on Figures 3a and 3b, thus indicating that our racy. Note that both our HFE/Perturbation algorithms lead to
method provides reasonable results. Note that both solutions the same performance index value as both NDSolve solutions,
104988 VOLUME 9, 2021

FIGURE 4. Performance index evolution for growing values of trajectory duration T for the optimal motion simulation robotic manipulators: (a) 1-DOF
subject to boundary conditions (17); (b) 2-DOF subject to boundary conditions (20). Comparison of four solvers. NDSolve (Shooting) is unable to
produce results for T > 9 s for the 1-DOF manipulator, and for T > 4.7 s for the 2-DOF manipulator. HFE/Perturbation is able to produce results for
longer trajectory durations than NDSolve.
thus exhibiting adequate performance. QHFE/Perturbation goes down to J = 0.158 and the RMS torque for the trajectory
does however display lower error norm values, further ver- drops to 0.085 N m, and at T = 100 s, we obtain J =
ifying the accuracy of our method. This occurs because 0.095 and an RMS torque of 0.051 N m thus confirming that
QHFE/Perturbation directly computes position and torque, both RMS torque and performance index follow a similarly
along with their first and second derivatives at each node, decreasing pattern.
which are then used to calculate norms kf m k and kf c k.
NDSolve provides with (q, u) as an output, so these have to be C. OPTIMAL CONTROL OF A 2-DOF ROBOTIC
differentiated afterwards in order to compute kf m k and kf c k. MANIPULATOR
It is important to remark that the precision of our methods can The previous results indicate that our HFE/Perturbation algo-
be augmented by increasing the total number of interpolation rithms can be successfully applied for the optimal path plan-
nodes (which was of N = 50 for this example). ning of robotic manipulators, and also illustrate how our
Additionally, it is possible to verify the stability over the method is applied. However, the previous 1-DOF example has
trajectory duration of each solver by conducting simulations a single constant mass tensor component, and such a robotic
with increasing values of T , where boundary conditions manipulator does not exhibit centrifugal and Coriolis effects
on position and velocity are kept equal. Naturally, solvers embedded in the second term of motion equation (2).
struggle more with longer simulation periods. We have We therefore now apply our method to the optimal path
therefore conducted the following test: equations (16), planning of a 2-DOF robotic manipulator with revolute
subject to boundary conditions (17), were solved using joints which exhibits most complexities and nonlinearities
the four solvers NDSolve (Shooting), NDSolve (RK), that are characteristic of realistic robotic systems such as:
CHFE/Perturbation and QHFE/Perturbation, for increasing configuration-dependent mass tensor components, centrifu-
values of T going from 1 s to 20 s. It resulted that three solvers gal and Coriolis effects, and inertia effects mutually induced
were equally stable: NDSolve (RK), CHFE/Perturbation and by each link. We conducted our simulations on a 2-DOF
QHFE/Perturbation all were able to give a solution up to robotic manipulator platform for which the corresponding
T = 20 s. However, NDSolve (Shooting) was only able to parameters are reported in [39] (see Figure 7). For this robot,
provide an adequate solution up to T = 9 s. For every value the system of 2n second order nonlinear ODE (10) translates
of T > 9 s, NDSolve (Shooting) was unable to provide a into a TPBVP involving a set of 4 ODE subject to 8 boundary
solution to (16) that meets boundary conditions (17). This is values.
shown by Figure 4a where the curve for NDSolve (Shooting) In this case, our simulation exercise was similar to the one
truncates at T = 9 s. The other three solvers returned prac- we held in the previous section. It consisted in taking the
tically the same performance index values for this particular robotic manipulator from an initial state, to a final state with
example for T > 9 s. increasing prescribed time T values. Boundary conditions
Interestingly, we could also verify that as T grows, were set to
the performance index tends to zero, which is in accordance  1
q (0), q2 (0) = (0, 0) rad


with our optimization goal, i.e., the minimization of (6) 
 q̇1 (0), q̇2 (0) = (0, 0) rad s−1

(see Figure 4a). For example, at T = 20 s, J = 0.481 and the (20)
1 (T ), q2 (T ) = (0.8, 1.0) rad

RMS torque for the trajectory evaluates to 0.258 N m. Upon 

 q
q̇1 (T ), q̇2 (T ) = (0, 0) rad s−1 .

extending duration up to T = 60 s, the performance index
VOLUME 9, 2021 104989

These simulations were conducted with Mathematica’s but k1qs k increases instead of decreasing to a value lower
NDSolve ODE solver set up to operate with shooting methods than in Algorithm 2. On the contrary, CHFE/Perturbation
first, and then set up to operate with a RK time integration is able to provide solutions up to T = 12 s implying that
algorithm to obtain alternative results. Simulations were then the CHFE/Perturbation solver is more stable with respect to
conducted with both our HFE/Perturbation methods with a trajectory duration.
fixed number of nodes set to N = 60 for each case. Recall Interestingly, since our HFE/Perturbation algorithms allow
that QHFE provide with positions, velocities and accelera- to calculate optimal trajectories with durations of T ≥ 5 s,
tions at each node and show higher precision than CHFE it is now possible to analyze the behavior of our system
(see section III-B and Table 1). These features might be desir- as T grows. Figures 5a and 5c show the optimal positions
able depending on the experimental platform requirements (q1h , q2h ) for three different values of T . Note how for T = 1 s,
and thus QHFE might be favored over CHFE. the joints reach the goal with one swing, whereas it requires
Figure 4b shows the performance index evolution for more swings as T grows. This behavior was observed for both
increasing values of T for the optimal motion simulation robot joints in our simulations. A tendency for the optimal
of the 2-DOF manipulator subject to boundary values (20). position can thus be appreciated: as T grows, the robot links
It compares the performance index values obtained with four will oscillate ever approaching the desired goal value.
solvers and is the analog of Figure 4a but for the 2-DOF Now, consider Figures 5b and 5d which shows the optimal
robot case. As opposed to the 1-DOF case, we now see that torques (u1h , u2h ) for three different values of T . Note how as
neither NDSolve (Shooting) nor NDSolve (RK) are able to T grows, torque values tend to oscillate around zero. Again,
complete the required trajectory past T = 4.7 s in Figure 4b. this behavior was observed for both joints in our simulations.
This situation occurs because the system of equations (10) This occurs because our optimization method focuses in min-
is more complicated to solve for a 2-DOF robot than it is imizing a performance index (6) for which the integrand is a
for a 1-DOF robot. Therefore, this example shows that our function of the joint torques. It is therefore natural that torques
proposed HFE/Perturbation algorithms are more stable with oscillate around the zero value.
respect to an increasing trajectory duration.
In fact, for T > 2.8 s, no performance index values are D. HFE/PERTURBATION METHOD CONVERGENCE
obtained with NDSolve (Shooting). This is because with ANALYSIS
this method, position or velocity boundary values could not The results presented in the previous section indicate that
be met (depending on the case) and therefore the desired our HFE/Perturbation algorithms are more stable with respect
trajectory could not be completed. Using the RK option for to the trajectory duration T than the NDSolve solver. How-
this solver, ensures that boundary values are met for higher ever, the convergence of both our proposed HFE/Perturbation
values of T . However, for prescribed times greater than 4.7 s, solvers shall be analyzed for our practical case. Indeed,
this is no longer the case because the solver indicates that the a superconvergence of the HFE interpolations was demon-
system is suspected to be stiff. It has been noticed that opti- strated in section III-B. However, solving equations (10) is
mal control methodologies often lead to complex numerical more complicated than a simple interpolation.
problems [40]. Stiffness is a common numerical issue that In order to determine the convergence of our algorithms to
arises in the control of nonlinear systems [24]. We therefore solve equations (10), we follow the methodology proposed
set up NDSolve to account for stiffness but the outcome did in [9], which for our case articulates in the following steps.
not change. 1) Establish the most converged solutions to equations (10)
As opposed to these methods, our HFE/Perturbation suc- subject to fixed boundary values following Algorithm 2
cessfully achieves a trajectory that meets boundary con- with a large number of nodes N . Solutions are denoted
ditions (20) even for specified times T ≥ 5.0 s with (q∗ , u∗ ) and serve as a reference against which other
acceptable accuracy. Interestingly, performance index values ‘‘less converged’’ solutions are compared with.
tend to be slightly lower with CHFE/Perturbation. However, 2) Obtain other ‘‘less converged’’ solutions to equa-
as T grows, its accuracy diminishes indicating that more tions (10) subject to the same boundary values as for
nodes may be required. Additionally, CHFE/Perturbation step 1) with a growing number of nodes N . In this step,
usually requires more iterations of Algorithm 2 than the maximum number of nodes N should be chosen such
QHFE/Perturbation, which is in accordance with Figure 2a. that both kq∗ − qh k ≤ ζ and ku∗ − uh k ≤ ζ . In our case,
Nevertheless, the precision of our HFE/Perturbation methods we have settled with ζ = 10−5 .
increases by raising the number of nodes as demonstrated in 3) For each case of N , compute the logarithm of kq∗ − qh k,
section III-B (see Figure 2a). ku∗ − uh k, kf m k and kf c k. Establish curves of these
Figure 4b also shows that no performance index values values against the corresponding value of log(h), where
are obtained with the QHFE/Perturbation for T > 10 s. h is the step size.
Interestingly this method struggles for trajectory durations 4) Fit the resulting curves with a first degree polynomial to
that approach T = 10 s for the 2-DOF robot manipula- obtain the convergence rate.
tor. Afterwards, the solutions obtained with this solver do These steps were followed for the optimal motion simu-
meet boundary values, because of the nature of the FEM, lation of both a 1-DOF robotic manipulator and a 2-DOF
104990 VOLUME 9, 2021

FIGURE 5. Optimal motion simulations of a 2-DOF robotic manipulator with QHFE/Perturbation for three different values of T . Figures show the
tendency as T increases for: (a) optimal configurations q1h ; (b) optimal controls u1h ; (c) optimal configurations q2h ; (d) optimal controls u2h .
robotic manipulator. For both robots, we performed a tra- reference. Convergence of all norms resulted to be in h4 for
jectory where T = 1 s. Boundary values (17) were taken QHFE/Perturbation.
for the case of the 1-DOF robotic manipulator, and (20) for In the case of the 2-DOF robotic manipulator, Figure 6c
the case of the 2-DOF robotic manipulator. For the 1-DOF shows the various norm values plotted against the step size
robot, we set N = 1500 to calculate (q∗ , u∗ ) (step 1) above) in the logarithmic scale for CHFE/Perturbation. This fig-
with both CHFE/Perturbation and QHFE/Perturbation. For ure shows a curve with equation 200h3 located between
the 2-DOF, we set N = 1000 to calculate (q∗ , u∗ ) with both norms kf m k and kf c k for reference. Convergence of these
HFE/Perturbation algorithms. norms resulted to be in h3 . The same figure also shows a curve
Starting with the 1-DOF robotic manipulator, Figure 6a with equation 25h3.5 located between norms kq∗ − qh k and
shows the various norm values plotted against the step size ku∗ − uh k for reference. Convergence of these norms resulted
in the logarithmic scale for CHFE/Perturbation. This fig- to be in h3.5 . Analogously, Figure 6d displays the same norms
ure shows a curve with equation 25h3 located between norms plotted against the step size also in the logarithmic scale for
kf m k and kf c k for reference. Convergence of these norms QHFE/Perturbation. This figure shows a curve with equation
resulted to be in h3 . The same figure also shows a curve 30h4 located between norms kq∗ − qh k and ku∗ − uh k for
with equation 2h3.5 located between norms kq∗ − qh k and reference. Convergence of all norms resulted to be in h4 for
ku∗ − uh k for reference. Convergence of these norms resulted QHFE/Perturbation.
to be in h3.5 . Analogously, Figure 6b displays the same Let us remark that convergence rates were the same for
norms plotted against the step size also in the logarithmic both manipulators. This is significant because it indicates that
scale for QHFE/Perturbation. This figure shows a curve with our HFE/Perturbation method convergence rates are not sys-
equation 30h4 located between the norms kf m k and kf c k for tem dependent. Instead, these rates depend on the target ODE
VOLUME 9, 2021 104991

FIGURE 6. Convergence of HFE/Perturbation for the optimal control of robotic manipulators: (a) CHFE/Perturbation for a 1-DOF robot;
(b) QHFE/Perturbation for a 1-DOF robot; (c) CHFE/Perturbation for a 2-DOF robot; (d) QHFE/Perturbation for a 2-DOF robot. Convergence rates are
equal for both manipulators.
structure. As a summary, Table 2 presents the convergence TABLE 2. Norm convergence rates with the proposed HFE/Perturbation
method upon solving the second order nonlinear set of equations (10)
rates for both CHFE/Perturbation and QHFE/Perturbation. subject to boundary values for both a 1-DOF and a 2-DOF robotic
Table 2 reports two main facts: manipulators. Scalar h denotes the step size.
1) that the superconvergence of HFE interpolations is not
inherited to the case of solving equations (10);
2) that with QHFE/Perturbation, error norms did converge
at a higher rate than with CHFE/Perturbation, but at a
lower rate than expected.
Regarding the first issue, superconvergence was estab-
lished for a simple interpolation case in section III-B.
Admittedly, this motivated us to develop the presented
HFE/Perturbation method. However, we must emphasize that
the set of ODE (10) is not as simple. These target equations cubic interpolations. In fact, error norms kq∗ − qh k and
are second order nonlinear ODE. Nevertheless, we must also ku∗ −uh k converge at a rate which is half a degree higher than
emphasize that for CHFE/Perturbation, the convergence rate the order of the cubic interpolations. Regarding the second
of the evaluated norms is at least in the order of the chosen issue, surprisingly, the evaluated error norms revealed to
104992 VOLUME 9, 2021

converge at a rate which is one degree lower than the order of

the chosen quintic interpolations with the QHFE/Perturbation
method.
Recall that QHFE require one more basis function
than CHFE (see section III-A). This means that to
solve equations (10), QHFE/Perturbation computes 2nN
more unknowns than CHFE/Perturbation according to
Algorithm 1. This is beneficial when second time deriva-
tives of (q, u) are required at each node. However, CHFE/
Perturbation reveals to be more cost effective (computa-
tionally speaking) whenever first time derivatives of (q, u),
at each node, suffice.
These results illustrate the performance and accuracy of
our approach for the optimal path planning of robotic manip-
ulators, and encourage us to perform motion control experi-
ments to validate our proposal.
V. EXPERIMENTAL VALIDATION
In order to validate our approach, we conducted controlled
motion experiments on a 2-DOF robotic manipulator plat-
form presented in [39], [41] and located at the Instituto FIGURE 7. Robotic manipulator platform used for the experimental
validation. It is a 2-DOF manipulator actuated by direct-drive brushless
Tecnológico de La Laguna, Mexico, previously used in [25]. servo motors operated in torque mode.
The robotic manipulator is actuated by direct-drive brush-
less servo motors operated in torque mode. These act as
torque sources, receiving analog voltages as torque reference previously described. In the absence of external disturbances
signals. This property is ideal because it is thus possi- and parametric uncertainties, and if the initial conditions at
ble to directly feed the optimal torques obtained with our time t = 0 are precisely the ones that are used as boundary
HFE/Perturbation method as an input reference signal. Opti- conditions, the output of the control law is precisely the feed-
mal positions and velocities are compared to the positions forward term, i.e., the optimal torque.
and velocities fed back from the robot. A 32-bit DSP micro- In practice, it may be that the initial conditions for the
processor receives joint positions obtained via incremental system deviate by a small amount due to measuring error.
encoders. The control algorithm is executed in a sampling The PD part of the controller corrects this deviation. A devi-
period of 2.5 milliseconds on a host computer running the ation from initial conditions that is big in magnitude may
WinMechLab environment presented in [42]. be corrected by selecting appropriate values of the initial
conditions (20). The PD term also adds robustness in the case
A. CONTROL SCHEME where vanishing disturbances arise during operation or when
The control law used to implement the optimal trajectories manipulator parameters are not properly identified.
may be divided into two operation modes, an optimal reach- After the prescribed time T , the holding phase is active.
ing phase, which is active if 0 ≤ t ≤ T , and a holding phase During this phase, the control law becomes a PD plus grav-
that is active if t > T , with T given as in (20). ity compensation controller. The equation that describes the
The control scheme used to implement the optimal reach- control law can be written as
ing phase follows the structure of a Proportional-Derivative
(PD) control plus a feed-forward term. Its purpose is to τ = Kp q̃ − Kv q̇ + g(q),
make the system follow a prescribed optimal trajectory which where g(q) is the gravitational torque vector of the robot, and
attains a specific value on a fixed prescribed time. The control q̃ = qh − q, with qh a constant vector that contains the
law during this phase is described by the equation values of the boundary conditions at t = T . On this phase
τ = uh + Kp q̃ + Kv q̃,
˙ the PD term serves the same purpose as in the previous phase
and gravity compensation is used to keep the manipulator in
where τ ∈ Rn is comprised of the torques that are applied on
place.
each link of the n-DOF robot, and q̃ = qh − q represents the
In summary, the overall control law may be written as
position error with qh as the desired optimal position of the (
links. The matrices Kp and Kv are diagonal positive definite uh + Kp q̃ + Kv q̃˙ if 0 ≤ t ≤ T
matrices of dimension n × n. A schematic representation of τ=
Kp q̃ − Kv q̇ + g(q) if t > T
this section of the control law may be seen in Figure 8.
The feed-forward term is denoted as uh and its value is Friction compensation was also used during each phase to
the optimal torque obtained from the optimization method cancel the effect of kinetic friction. On the prototype, kinetic
VOLUME 9, 2021 104993

FIGURE 8. Schematic representation of the controller.
friction was modeled as Coulomb friction (as described where M is the inertia matrix of the manipulator, C is a
in [43]) and the compensation that was used is as follows. Let matrix that captures the effect of centrifugal and Coriolis
τexp denote the torque that is actually applied to the manipula- forces and g is a vector that contains the torques on the system
tor during the experiment. The applied control torque, in the that are due to the effects of gravity. Further details on this
experiment, may be expressed as representation of the dynamical model may be found in [44].
Throughout this section, the external disturbance is
τexp = τ + FC sign(q̇)
assumed to be additive, that is, the motion equations of the
where τ denotes the torque produced by either the opti- mechanical system may be expressed as
mal or reaching phase control laws, and FC is a diagonal
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ (t) + d(t)
matrix containing the coulomb friction coefficients. Since
friction coefficients were previously identified, this strategy where τ is the torque applied to each link (which is the sum
effectively cancels out the effects of the most predominant of a PD term and uh ), and d is the external disturbance on
nonconservative forces affecting our prototype. each link. It is also assumed that the norm of the external
We used the same control scheme as in [25], where its disturbance can be bounded by a positive constant δd such
stability was formally analyzed. that kd(t)k ≤ δd for all t ≥ 0.
With this in mind, it is possible to state that the closed loop
B. THE EFFECT OF EXTERNAL DISTURBANCES system takes the following form during the optimal reaching
We now analyze the effect of external disturbances on the phase:
closed loop system. To quantify the effect of external distur- " ˙
#
bances, it is possible to consider each phase independently d q̃ q̃ i , (21)
= h
(optimal reaching and holding), as it is done to show the dt q̃˙ M (q)−1 −Kp q̃ − Kv q̃˙ + z1 (t)
stability of the closed loop in [25]. The overall disturbance
effect depends on the controller gains and the disturbance’s where
upper bound. z1 (t) = −C(q, q̇)q̃˙ − h(t, q̃, q̃)
˙ − d(t),
As it was shown in [25], it is possible to express the solution
uh of equation (10) as a function of qh in the following way: and h denotes the residual dynamics given by
uh (t) = M (qh )q̈h + C(qh , q˙h )q̇h + g(qh ) ˙ = [M (qh ) − M (qh − q̃)]q̈h
h(t, q̃, q̃)
104994 VOLUME 9, 2021

+ [C(qh , q̇h ) − C(qh − q̃, q̇h − q̃˙ h )]q̇h Notice that V̇ ≤ 0 if µ1 ≤ kxk ≤ r, where µ1 is given by
+ g(qh ) − g(qh − q̃). δd max{γ , 1}
µ1 = .
Consider now the function γ λMax {Q1D }
1 ˙T To ensure that function (22) is positive definite and its

˙ =
V (t, q̃, q̃) q̃ M (q)q̃˙ time derivative is non-positive on a subset of the domain Dr ,
2
1 the same tuning conditions that were presented in [25] should
+ q̃T Kp q̃ + γ tanh(q̃)T M (q)q̃,
˙ (22) be met along with an additional one given by
2
˙ ≤ α2 kxk2
which satisfies the bounds α1 kxk2 ≤ V (t, q̃, q̃) [2γ a + kh2 ]2
Kpm > + kh2
4ε 2 γ Kvm − kh1 − γ b

with x = [q̃T q̃˙ T ]T and constants α1 , α2 > 0, whose exis-
tence is ensured by the tuning conditions presented in [25], where kh1 and kh2 are defined in [44], and constants a and b
given by are given by
α1 = λmin {P1 }, 1
KvM + kC1 kq̇h kM + kh1 ,

a=
α2 = λMax {P2 }, 2 √
b = KMM + nkC1 .
where λmin {P1 } and λMax {P2 } denote the smallest and largest
eigenvalues, respectively, of matrices P1 and P2 that are Under these given conditions and invoking Theorem 4.18
defined as from [45], it is possible to state that that there exists a time
TC such that every solution with initial conditions
Kpm −γ KMM
P1 = r
α1
−γ KMM KMm
kx0 k ≤ r0
α2
and
where r0 < r, satisfies
KpM γ KMM

P2 = . α2
γ KMM KMM
r
kx(t)k ≤ µ1 , ∀t ≥ t + TC . (25)
α1
Constants Kpm , Kvm , KMm denote lower bounds on the
eigenvalues of matrices Kp , Kv and M (q), respectively. Con- In other words, during the optimal reaching phase, if an
stants KpM , KvM , KMM denote upper bounds of the eigenval- external disturbance is present, solutions enter a set that
ues. can be made arbitrarily small with appropriate tuning as a
The time derivative of (22) along the trajectories of (21) function of the upper bound on the disturbance.
satisfies If the switching time T is greater than TC , that is T > TC ,
T then solutions will converge and remain inside the set
ktanh(q̃)k ktanh(q̃)k described by (25) until the switching time. After switching
V̇ ≤ −γ ˙ Q1 ˙
kq̃k kq̃k occurs, another transient state might ensue depending on the
+ δd kq̃˙ + γ tanh(q̃)k (23) used gains during the holding phase. Therefore, to completely
describe the effects of the external disturbance, the behavior
with during the holding phase must first be analyzed. To this end,
 1 kh2  notice that during the holding phase, the closed loop system
Kpm − kh2
−a − may be expressed as
γ 2
Q1 =  .
 
1 kh2 1 " ˙
#
−a − Kvm − kh1 − b d q̃ q̃
γ 2 γ = h i , (26)
dt q̃˙ M (q)−1 −Kp q̃ − Kv q̃˙ + z2 (t)
Now, consider a domain Dr = {x ∈ R2n : kxk ≤ r}, where
˙ T . On Dr , by defining ε = tanh(r) , a positive
x = [kq̃k kq̃k] where
r
constant that depends on the size of Dr , it is possible to state z2 (t) = −C(q, q̇)q̃˙ − d(t).
that kq̃k ≥ ktanh(q̃)k ≥ εkq̃k on Dr . Therefore, on Dr it is
true that Notice also that after the switch, solutions at time t = T
take the value of the initial conditions for the system evolu-
V̇ ≤ −γ λMax {Q1D }kxk2 + δd max{γ , 1}kxk (24) tion (26).
By using the same function (22), assertions can be made
where
regarding the stability of the solutions to (26). It can be
 1 kh2 
shown that the time derivative of V along the solutions of (26)
ε2 (Kpm − kh2 ) −a −
γ 2 satisfies
Q1D =  .
 
1 kh2 1
−a − Kvm − kh1 − b
γ 2 γ V̇ ≤ −γ λMax {Q2D }kxk2 + δd max{γ , 1}kxk (27)
VOLUME 9, 2021 104995

on Dr with Q2D given by TABLE 3. Performance index values for the controlled motion
experiments held on a 2-DOF robotic manipulator subject to boundary
1
  conditions.
ε2 Kpm − KvM
Q2D =  1
 2 .

1 √
− KvM Kvm − nkC1 − KMM
2 γ
This ensures that function (22) has a non-positive time
derivative on a subset of the domain Dr given by µ2 ≤ kxk ≤
r, where µ2 is defined as
δd max{γ , 1}
µ2 = ,
γ λMax {Q2D }
under the same tuning conditions that were presented in [25]
with an additional one given by
Figure 9 compares the robot trajectory positions and
4ε2 Kpm Kvm torques with the input signals during the three exper-
√ > γ.
Kv2M + 4ε2 Kpm nkC1 + KMM iments. Figures 9a, 9b and 9c correspond to experi-
Just as for the optimal reaching phase, under these given ment 1. Figures 9d, 9e and 9f correspond to experiment 2.
conditions and invoking Theorem 4.18 of [45], it is possible Figures 9g, 9h and 9i correspond to experiment 3. Note
to state that there exists a time TF such that every solution that for the three experiments, the trajectory position
with initial conditions was smoothly tracked. Negligible positioning errors were
recorded along each trajectory. Final goal positions were
α1
r
kx0 k ≤ r0 attained with relative errors in the order of 10−3 rad at each
α2 joint for the three experiments. Note also that the experiments
where r0 < r, satisfies were extended past the prescribed trajectory duration T ; after-
wards, torque values remain constant so that the robot remains
α2
r
kx(t)k ≤ µ2 , ∀t ≥ t + TF . (28) in its goal position.
α1 As per equation (21), our control strategy compensates
This means that if an external disturbance is present, solu- friction effects online during the experiments by taking into
tions during the holding phase also enter a set that can be account the robot parameters. The applied control law in
made arbitrarily small with appropriate tuning. the experiment took these effects into account in order to
Controller gains during the holding phase can be differ- cancel them out. The reader is referred to [25] for more
ent from the ones used during the optimal reaching phase. details on this procedure. In order to analyze the impact
Consequently, values for α1 , α2 may be different for the of this compensation on the input signal, we have recorded
holding phase. This also implies that if, for the optimal the robot torques both with and without friction compensa-
reaching phase, TC < T and the region of convergence is tion. Let us remark that torques without friction compensa-
smaller for the holding phase than for the optimal reaching tion do not produce the required motion, these are just the
phase, another small transient might be present. On the other robot torques with friction compensation to which we sub-
hand, if TC > T , then the solutions can still be expected to tracted friction terms in (21) for analysis purposes. Therefore,
fulfill (28). Figures 9b, 9e and 9h show robot torques with friction com-
In summary, if a bounded external disturbance is present pensation for experiments 1, 2 and 3 respectively. Addition-
it is possible to establish that there will be a deviation from ally, Figures 9c, 9f and 9i show robot torques without friction
the optimal trajectory that may be corrected up to a certain compensation for experiments 1, 2 and 3 respectively.
degree depending on the controller gain values. When comparing robot torques with friction compensa-
tion against torques without friction compensation for each
C. EXPERIMENTAL RESULTS trajectory, we can see that this compensation does increase
We have conducted three experiments with different the required torques in order to produce the desired motion.
prescribed trajectory durations. We note that torques with friction compensation do follow the
• Experiment 1: duration T = 1 s and boundary val- tendency of the optimal input signal, but noticeably modify
ues (20) were taken. its value. Online friction compensation naturally affects the
• Experiment 2: duration T = 3 s; initial positions and performance index values for the experiment (see Table 3),
initial and final velocities of (20) were taken but q(T ) = which are larger for the actual robot motion (column 3) than
(1.2, 1.1)T rad instead. for the simulation case (input signal, last column of Table 3).
• Experiment 3: duration T = 5 s and boundary val- However, maximum output torque values u remain very low
ues (20) were taken. compared to the maximum output that our robot can generate
Note that the only difference between experiments 1 and 3 is in all three experiments. Let us now present the specifics of
the trajectory duration. each experiment.
104996 VOLUME 9, 2021

FIGURE 9. Controlled motion experiments on a 2-DOF manipulator platform. Experiment 1: T = 1 s; final positions q(T ) = (0.8, 1)T rad; (a) trajectory
position; (b) torque with friction compensation; (c) torque without friction compensation. Experiment 2: T = 3 s; final positions q(T ) = (1.1, 1.2)T rad;
(d) trajectory position; (e) torque with friction compensation; (f) torque without friction compensation. Experiment 3: T = 5 s; final positions
q(T ) = (0.8, 1)T rad; (g) trajectory position; (h) torque with friction compensation; (i) torque without friction compensation. Trajectory positions were
smoothly tracked for each experiment.
1) EXPERIMENT 1 the experiment. Negligible positioning errors were recorded

Optimal input signals (qh , uh ) were calculated with the four along the trajectory. Figure 9b compares the optimal torque
solvers used in section IV (NDSolve (shoot), NDSolve (RK), uh with the system output u. Table 3 reports the performance
CHFE/Perturbation and QHFE/Perturbation) according to the index (PI) values obtained for this experiment in the first row.
strategy described therein. For this particular example, differ- The increase in the performance index for the experiment
ences between solvers were negligible and thus we retained (column 3) compared against the simulation case (column 5)
the solution obtained with our QHFE/Perturbation method. is noticeable (almost five times as large) but results from the
This provided with optimal torques uh , used as input signals, online friction compensation as previously mentioned.
and optimal positions qh and velocities q̇h that are compared
with the feedback signals. 2) EXPERIMENT 2
Figure 9a displays the optimal position qh , used as refer- For this experiment, we also calculated optimal input sig-
ence for feedback, compared with the output system position nals (qh , uh ) with the four solvers used in IV (NDSolve
q during the experiment. As previously mentioned, the tra- (Shooting), NDSolve (RK), CHFE/Perturbation and QHFE/
jectory was smoothly tracked, following the reference during Perturbation). However, as shown by Figure 4b, NDSolve
VOLUME 9, 2021 104997

(Shooting) could not calculate the required motion because Table 3 shows that when suppressing friction compensation
final conditions on position and velocity could not from the output signal, the torque discrepancy with respect
be met. The remaining three solvers (NDSolve (RK), with the input signal does diminish. It does however stay large
CHFE/Perturbation and QHFE/Perturbation) successfully specially for experiment 3 (third row of Table 3).
calculated the required motion, satisfying all bound- We had previously established that the performance index
ary conditions with little differences as shown also by should decrease as T increases (see Figures 4a and 4b
Figure 4b. Here, we retained the solution obtained with our in section IV) when the boundary conditions on position
CHFE/Perturbation method. Note that we could have retained and velocity stay equal. Surprisingly, this feature was not
either of the other two solution methods for this experiment recovered in the experimental case, as shown by Table 3.
without affecting the outcome of the experiment because the Experiments 1 and 3 had the same final positions but the
obtained solutions are so similar. performance index with friction compensation is larger for
Figure 9d displays the optimal position qh , used as refer- experiment 3 where T is five times longer.
ence for feedback, compared with the output system position Therefore, we hypothesize that these problems could be
q during the experiment. Again, the trajectory was smoothly solved by incorporating nonconservative forces (such as fric-
tracked with negligible positioning errors along the trajec- tion effects) into our model (10) as prescribed in section 2.3
tory. Figure 9e compares the optimal torque uh with the of [25]. By doing this, an online friction compensation could
system output u. Table 3 reports the performance index values be avoided, thus reducing the absolute value of the required
obtained for this experiment in the second row. The increase robot torques to produce optimal motion. This could result
in the performance index for the experiment (column 3) in preserving optimal input torques up to a certain degree
compared against the simulation case (column 5) is also and recovering a decreasing performance index whenever T
noticeable (this time being twenty times as large) and also increases.
results from the online friction compensation as previously Another source of uncertainty that may cause the exper-
mentioned. imental performance of the controller to be different from
the theoretical one may be the discretization error induced
3) EXPERIMENT 3 by the experimental setup. Measurements from the sensors
For this final experiment, we calculated optimal input signals embedded in the manipulator are taken every 2.5 milliseconds
(qh , uh ) with the three solvers that succeeded experiment 2: and the control law is computed with the same frequency.
NDSolve (RK), CHFE/Perturbation and QHFE/Perturbation. Although the overall effect of this phenomenon on the per-
However, this time, NDSolve (RK) could not calculate the formance of the control scheme should not be as important
required motion because boundary conditions could not be as friction, it may also have an impact on the discrepancies
met, as shown by Figure 4b. Both our CHFE/Perturbation found between theoretical results and the experiments.
and QHFE/Perturbation methods successfully calculated the We should emphasize however that even in the experi-
required motion, exactly satisfying all boundary condi- mental case, all of the trajectories were tracked with good
tions. Here, we retained the solution obtained with our accuracy. Also, while the trajectory for experiment 1 could be
QHFE/Perturbation method. Note that we could have instead calculated with all of the solvers, experiment 2 could not be
retained the CHFE/Perturbation method solution for this held with the NDSolve (Shooting) solver, and experiment 3
experiment without affecting its outcome. could not be held with either NDSolve (Shooting) or its RK
Figure 9g displays the optimal position qh , used as refer- variant. On the contrary, our proposed CHFE/Perturbation
ence for feedback, compared with the output system position and QHFE/Perturbation solvers were able to complete all of
q during the experiment. Again, the trajectory was smoothly the experiments required trajectories.
tracked with negligible positioning errors along the trajec- These results show that the optimal trajectory obtained
tory. Figure 9h compares the optimal torque uh with the with our proposed HFE/Perturbation method can be tracked
system output u. Table 3 reports the performance index with good accuracy and thus validate our approach. We now
values obtained for this experiment in the third row. The proceed to conclude our paper by recalling the main advan-
increase in the performance index for the experiment (col- tages of our proposed method, as well as summarizing our
umn 3) compared against the simulation case (column 5) is findings in the next section.
large (this time being one hundred times as large) and also
results from the online friction compensation as previously VI. CONCLUDING REMARKS
mentioned. Upon establishing a coupled set of second order, nonlin-
ear, covariant ODE controlling torques and trajectories that
4) EXPERIMENTS DISCUSSION involve the covariant derivatives of the gravitational potential
All of the experiments were extended past the prescribed and the Riemann-Christoffel curvature tensor, an algorithm
trajectory durations T ; afterwards, torque values remain con- has been elaborated for finding the optimal torques and tra-
stant so that the robot remains in its goal position. As men- jectories. This algorithm consists in solving the system of
tioned before, the discrepancy between the robot torques ODE that minimizes an objective functional of the torques,
required for motion and the optimal input signal is noticeable. regarded as a performance index.
104998 VOLUME 9, 2021

Our procedure approximates the configuration parame- However, the latter still displays higher convergence rates
ters and the torques by piecewise HFE interpolations in and may be desirable whenever first and second time
the framework of the FEM. CHFE and QHFE are pro- derivatives of positions and torques are required at each
posed as a more precise alternative. Physical interpretation node.
of finite elements coefficients are given: approximations of Finally, experiments were conducted with a robotic manip-
position, velocity and acceleration are directly obtained at ulator by directly feeding optimal torques obtained with
each node. Two examples illustrate the superconvergence of our method, as inputs. This resulted in the generation of
these interpolations when simulating the torques and trajec- smooth trajectories that met specified boundary conditions,
tories. A perturbation method involving Hermite finite ele- thus validating our approach. Our algorithm is therefore well
ments for time integration was proposed. Therefore, explicit suited for efficiently generating optimal motion for robotic
expressions for perturbed motion and control equations were systems. However, and unlike what we had established for
given. optimal motion simulations, the performance index did not
The presented HFE/Perturbation method was specifically decrease when the trajectory duration increased. On the con-
designed and applied to solve the OCP of a robotic manip- trary, online friction compensation and parametric uncer-
ulator. However, it can be easily adapted to solve general tainties noticeably affected the performance index so that
nonlinear second order ODE arising in multibody systems it increased with trajectory duration instead. Therefore, our
dynamics. Solutions approximations obtained with our algo- optimal control method could be improved by taking non-
rithm were compared with those obtained with a commercial conservative forces into account in our model. This should
ODE solver, on the basis of specific performance indicators, result in preserving optimal input torques up to a certain
and reveal to provide better performance overall, provided extent, directly leading to lower experimental torques values
that sufficient nodes are being used. Additionally, accuracy of for motion. This will be the object of further research for
the obtained solutions approximations can be easily increased which the presented HFE/Perturbation method shall be used
as required. Note also that stiffness does not affect our method to solve the arising nonlinear ODE.
for fairly large trajectory durations, thus enabling further The control scheme was shown to have robustness prop-
analysis of the optimal control procedure. Therefore, when erties with respect to external disturbances. Namely, it was
compared with the commercial ODE solver, our FE-based shown, using Lyapunov based analysis, that if a bounded
method has the following advantages: external disturbance is present it is possible to establish that
• it directly provides with optimal positions and torques, there will be a bounded deviation from the optimal trajectory.
along with their respective first (with CHFE) and sec- This also implies that in spite of the external disturbance,
ond (with QHFE only) time derivatives at each the closed-loop solutions remain bounded. In practice, this
node; means that external disturbances would not render the system
• due to the nature of the FE interpolation, there is no need unstable.
to shoot the final conditions nor to rebuild the control In summary, we have presented a time FE-based method
variables; that effectively solves the optimal control problem of robotic
• lower error norms can be obtained because further dif- manipulators involving covariant control equations as opti-
ferentiation of positions and torques is not required; mality conditions; the proposed control scheme provides the
• longer duration trajectories can be calculated. optimized control action required to take the robot from an
With respect to this last point, i.e., in terms of stability with initial to a final state in a prescribed time. This FE-based
respect to trajectory duration, CHFE/Perturbation resulted to method was compared with a commercial ODE solver and
be the most stable. Our solver was able to provide solutions revealed to be more stable for extended prescribed trajectory
for durations of up to 12 s for the optimal motion of a 2-DOF times. Experimental results on a robotic manipulator system
robotic manipulator, where the commercial ODE solver could validated our theoretical proposal. Motion trajectories were
not go further than 4.7 s. smoothly tracked and results exposed further methodology
A convergence analysis of our proposed HFE/Perturbation improvements.
algorithms revealed that the superconvergence of HFE inter-
polations is not inherited when applied to our optimal control REFERENCES
methodology. However, convergence of the evaluated norms
[1] K. M. Liew, X. Q. He, T. Y. Ng, and S. Sivashanker, ‘‘Active control
with CHFE/Perturbation resulted to be at least in the order of FGM plates subjected to a temperature gradient: Modelling via finite
of the cubic interpolations and even higher for errors of q element method based on FSDT,’’ Int. J. Numer. Methods Eng., vol. 52,
(position) and u (torques). Interestingly, convergence of the no. 11, pp. 1253–1271, Dec. 2001, doi: 10.1002/nme.252.
[2] P. Mäkinen and J. Mattila, ‘‘Finite element-based control of a single-link
evaluated norms with QHFE/Perturbation were lower than flexible hydraulic manipulator,’’ in Proc. ASME/BATH 2017 Symp. Fluid
expected, at a rate of one degree lower than the order of the Power Motion Control (Fluid Power Systems Technology), Oct. 2017,
quintic interpolations. pp. 1–10, doi: 10.1115/FPMC2017-4264.
Generally speaking, CHFE/Perturbation is more stable [3] T. M. Bieze, F. Largilliere, A. Kruszewski, Z. Zhang, R. Merzouki,
and C. Duriez, ‘‘Finite element method-based kinematics and closed-
with respect to trajectory duration, and more cost effec- loop control of soft, continuum manipulators,’’ Soft Robot., vol. 5, no. 3,
tive (in the numerical sense) than QHFE/Perturbation. pp. 348–364, 2018, doi: 10.1089/soro.2017.0079.
VOLUME 9, 2021 104999

[4] P. Ning, Z.-Q. Feng, J. A. R. Quintero, Y.-J. Zhou, and L. Peng, ‘‘Uzawa [25] J. A. Rojas-Quintero, J. A. Rojas-Estrada, J. Villalobos-Chin,
algorithm to solve elastic and elastic–plastic fretting wear problems within V. Santibañez, and E. Bugarin, ‘‘Optimal controller applied to robotic
the bipotential framework,’’ Comput. Mech., vol. 62, no. 6, pp. 1327–1341, systems using covariant control equations,’’ Int. J. Control, pp. 1–14,
Dec. 2018, doi: 10.1007/s00466-018-1567-8. Jan. 2021, doi: 10.1080/00207179.2020.1865570.
[5] Y.-J. Zhou, Z.-Q. Feng, J. A. R. Quintero, and P. Ning, ‘‘A computational [26] F. Dubois, D. Fortuné, J. A. R. Quintero, and C. Vallée, ‘‘Pontryagin
strategy for the modeling of elasto-plastic materials under impact load- calculus in Riemannian geometry,’’ in Geometric Science of Information,
ings,’’ Finite Elements Anal. Design, vol. 142, pp. 42–50, Mar. 2018, doi: F. Nielsen and F. Barbaresco, Eds. Cham, Switzerland: Springer, 2015,
10.1016/j.finel.2018.01.003. pp. 541–549, doi: 10.1007/978-3-319-25040-3_58.
[6] F. Williamson, ‘‘Richard courant and the finite element method: A further [27] Z. Li, C. Li, S. Li, and X. Cao, ‘‘A fault-tolerant method for
look,’’ Historia Math., vol. 7, no. 4, pp. 369–378, 1980, doi: 10.1016/0315- motion planning of industrial redundant manipulator,’’ IEEE Trans.
0860(80)90001-4. Ind. Informat., vol. 16, no. 12, pp. 7469–7478, Dec. 2020, doi:
[7] G. A. Evans, J. M. Blackledge, and P. D. Yardley, Numerical Methods 10.1109/TII.2019.2957186.
for Partial Differential Equations. London, U.K.: Springer, 2000, doi: [28] Z. Li, W. Zuo, and S. Li, ‘‘Zeroing dynamics method for motion control of
10.1007/978-1-4471-0377-6. industrial upper-limb exoskeleton system with minimal potential energy
[8] O. C. Zienkiewicz, R. L. Taylor, and J. Z. Zhu, The Finite Element Method: modulation,’’ Measurement, vol. 163, Oct. 2020, Art. no. 107964, doi:
Its Basis Fundamentals, 7th ed. London, U.K.: Butterworth-Heinemann, 10.1016/j.measurement.2020.107964.
2013, doi: 10.1016/B978-1-85617-633-0.00001-0. [29] P. Grinfeld, Introduction to Tensor Analysis and the Calculus of Moving
[9] G. Allaire and A. Craig, Numerical Analysis and Optimization: An Intro- Surfaces. New York, NY, USA: Springer, 2013, doi: 10.1007/978-1-4614-
duction to Mathematical Modelling and Numerical Simulation (Numerical 7867-6.
Mathematics and Scientific Computation). Oxford, U.K.: Oxford Univer- [30] A. Nikoobin and M. Moradi, ‘‘Optimal balancing of robot manipulators in
sity Press, 2007. point-to-point motion,’’ Robotica, vol. 29, no. 2, pp. 233–244, Mar. 2011.
[10] G. Marchouk and V. Agochkov, Introduction aux Méthodes des éléments [31] C. Mirz, F. Schöler, J. P. Barreto, and B. Corves, ‘‘Optimal control
Finis (French). Moscow, Russia: Mir, 1985. based path planning for parallel kinematic manipulators utilising natu-
[11] A. Eriksson, ‘‘Temporal finite elements for target control dynamics ral motion,’’ in Proc. IEEE 14th Int. Conf. Automat. Sci. Eng. (CASE),
of mechanisms,’’ Comput. Struct., vol. 85, nos. 17–18, pp. 1399–1408, Aug. 2018, pp. 223–228.
Sep. 2007, doi: 10.1016/j.compstruc.2006.08.080. [32] M. R. Vezvari, A. Nikoobin, and A. Ghoddosian, ‘‘Zero-power balancing a
[12] M. Dorosti, R. H. B. Fey, M. F. Heertjes, M. M. J. van de Wal, and two-link robot manipulator for a predefined point-to-point task,’’ J. Mech.
H. Nijmeijer, ‘‘Iterative pole-zero finite element model updating using Sci. Technol., vol. 34, no. 6, pp. 2585–2595, Jun. 2020.
generic parameters,’’ Mechatronics, vol. 55, pp. 180–193, Nov. 2018, doi: [33] M. Asgari and A. Nikoobin, ‘‘Analysis of optimal dynamic manipulation
10.1016/j.mechatronics.2018.06.012. for robotic manipulator based on Pontryagin’s minimum principle,’’ Ara-
[13] L. Paunonen and D. Phan, ‘‘Reduced order controller design for bian J. Sci. Eng., vol. 45, no. 11, p. 9159—9169, 2020.
robust output regulation,’’ IEEE Trans. Autom. Control, vol. 65, no. 6, [34] M. Gautier and W. Khalil, ‘‘Exciting trajectories for the identification of
pp. 2480–2493, Jun. 2020, doi: 10.1109/TAC.2019.2930185. base inertial parameters of robots,’’ Int. J. Robot. Res., vol. 11, no. 4,
[14] D. Wang and X. Chen, ‘‘Closed-loop simulation integrating finite element pp. 362–375, Aug. 1992, doi: 10.1177/027836499201100408.
modeling with feedback controls in powder bed fusion additive manu- [35] M. H. Holmes, Introduction to Perturbation Methods (Texts in Applied
facturing,’’ in Proc. Int. Symp. Flexible Autom., Jul. 2020, pp. 1–7, doi: Mathematics), vol. 20, 2nd ed. New York, NY, USA: Springer-Verlag,
10.1115/isfa2020-9611. 2013, doi: 10.1007/978-1-4614-5477-9.
[15] G. Zheng, O. Goury, M. Thieffry, A. Kruszewski, and C. Duriez, [36] A. Beléndez, E. Arribas, M. Ortuño, S. Gallego, A. Márquez, and
‘‘Controllability pre-verification of silicone soft robots based on finite- I. Pascual, ‘‘Approximate solutions for the nonlinear pendulum
element method,’’ in Proc. Int. Conf. Robot. Autom. (ICRA), May 2019, equation using a rational harmonic representation,’’ Comput.
pp. 7395–7400, doi: 10.1109/ICRA.2019.8794370. Math. with Appl., vol. 64, no. 6, pp. 1602–1611, Sep. 2012, doi:
[16] R. Cavin and S. Tandon, ‘‘Distributed parameter system optimum con- 10.1016/j.camwa.2012.01.007.
trol design via finite element discretization,’’ Automatica, vol. 13, no. 6, [37] E. Salinas-Hernández, G. A. de Parga, S. Domínguez-Hernández, and
pp. 611–614, 1977, doi: 10.1016/0005-1098(77)90082-6. R. Muñoz-Vega, ‘‘An analytical approximation of a pendulum trajectory,’’
[17] J. Liu and Z. Zhou, ‘‘Finite element approximation of time fractional Eur. J. Phys., vol. 35, no. 4, Jul. 2014, Art. no. 045027, doi: 10.1088/0143-
optimal control problem with integral state constraint,’’ AIMS Math., vol. 6, 0807/35/4/045027.
no. 1, pp. 979–997, 2021, doi: 10.3934/math.2021059. [38] A. Beléndez, C. Pascual, D. I. Méndez, T. Beléndez, and C. Neipp,
[18] M. Diehl, H. G. Bock, H. Diedam, and P.-B. Wieber, ‘‘Fast direct multiple ‘‘Exact solution for the nonlinear pendulum,’’ Revista Brasileira de
shooting algorithms for optimal robot control,’’ Fast Motions in Biome- Ensino de Física, vol. 29, no. 4, pp. 645–648, 2007, doi: 10.1590/S1806-
chanics and Robotics (Lecture Notes in Control and Information Sciences), 11172007000400024.
vol. 340. Berlin, Germany: Springer, 2006, doi: 10.1007/978-3-540-36119- [39] F. Reyes and R. Kelly, ‘‘Experimental evaluation of identification schemes
0_4. on a direct drive robot,’’ Robotica, vol. 15, no. 5, pp. 563–571, Sep. 1997,
[19] A. Eriksson and A. Nordmark, ‘‘Temporal finite element formula- doi: 10.1017/S0263574797000659.
tion of optimal control in mechanisms,’’ Comput. Methods Appl. [40] Y. Zhang, S. Li, and X. Zhou, A Survey of Near-Optimal Control of
Mech. Eng., vol. 199, nos. 25–28, pp. 1783–1792, May 2010, doi: Nonlinear Systems. Cham, Switzerland: Springer, 2020, pp. 1–20, doi:
10.1016/j.cma.2010.02.003. 10.1007/978-3-030-33384-3_1.
[20] D. Liberzon, Calculus of Variations and Optimal Control Theory: [41] F. R. Cortés and R. Kelly, ‘‘Experimental evaluation of model-based con-
A Concise Introduction. Princeton, NJ, USA: Princeton Univ. Press, trollers on a direct-drive robot arm,’’ Mechatronics, vol. 11, pp. 267–282,
2011. Apr. 2001, doi: 10.1016/S0957-4158(00)00008-8.
[21] G. R. G. da Silva, A. S. Bazanella, C. Lorenzini, and L. Campestrini, [42] R. Campa, R. Kelly, and V. Santibáñez, ‘‘Windows-based real-time
‘‘Data-driven LQR control design,’’ IEEE Control Syst. Lett., vol. 3, no. 1, control of direct-drive mechanisms: Platform description and experi-
pp. 180–185, Jan. 2019, doi: 10.1109/LCSYS.2018.2868183. ments,’’ Mechatronics, vol. 14, no. 9, pp. 1021–1036, Nov. 2004, doi:
[22] T. Çimen, ‘‘State-dependent Riccati equation (SDRE) control: A sur- 10.1016/j.mechatronics.2004.04.004.
vey,’’ IFAC Proc. Volumes, vol. 41, no. 2, pp. 3761–3775, 2008, doi: [43] S. Sánchez-Mazuca and R. Campa, ‘‘An improvement proposal to the
10.3182/20080706-5-KR-1001.00635. static friction model,’’ Math. Problems Eng., vol. 2013, Jun. 2013,
[23] S. R. Nekoo, ‘‘Tutorial and review on the state-dependent Riccati equa- Art. no. 946526, doi: 10.1155/2013/946526.
tion,’’ J. Appl. Nonlinear Dyn., vol. 8, no. 2, pp. 109–166, Jun. 2019, doi: [44] R. Kelly, V. Santibá nez, and J. A. Loría, Control of Robot Manipulators
10.5890/JAND.2019.06.001. in Joint Space (Advanced Textbooks in Control and Signal Processing).
[24] A. Grancharova and T. A. Johansen, Survey of Explicit Approaches to Con- London, U.K.: Springer-Verlag, 2005.
strained Optimal Control. Berlin, Germany: Springer, 2005, pp. 47–97, [45] H. Khalil, Nonlinear Systems (Pearson Education). Upper Saddle River,
doi: 10.1007/978-3-540-30560-6_3. NJ, USA: Prentice-Hall, 2002.
105000 VOLUME 9, 2021

JUAN ANTONIO ROJAS-QUINTERO received VICTOR SANTIBANEZ (Member, IEEE) received

the B.E. degree in science and technology of the B.E. and M.Sc. degrees in electronic engineer-
mechanics and engineering, the M.Sc. degree in ing from the Instituto Tecnológico de La Laguna,
mechanics and engineering sciences, and the Ph.D. Torreon, Coahuila, Mexico, in 1977 and 1984,
degree in mechanics and engineering sciences respectively, and the Ph.D. degree from CICESE,
from the University of Poitiers, Poitiers, France, Mexico, in 1997. He has coauthored two books
in 2007, 2009, and 2013, respectively. on robot control: Control of Robot Manipulators
He was a Postdoctoral Fellow with the School of (Prentice Hall, 2003) and Control of Robot Manip-
Mechanics and Engineering, Southwest Jiaotong ulators in Joint Space (Springer, 2005). He is the
University, Chengdu, China, from 2014 to 2016. author or coauthor of peer-reviewed journal arti-
He is currently a CONACYT Research Fellow with the National Technology cles and international conference papers. He is currently a Professor and
Institute (Tecnológico Nacional de México), Ensenada Campus, Mexico. His a Researcher with the Instituto Tecnológico de la Laguna. His research
research interests include nonlinear dynamical systems, optimal control, and interests include robot control, nonlinear systems control, fuzzy control, and
robotics. adaptive control.
JORGE VILLALOBOS-CHIN received the B.E.

degree in mechatronic engineering and the M.Sc.
degree in electric engineering from the Laguna
Institute of Technology, Torreon, Coahuila, Mex-
ico, in 2017 and 2019, respectively, where he is
currently pursuing the Ph.D. degree. His research
interests include stability analysis, nonlinear con-
trol design, and control of mechanical systems.
VOLUME 9, 2021 105001

Rojas Quintero Villalobos Chin Santibañez Optimal Control of Robotic System Using FE For Time Integration Covariant Control Equations

Uploaded by

Copyright:

Available Formats

Rojas Quintero Villalobos Chin Santibañez Optimal Control of Robotic System Using FE For Time Integration Covariant Control Equations

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Rojas Quintero Villalobos Chin Santibañez Optimal Control of Robotic System Using FE For Time Integration Covariant Control Equations

Uploaded by

Copyright:

Available Formats

Received July 13, 2021, accepted July 19, 2021, date of publication July 26, 2021, date of current

version August 2, 2021.

Optimal Control of Robotic Systems Using Finite

AND VICTOR SANTIBANEZ 2 , (Member, IEEE)

I. INTRODUCTION simpler subdomains that are called finite elements to achieve

VOLUME 9, 2021 104981

where the term M jk ∇k V = ∇ j V is the j th contravariant

104982 VOLUME 9, 2021

(1 − |t|)2 (1 + 2 |t|) ∀ |t| ≤ 1

VOLUME 9, 2021 104983

t (1 − |t|)3 (1 + 3|t|) ∀ |t| ≤ 1

104984 VOLUME 9, 2021

VOLUME 9, 2021 104985

Algorithm 1 HFE Time Discretization Algorithm

104986 VOLUME 9, 2021

VOLUME 9, 2021 104987

subject to the above mentioned boundary conditions, where

104988 VOLUME 9, 2021

VOLUME 9, 2021 104989

104990 VOLUME 9, 2021

VOLUME 9, 2021 104991

104992 VOLUME 9, 2021

converge at a rate which is one degree lower than the order of

VOLUME 9, 2021 104993

FIGURE 8. Schematic representation of the controller.

104994 VOLUME 9, 2021

1 ˙T To ensure that function (22) is positive definite and its

VOLUME 9, 2021 104995

104996 VOLUME 9, 2021

1) EXPERIMENT 1 the experiment. Negligible positioning errors were recorded

VOLUME 9, 2021 104997

104998 VOLUME 9, 2021

VOLUME 9, 2021 104999

105000 VOLUME 9, 2021

JUAN ANTONIO ROJAS-QUINTERO received VICTOR SANTIBANEZ (Member, IEEE) received

JORGE VILLALOBOS-CHIN received the B.E.

VOLUME 9, 2021 105001

You might also like