# A Method of Energy-Optimal Trajectory Planning for Palletizing Robot

Authors: Yanjie Liu; Le Liang; Haijun Han; Shijie Zhang
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

In this work, the energy-optimal trajectory planning and initial pick point searching problem for palletizing robot with high load capacity and high speed are studied, in which the pick point and place point of the robot are fixed to a desired location for each single task. These optimization problems have been transformed to ternary functional extremum problem and parameters optimal selection problem in which the performance index of the problems the rigid-flexible coupling dynamics model of the robot, and the constraint and boundary conditions of the robot are given. The fourth-order Runge-Kutta method, multiple shooting method, and traversing method are used to solve these specific mathematical problems. The effectiveness of the trajectory planning method is validated by the experimental and simulating results; thus the research work done here provides important support for subsequent palletizing robot research.


---

## Body

## 1. Introduction

During the last years, the rapid increase of the energy price together with strictly international and national policies has pointed out the problem of energy efficiency [1]. Reduction of our energy consumption, both on a household and on an industrial scale, therefore seems primordial from an economical as well as an ecological point of view [2].With the development of social productive forces, the handling efficiency and loading capacity of the palletizing robots have put forward more and more requirement in the material handling areas. Thus the high-energy consumption problem of the palletizing robot has aroused great concern, moving companies’ awareness from the reduction of the production time to the identification of an optimal trade-off between production time and energy consumption. In this work a method of energy-optimal trajectory planning for palletizing robot with high load capacity and high speed is proposed to solve the problem. The subject about trajectory planning has been extensively studied. The problem of trajectory planning for a robot is a very complex task and plays a crucial role in design and application of robots. Most of the work in this area is focused on smoothness, time-optimal, and energy-optimal trajectory planning.In the process of robotic trajectory planning, the continuity of the joint variables and their first two derivative must be ensured; otherwise vibration and impact will be produced; thus wear and consumption of the robot drive elements will be increased especially for high speed overloaded palletizing robot [3]. Aiming at this problem, Boryga and Graboś present a planning mode of trajectory motion for serial-link manipulators with higher-degree polynomials application, which is planned as the polynomials of degrees 5, 7, and 9 for the acceleration profiles of end-effector [4]. Azizi and Khani presented a new algorithm for smooth trajectory planning optimization of isotropic translational parallel manipulators that their Jacobian matrixes are constant and diagonal over the whole robot workspace [5]. Gasparetto and Zanotto proposed a new methodology, which is based on minimization of an objective function taking into account the execution time and integral of squared jerk along a whole trajectory of robotic manipulators [6].The above methods mainly considered the smoothness of robot trajectories under constraints of kinematics, dynamics, and control performances, which did not take into account time-optimal and energy-optimal aspects. Haddad et al. proposed an efficient stochastic scheme for minimum-time trajectory planning of a nonholonomic unicycle mobile robot under constraints on path curvature, velocities, and torques, which incorporated a trapezoidal-velocity-profile constraint that helps reduce the number of unknown parameters and speeds up the calculation steps [7]. A polynomial basis function trajectory parametrization was presented in [8], which enables direct export to executable robot code and reduces the number of variables in the optimization problem. Biagiotti and Melchiorri developed the trajectory planning of desired motion profiles for actuation systems of automatic machines [9]. Huang et al. proposed a novel minimum energy PTP trajectory planning method for a motor-toggle servomechanism [10]. Rubio et al. proposed an analysis of productivity of the industrial robots trajectory planning, which posed a multiobjective optimization problem to assess the trade-offs between the economic variables by means of the Pareto fronts [11]. Bonami et al. studied the energy-optimal motion planning problem for planar robot manipulators with two revolute joints [12]. Fung and Cheng designed a novel point-to-point trajectory based on minimum absolute input energy for an LCD glass-handing robot, which is driven by a permanent magnet synchronous motor [13]. Paes et al. presented a systematic methodology for on-site identification and energy-optimal path planning of an industrial robot, which was validated on an IRB1600 industrial ABB robot [14]. The optimization of energy consumption in product manufacture was deeply analyzed in [15], mainly focusing on the energy directly absorbed by the manufacturing process. Wang et al. considered real-time energy-optimal trajectory generation for a servomotor system which performs a single-axis point-to-point positioning task for a fixed time interval [16]. Saramago and Steffen Jr. considered a solution to the problem of moving a robot manipulator with minimum cost along a specified geometric path. The optimal traveling time and the minimum mechanical energy of the actuators are considered together to build a multiobjective function and the results depend on the associated weighting factors [17].Furthermore, there are many other research works about robotic trajectory planning [18–20]; however most of these research works still show some drawbacks. On the one hand, these research works are mostly based on specific functional form (such as B spline trajectory, etc.), which limits the concrete trajectory form and usually makes it hard to obtain global optimal solution. On the other hand, the weighting factors are frequently used to take off the objectives of time expending and energy consumption, which is too dependent on the selection of weight factors and lack of objectivity. This paper proposes a novel approach to solve the trajectory planning problem for palletizing robots. The planning objectives were mainly aimed at the optimal energy consumption, meanwhile the running time was set as constraint condition. In addition, this work considered the impacts of joint flexibility on the robot motion performance and innovatively proposed the initial pick point searching problem according to the working characteristics of palletizing robots. Many different calculation algorithms are used to solve these mathematical problems, and the effectiveness of the method is validated by the experimental and simulating results.The paper is structured as follows. Section2 establishes the rigid-flexible coupling dynamics model of the robot. The mathematical description and analysis process of the energy-optimal trajectory planning problem and energy-optimal pick point searching problem are proposed in Sections 3 and 4, respectively. Section 5 shows the experimental results and the conclusions of this paper are drawn in Section 6.


## 2. Rigid-Flexible Coupling Dynamics Model

Figure1 shows the three-dimensional model of the palletizing robot. The robot can be regarded as a series-parallel hybrid structure, which is composed of two groups of parallelogram mechanisms as shown in Figure 2. The red part is the main parallelogram, which can be used to determine the position of the end-effecter. And the green part is the subsidiary parallelogram, which can determine the orientation of the end-effecter.Figure 1
Three-dimensional model of the palletizing robot. 1: big arm; 2: forearm; 3: parallel arm; 4: drive link of small arm; 5: parallel link of big arm; 6: parallel link of forearm; 7: triangle bracket; 8: the end of the wrist; 9: waist support; 10: base.Figure 2
Schematic of the robot mechanism.Table1 shows the definition of main structural physical parameters of the palletizing robot.Table 1
Definition of main structural physical parameters.
Physical meaning
Symbol
Value
Waist joint angle/rad
θ
1
—
Big arm joint angle/rad
θ
2
—
Forearm joint angle/rad
θ
3
—
Length of big arm/mm
l
1
945
Length of forearm/mm
l
2
1025
Length of parallel arm/mm
l
3
400
Offset distance of 2-link/mm
s
1
267
Offset distance of 3-link /mm
s
2
740The kinetic energy of all components can be calculated and the rigid-body dynamics equation can be obtained using the second-type Lagrange equation as follows:(1)
M
θ
·
θ
¨
+
C
θ
,
θ
˙
θ
˙
+
G
θ
=
T
,where M
θ is the inertia matrix, c
θ
,
θ
˙ is Coriolis force item and centrifugal force item, G
θ is gravity item.The inertia matrix is as follows:(2)
M
θ
=
M
11
0
0
0
M
22
M
23
0
M
32
M
33
,where(3)
M
11
=
4.36
cos
2
⁡
θ
3
+
0.26
-
45.10
cos
⁡
θ
3
+
0.26
cos
⁡
θ
2
+
234.42
cos
2
⁡
θ
3
+
345.89
cos
2
⁡
θ
2
-
394.58
cos
⁡
θ
2
cos
⁡
θ
3
+
192.48
cos
⁡
θ
2
-
94.58
cos
⁡
θ
3
-
10.76
cos
⁡
θ
3
+
0.26
+
52.56
,
M
22
=
365.37
,
M
33
=
242.26
,
M
23
=
M
32
=
22.55
cos
⁡
θ
2
-
θ
3
-
0.26
-
194.19
cos
⁡
θ
2
-
θ
3
.The matrix of Coriolis force item and centrifugal force item is as follows:(4)
C
θ
,
θ
˙
=
0
C
12
θ
˙
2
C
13
θ
˙
3
C
21
θ
˙
1
0
C
23
θ
˙
3
C
31
θ
˙
1
C
32
θ
˙
2
0
,where(5)
C
12
=
-
2
C
21
=
45.10
sin
⁡
θ
2
cos
⁡
θ
3
+
0.26
-
345.89
sin
⁡
θ
2
+
394.58
sin
⁡
θ
2
cos
⁡
θ
3
-
192.49
sin
⁡
θ
3
,
C
13
=
-
2
C
31
=
10.76
sin
⁡
θ
3
+
0.26
-
4.36
sin
⁡
2
θ
3
+
0.53
+
394.58
cos
⁡
θ
2
sin
⁡
θ
3
+
45.10
sin
⁡
θ
3
+
0.26
cos
⁡
θ
2
+
94.58
sin
⁡
θ
3
-
235.42
sin
⁡
2
θ
3
,
C
23
=
-
C
32
=
22.55
sin
⁡
θ
2
-
θ
3
-
0.26
-
194.19
sin
⁡
θ
2
-
θ
3
.It is observed that the parameters in the matrixes ofM
θ and c
θ
,
θ
˙ have the below relationship, which will bring convenient for the Euler equation simplification in Section 3.2.(6)
∂
M
11
∂
θ
2
=
C
12
,
∂
M
11
∂
θ
3
=
C
13
,
∂
M
23
∂
θ
2
=
-
C
23
,
∂
C
23
∂
θ
2
=
M
23
,
∂
M
32
∂
θ
3
=
-
C
32
,
∂
C
32
∂
θ
3
=
M
32
.Figure3 exhibits the equivalence principle of the robot flexible joint. The reducer was equivalent to a stiffness reduction device and a flexible torsion bar.Figure 3
The model of flexible joint.Combined with rigid-body dynamic equation, the rigid-flexible coupling dynamics equation can be obtained as follows:(7)
T
m
=
J
m
θ
m
¨
+
I
-
1
T
l
,
T
l
=
K
I
-
1
θ
m
-
θ
l
+
B
I
-
1
θ
˙
m
-
θ
˙
l
,
(8)
M
θ
l
·
θ
l
¨
+
C
θ
l
,
θ
l
˙
θ
l
˙
+
G
θ
l
=
K
D
+
B
D
˙
,
T
m
=
J
m
θ
m
¨
+
K
I
-
1
D
+
B
I
-
1
D
˙
,
D
=
I
-
1
θ
m
-
θ
l
,where T
m is the output torque of motor, θ
m is the output angle of motor, T
l is the output torque of robot, θ
l is the output angle of robot, J
m is the motor rotor inertia,K is the reducer stiffness,B is the damping ratio, andI is the reduction ratio.In order to verify the correctness of the dynamic equations, the torque curves of rigid-body dynamics model (RDM) and rigid-flexible coupling dynamics model (RFCDM) and ADAMS-based virtual prototype model (VPM) are compared. The palletizing robot worked along door-shaped velocity trajectory. The torque errors between RDM and VPM are expressed in magenta chain lines (TERDM), and the torque errors between RFCDM and VPM are expressed in black chain lines (TERFCDM) as shown in Figure4.Figure 4
Torque error between dynamic models and virtual prototype model. (a) Joint 1; (b) joint 2; (c) joint 3.
(a)
(b)
(c)It can be observed that RFCDM can make the maximum torque error of joint 1 to joint 3 reduce from (647.06 Nm, 238.46 Nm, and 206.25 Nm) to (294.12 Nm, 230.77 Nm, and 61.25 Nm), respectively, which can verify the correctness of the rigid-flexible coupling dynamics model.


## 3. Energy-Optimal Trajectory Planning

In this section, the energy-optimal trajectory planning problem for palletizing robot will be stated, where the end-effector is constrained to move between the pick point and place point. Then, based on the optimal trajectory for each handling task, the energy-optimal pick point searching problem will be introduced in Section4.


### 3.1. Mathematical Description of the Problem

This paper focuses on the robot energy consumption in pick-and-place task. The pick point and place point will be fixed for this problem, and the running time will be constrained as constant based on testing data and engineering requirement. In the process of calculation the following energy consumption will be neglected, such as mechanical friction, motor braking, and some affiliated electrical power, which is not much different under different trajectories.The coordinates of pick point and place point are set asθ
l
10
,
θ
l
20
,
θ
l
30 and θ
l
1
f
,
θ
l
2
f
,
θ
l
3
f, respectively, in joint space. In order to ensure the rapidity of the robot, the running time is set as t
f
=
d
/
v
-
m
a
x, whered is the total distance and v
-
m
a
x is the maximum average palletizing speed. The energy consumption of the robot can be obtained as follows:(9)
E
=
∑
i
=
1
3
∫
0
t
f
T
m
i
θ
˙
m
i
d
t
.Since the above equation contains absolute value function which is not convenient to solve the functional extremum problem, the performance indexJ is selected as energy consumption index.(10)
J
=
∫
0
t
f
T
m
T
T
m
d
t
=
∑
i
=
1
3
∫
0
t
f
T
m
i
2
d
t
.In addition, the output torque of the motors is selected as constraint condition, the initial velocity and acceleration are selected as initial conditions, and the terminal velocity and acceleration are selected as terminal conditions. So the energy-optimal trajectory planning problem can be described as follows:The robotic rigid-flexible coupling dynamics condition of the energy consumption indexJ is proposed in equation (8).The constraint conditions are(11)
θ
l
0
=
θ
l
0
,
θ
l
t
f
=
θ
l
f
,
t
f
=
d
v
-
m
a
x
,
T
m
i
n
≤
T
m
≤
T
m
a
x
.The boundary conditions are(12)
θ
˙
l
0
=
0
,
θ
¨
l
0
=
0
,
θ
˙
l
t
f
=
0
,
θ
¨
l
t
f
=
0
.Under these conditions, the energy-optimal trajectoryθ
l
t (θ
l
t
=
θ
l
1
t
,
θ
l
2
t
,
θ
l
3
t
T) will be solved.So the energy-optimal trajectory planning problem is transformed to a ternary functional extremum problem with mixed boundary conditions.


### 3.2. Solving and Analysis

In this section the fourth-order Runge-Kutta method and multiple shooting method are used to solve the ternary functional extremum problem.According to the Euler function of the functional extremum condition, the following equation can be obtained:(13)
∑
k
=
0
n
-
1
k
d
k
d
t
k
F
θ
l
i
k
=
0
i
=
1,2
,
3
,where F
θ
l
i
k is the first-order partial derivative ofF with respect to θ
l
i
k,F is the performance index of the extremum problem; that is, F
=
T
m
T
T
m
=
T
m
1
2
+
T
m
2
2
+
T
m
3
2, θ
l
i
k is thekth-order derivative of θ
l
i with respect to timet; that is, θ
l
i
k
=
d
k
/
d
t
k
θ
l
i, andn is the order of the highest-order derivative.According to (8), n equates 2; then (13) can be transformed as follows:(14)
F
θ
l
1
-
d
d
t
F
θ
˙
l
1
+
d
2
d
t
2
F
θ
¨
l
1
=
0
,
F
θ
l
2
-
d
d
t
F
θ
˙
l
2
+
d
2
d
t
2
F
θ
¨
l
2
=
0
,
F
θ
l
3
-
d
d
t
F
θ
˙
l
3
+
d
2
d
t
2
F
θ
¨
l
3
=
0
.Putting the rigid-flexible coupling dynamics equation (8) into (14) and using (6) for simplification, the fourth-order ordinary differential equations of θ
1, θ
2, and θ
3 can be obtained as follows:(15)
θ
1
4
=
f
1
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
,
θ
2
4
=
f
2
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
,
θ
3
4
=
f
3
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
.Setting:x
1
=
θ
1, x
2
=
θ
2, x
3
=
θ
3, x
4
=
θ
˙
1, x
5
=
θ
˙
2, x
6
=
θ
˙
3, x
7
=
θ
¨
1, x
8
=
θ
¨
2, x
9
=
θ
¨
3, x
10
=
θ
⃛
1, x
11
=
θ
⃛
2, x
12
=
θ
⃛
3, and X
=
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
,
x
7
,
x
8
,
x
9
,
x
10
,
x
11
,
x
12
T, then (15) can be converted into solving the vectorX:(16)
X
˙
=
f
X
.The numerical solution of (16) is proceeded based on the four-level Runge-Kutta method:(17)
X
n
+
1
=
X
n
+
h
6
K
1
+
K
2
+
K
3
+
K
4
,
K
1
=
f
X
n
,
K
2
=
f
X
n
+
h
2
K
1
,
K
3
=
f
X
n
+
h
2
K
2
,
K
4
=
f
X
n
+
h
K
3
.Since the constraint condition of trajectory planning problem is not exactly initial condition but mixed boundary condition, only using Runge-Kutta method is infeasible. So the multiple shooting method is introduced to solve the problem together.Make assumptions on the initial conditions:θ
⃛
l
1
0
=
z
1, θ
⃛
l
2
0
=
z
2, and θ
⃛
l
3
0
=
z
3. So the problem is transformed to select the appropriate Z
1
,
Z
2
,
Z
3 which can make the results calculated by Runge-Kutta method meet constraint conditions, and then the numerical solution for the optimization problem can be obtained. The solving approach is that firstly suppose Z
1
=
Z
10, Z
2
=
Z
20, and Z
3
=
Z
30; then the coordinates of the place point can be calculated as β
10
,
β
20
,
β
30 by inverse kinematics analysis. If β
10
-
θ
1
l
f
2
+
β
20
-
θ
2
l
f
2
+
β
30
-
θ
3
l
f
2
≤
ε (ε is the error limit), the numerical solution of equation (16) can be obtained. Otherwise, the value of Z
1
,
Z
2
,
Z
3 will be reset.Z
i
i
=
1,2
,
3 can be calculated by linear interpolation method:(18)
z
i
k
+
1
=
z
i
k
-
β
1
k
-
θ
1
l
f
β
1
k
-
β
1
k
-
1
z
i
k
-
z
i
k
-
1
,
k
=
1,2
,
…
.The procedure flow chart of the energy-optimal trajectory planning problem is shown in Figure5.Figure 5
Procedure flow chart of the energy-optimal trajectory planning problem.In order to verify the effectiveness of the method, the comparison between the energy-optimal trajectory and seven-degree polynomial trajectory is proceeded as shown in Figure6. The maximum output torques of the first three joints are set to 100 Nm, 70 Nm, and 70 Nm, respectively, and the running time is set to 1 second.Figure 6
Comparison between the energy-optimal trajectory and seven-degree polynomial trajectory. (a) Trajectory comparison; (b) energy consumption comparison.
(a)
(b)As can be seen from Figure6, for the point-to-point task, the seven-degree polynomial trajectory is a straight line; however, the energy-optimal trajectory is a curve. And compared with seven-degree polynomial trajectory, the energy consumption of energy-optimal trajectory is reduced from 14.674 kJ to 12.702 kJ. The reduction rate can reach up to 13.44% which can verify the effectiveness of the method.


## 3.1. Mathematical Description of the Problem

This paper focuses on the robot energy consumption in pick-and-place task. The pick point and place point will be fixed for this problem, and the running time will be constrained as constant based on testing data and engineering requirement. In the process of calculation the following energy consumption will be neglected, such as mechanical friction, motor braking, and some affiliated electrical power, which is not much different under different trajectories.The coordinates of pick point and place point are set asθ
l
10
,
θ
l
20
,
θ
l
30 and θ
l
1
f
,
θ
l
2
f
,
θ
l
3
f, respectively, in joint space. In order to ensure the rapidity of the robot, the running time is set as t
f
=
d
/
v
-
m
a
x, whered is the total distance and v
-
m
a
x is the maximum average palletizing speed. The energy consumption of the robot can be obtained as follows:(9)
E
=
∑
i
=
1
3
∫
0
t
f
T
m
i
θ
˙
m
i
d
t
.Since the above equation contains absolute value function which is not convenient to solve the functional extremum problem, the performance indexJ is selected as energy consumption index.(10)
J
=
∫
0
t
f
T
m
T
T
m
d
t
=
∑
i
=
1
3
∫
0
t
f
T
m
i
2
d
t
.In addition, the output torque of the motors is selected as constraint condition, the initial velocity and acceleration are selected as initial conditions, and the terminal velocity and acceleration are selected as terminal conditions. So the energy-optimal trajectory planning problem can be described as follows:The robotic rigid-flexible coupling dynamics condition of the energy consumption indexJ is proposed in equation (8).The constraint conditions are(11)
θ
l
0
=
θ
l
0
,
θ
l
t
f
=
θ
l
f
,
t
f
=
d
v
-
m
a
x
,
T
m
i
n
≤
T
m
≤
T
m
a
x
.The boundary conditions are(12)
θ
˙
l
0
=
0
,
θ
¨
l
0
=
0
,
θ
˙
l
t
f
=
0
,
θ
¨
l
t
f
=
0
.Under these conditions, the energy-optimal trajectoryθ
l
t (θ
l
t
=
θ
l
1
t
,
θ
l
2
t
,
θ
l
3
t
T) will be solved.So the energy-optimal trajectory planning problem is transformed to a ternary functional extremum problem with mixed boundary conditions.


## 3.2. Solving and Analysis

In this section the fourth-order Runge-Kutta method and multiple shooting method are used to solve the ternary functional extremum problem.According to the Euler function of the functional extremum condition, the following equation can be obtained:(13)
∑
k
=
0
n
-
1
k
d
k
d
t
k
F
θ
l
i
k
=
0
i
=
1,2
,
3
,where F
θ
l
i
k is the first-order partial derivative ofF with respect to θ
l
i
k,F is the performance index of the extremum problem; that is, F
=
T
m
T
T
m
=
T
m
1
2
+
T
m
2
2
+
T
m
3
2, θ
l
i
k is thekth-order derivative of θ
l
i with respect to timet; that is, θ
l
i
k
=
d
k
/
d
t
k
θ
l
i, andn is the order of the highest-order derivative.According to (8), n equates 2; then (13) can be transformed as follows:(14)
F
θ
l
1
-
d
d
t
F
θ
˙
l
1
+
d
2
d
t
2
F
θ
¨
l
1
=
0
,
F
θ
l
2
-
d
d
t
F
θ
˙
l
2
+
d
2
d
t
2
F
θ
¨
l
2
=
0
,
F
θ
l
3
-
d
d
t
F
θ
˙
l
3
+
d
2
d
t
2
F
θ
¨
l
3
=
0
.Putting the rigid-flexible coupling dynamics equation (8) into (14) and using (6) for simplification, the fourth-order ordinary differential equations of θ
1, θ
2, and θ
3 can be obtained as follows:(15)
θ
1
4
=
f
1
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
,
θ
2
4
=
f
2
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
,
θ
3
4
=
f
3
θ
1
,
θ
2
,
θ
3
,
θ
˙
1
,
θ
˙
2
,
θ
˙
3
,
θ
¨
1
,
θ
¨
2
,
θ
¨
3
,
θ
⃛
1
,
θ
⃛
2
,
θ
⃛
3
.Setting:x
1
=
θ
1, x
2
=
θ
2, x
3
=
θ
3, x
4
=
θ
˙
1, x
5
=
θ
˙
2, x
6
=
θ
˙
3, x
7
=
θ
¨
1, x
8
=
θ
¨
2, x
9
=
θ
¨
3, x
10
=
θ
⃛
1, x
11
=
θ
⃛
2, x
12
=
θ
⃛
3, and X
=
x
1
,
x
2
,
x
3
,
x
4
,
x
5
,
x
6
,
x
7
,
x
8
,
x
9
,
x
10
,
x
11
,
x
12
T, then (15) can be converted into solving the vectorX:(16)
X
˙
=
f
X
.The numerical solution of (16) is proceeded based on the four-level Runge-Kutta method:(17)
X
n
+
1
=
X
n
+
h
6
K
1
+
K
2
+
K
3
+
K
4
,
K
1
=
f
X
n
,
K
2
=
f
X
n
+
h
2
K
1
,
K
3
=
f
X
n
+
h
2
K
2
,
K
4
=
f
X
n
+
h
K
3
.Since the constraint condition of trajectory planning problem is not exactly initial condition but mixed boundary condition, only using Runge-Kutta method is infeasible. So the multiple shooting method is introduced to solve the problem together.Make assumptions on the initial conditions:θ
⃛
l
1
0
=
z
1, θ
⃛
l
2
0
=
z
2, and θ
⃛
l
3
0
=
z
3. So the problem is transformed to select the appropriate Z
1
,
Z
2
,
Z
3 which can make the results calculated by Runge-Kutta method meet constraint conditions, and then the numerical solution for the optimization problem can be obtained. The solving approach is that firstly suppose Z
1
=
Z
10, Z
2
=
Z
20, and Z
3
=
Z
30; then the coordinates of the place point can be calculated as β
10
,
β
20
,
β
30 by inverse kinematics analysis. If β
10
-
θ
1
l
f
2
+
β
20
-
θ
2
l
f
2
+
β
30
-
θ
3
l
f
2
≤
ε (ε is the error limit), the numerical solution of equation (16) can be obtained. Otherwise, the value of Z
1
,
Z
2
,
Z
3 will be reset.Z
i
i
=
1,2
,
3 can be calculated by linear interpolation method:(18)
z
i
k
+
1
=
z
i
k
-
β
1
k
-
θ
1
l
f
β
1
k
-
β
1
k
-
1
z
i
k
-
z
i
k
-
1
,
k
=
1,2
,
…
.The procedure flow chart of the energy-optimal trajectory planning problem is shown in Figure5.Figure 5
Procedure flow chart of the energy-optimal trajectory planning problem.In order to verify the effectiveness of the method, the comparison between the energy-optimal trajectory and seven-degree polynomial trajectory is proceeded as shown in Figure6. The maximum output torques of the first three joints are set to 100 Nm, 70 Nm, and 70 Nm, respectively, and the running time is set to 1 second.Figure 6
Comparison between the energy-optimal trajectory and seven-degree polynomial trajectory. (a) Trajectory comparison; (b) energy consumption comparison.
(a)
(b)As can be seen from Figure6, for the point-to-point task, the seven-degree polynomial trajectory is a straight line; however, the energy-optimal trajectory is a curve. And compared with seven-degree polynomial trajectory, the energy consumption of energy-optimal trajectory is reduced from 14.674 kJ to 12.702 kJ. The reduction rate can reach up to 13.44% which can verify the effectiveness of the method.


## 4. Energy-Optimal Pick Point Searching

### 4.1. Mathematical Description of the Problem

In the field of palletizing robot application, the position of the final stack of the materials is usually fixed within the robot workspace. However, the selection of the pick point of the materials is based on experience, which does not take into account the effect of the location selection on the energy consumption in the whole process of palletizing.According to Section3.1, the energy consumption is mainly related to the location of the pick-and-place points. So the optimal location of the pick point can make the energy consumption minimum. The schematic diagram of the problem is shown in Figure 7, where P
0 is the pick point of the materials which usually is the end position of the conveyor belt, P
i
,
j
,
k is the place point among the stack, and i
,
j
,
k are numerical symbols of the materials location.Figure 7
Schematic diagram of the problem.The problem can be described as searching the optimal pick pointP
0
∗
=
x
0
∗
,
y
0
∗
,
z
0
∗ in order to minimize the energy consumption index of the whole stack J
S.(19)
J
S
=
∑
i
=
1
c
∑
j
=
1
l
∑
k
=
1
v
J
m
i
n
P
0
∗
,
P
i
,
j
,
k
,where J
m
i
n
P
0
∗
,
P
i
,
j
,
k is the energy consumption index for single task when the robot follow the trajectory mentioned in Section 3.1 and c
,
l
,
v are the number of materials in the stack along the direction of crosswise, lengthways, and vertical.The constraint condition is(20)
d
=
x
0
-
x
m
i
d
2
+
y
0
-
y
m
i
d
2
+
z
0
-
z
m
i
d
2
≥
d
m
i
n
,whered is the distance between the central point of the stack P
m
i
d
x
m
i
d
,
y
m
i
d
,
z
m
i
d and the pick point P
0
x
0
,
y
0
,
z
0 and d
m
i
n is the minimum distance which can be calculated by the robot kinematics analysis.Since the smaller the distance, the lower the total energy consumption,d is set equal to d
m
i
n. The optional position of P
0 constitutes a spherical surface, and its parametric equation can be expressed as follows:(21)
x
0
=
x
m
i
d
+
d
m
i
n
cos
⁡
α
cos
⁡
β
,
y
0
=
y
m
i
d
+
d
m
i
n
cos
⁡
α
sin
⁡
β
,
z
0
=
z
m
i
d
+
d
m
i
n
sin
⁡
α
,where α and β are positional parameters of the spherical surface, -
π
/
2
≤
α
≤
π
/
2, 0
≤
β
≤
2
π.So the energy-optimal pick point searching problem is transformed to a parameter (α, β) optimal selection problem.


### 4.2. Solving and Analysis

According to the solving procedure of Section3.2, the lowest energy consumption of the robot is mainly related to the location of pick point and place point when the points are fixed and can be obtained by the algorithm mentioned in Section 3.2. However, the algorithm can only obtain the numerical solution instead of a certain functional relationship. Since there is no analytical solution for the optimal solution, the traversing method will be used to solve the energy-optimal pick point searching problem.Firstly,α will be divided into a parts and β will be divided into b parts; that is,(22)
α
=
-
π
2
+
e
-
1
π
a
-
1
,
β
=
2
f
-
1
π
b
-
1
,where 1
≤
e
≤
a, 1
≤
f
≤
b.So the spherical surface, takingP
mid as sphere center and d
m
i
n as radius, is divided into a
×
b parts, and the position of P
0 will change with e and f. Then the energy-optimal trajectory from point P
0
e
,
f to point P
i
,
j
,
k and the minimum energy consumption of the single task can be calculated as Section 3.2. Afterward J
S can be obtained by adding this minimum energy consumption of the whole stack of materials, and the optimal pick point can be selected by comparing the values of J
S under different choice of P
0.Obviously the traversing method can only obtain an approximate solution ofP
0, and the higher the value of e and f, the greater the calculating precision. The procedure flow chart of the energy-optimal pick point searching problem is shown in Figure 8.Figure 8
Procedure flow chart of the energy-optimal pick point searching problem.The stacking style of materials is selected as a cuboid centered at (1.5, 0, 1), as shown in Figure9, the size of each material is 35
×
35
×
20 cm, with 10 layers and 9 materials on each layer, and the red part is the projection of the robotic workspace in x and z directions. Then the central point of the stack is P
m
i
d
0.8
+
0.35
i
,
-
0.35
+
0.35
j
,
-
0.1
+
0.2
k. And the relationship between the energy consumption and initial location of the pick point is shown in Figure 10.Figure 9
Relationship between stacking style and robotic workspace.Figure 10
Relationship between energy consumption and pick point location.In Figure10, the spherical surface represents the optional position range of the pick points, and the corresponding energy consumption value is shown by cloud chart. According to the simulation result, when e
=
90 and f
=
1, that is, P
0
∗
1.8275,0
,
1.9448, the total energy consumption is minimum as 4.0092
×
10
5 J. However, when the pick point is set in the edge of the robotic workspace as (1.1543, 0.3440, 0.11428), the total energy consumption increases almost 20-fold as 8.3747
×
10
6 J, which is because the pick point is adverse to the start and stop motion of the robot. So the selection of the materials pick point location is very necessary in the process of palletizing robot system design.


## 4.1. Mathematical Description of the Problem

In the field of palletizing robot application, the position of the final stack of the materials is usually fixed within the robot workspace. However, the selection of the pick point of the materials is based on experience, which does not take into account the effect of the location selection on the energy consumption in the whole process of palletizing.According to Section3.1, the energy consumption is mainly related to the location of the pick-and-place points. So the optimal location of the pick point can make the energy consumption minimum. The schematic diagram of the problem is shown in Figure 7, where P
0 is the pick point of the materials which usually is the end position of the conveyor belt, P
i
,
j
,
k is the place point among the stack, and i
,
j
,
k are numerical symbols of the materials location.Figure 7
Schematic diagram of the problem.The problem can be described as searching the optimal pick pointP
0
∗
=
x
0
∗
,
y
0
∗
,
z
0
∗ in order to minimize the energy consumption index of the whole stack J
S.(19)
J
S
=
∑
i
=
1
c
∑
j
=
1
l
∑
k
=
1
v
J
m
i
n
P
0
∗
,
P
i
,
j
,
k
,where J
m
i
n
P
0
∗
,
P
i
,
j
,
k is the energy consumption index for single task when the robot follow the trajectory mentioned in Section 3.1 and c
,
l
,
v are the number of materials in the stack along the direction of crosswise, lengthways, and vertical.The constraint condition is(20)
d
=
x
0
-
x
m
i
d
2
+
y
0
-
y
m
i
d
2
+
z
0
-
z
m
i
d
2
≥
d
m
i
n
,whered is the distance between the central point of the stack P
m
i
d
x
m
i
d
,
y
m
i
d
,
z
m
i
d and the pick point P
0
x
0
,
y
0
,
z
0 and d
m
i
n is the minimum distance which can be calculated by the robot kinematics analysis.Since the smaller the distance, the lower the total energy consumption,d is set equal to d
m
i
n. The optional position of P
0 constitutes a spherical surface, and its parametric equation can be expressed as follows:(21)
x
0
=
x
m
i
d
+
d
m
i
n
cos
⁡
α
cos
⁡
β
,
y
0
=
y
m
i
d
+
d
m
i
n
cos
⁡
α
sin
⁡
β
,
z
0
=
z
m
i
d
+
d
m
i
n
sin
⁡
α
,where α and β are positional parameters of the spherical surface, -
π
/
2
≤
α
≤
π
/
2, 0
≤
β
≤
2
π.So the energy-optimal pick point searching problem is transformed to a parameter (α, β) optimal selection problem.


## 4.2. Solving and Analysis

According to the solving procedure of Section3.2, the lowest energy consumption of the robot is mainly related to the location of pick point and place point when the points are fixed and can be obtained by the algorithm mentioned in Section 3.2. However, the algorithm can only obtain the numerical solution instead of a certain functional relationship. Since there is no analytical solution for the optimal solution, the traversing method will be used to solve the energy-optimal pick point searching problem.Firstly,α will be divided into a parts and β will be divided into b parts; that is,(22)
α
=
-
π
2
+
e
-
1
π
a
-
1
,
β
=
2
f
-
1
π
b
-
1
,where 1
≤
e
≤
a, 1
≤
f
≤
b.So the spherical surface, takingP
mid as sphere center and d
m
i
n as radius, is divided into a
×
b parts, and the position of P
0 will change with e and f. Then the energy-optimal trajectory from point P
0
e
,
f to point P
i
,
j
,
k and the minimum energy consumption of the single task can be calculated as Section 3.2. Afterward J
S can be obtained by adding this minimum energy consumption of the whole stack of materials, and the optimal pick point can be selected by comparing the values of J
S under different choice of P
0.Obviously the traversing method can only obtain an approximate solution ofP
0, and the higher the value of e and f, the greater the calculating precision. The procedure flow chart of the energy-optimal pick point searching problem is shown in Figure 8.Figure 8
Procedure flow chart of the energy-optimal pick point searching problem.The stacking style of materials is selected as a cuboid centered at (1.5, 0, 1), as shown in Figure9, the size of each material is 35
×
35
×
20 cm, with 10 layers and 9 materials on each layer, and the red part is the projection of the robotic workspace in x and z directions. Then the central point of the stack is P
m
i
d
0.8
+
0.35
i
,
-
0.35
+
0.35
j
,
-
0.1
+
0.2
k. And the relationship between the energy consumption and initial location of the pick point is shown in Figure 10.Figure 9
Relationship between stacking style and robotic workspace.Figure 10
Relationship between energy consumption and pick point location.In Figure10, the spherical surface represents the optional position range of the pick points, and the corresponding energy consumption value is shown by cloud chart. According to the simulation result, when e
=
90 and f
=
1, that is, P
0
∗
1.8275,0
,
1.9448, the total energy consumption is minimum as 4.0092
×
10
5 J. However, when the pick point is set in the edge of the robotic workspace as (1.1543, 0.3440, 0.11428), the total energy consumption increases almost 20-fold as 8.3747
×
10
6 J, which is because the pick point is adverse to the start and stop motion of the robot. So the selection of the materials pick point location is very necessary in the process of palletizing robot system design.


## 5. Experimental Verification

### 5.1. Experimental System

A servo control system based on EtherCAT bus and palletizing robot body experimental platform were built in this work. Figure11 shows the entire experimental system which mainly consists of EtherCAT based servo control system and the robot mechanical system.Figure 11
Robotic system.Beckhoff IPC C6640 was used as industrial computer which installed TwinCAT3 programming software. TwinCAT software based on real-time operating system integrates the functionality of PLC and NC, and the real-time control cycle is only 50μs, which can meet the real-time requirements of the trajectory planning algorithm. The RS2 servo drives were applied in this work which supported EtherCAT bus communications. So real-time communication and motor control could be realized via high-speed EtherCAT bus.


### 5.2. Experimental Verification

The experimental verification for the energy-optimal trajectory planning algorithm was accomplished in this work. The seven-degree polynomial trajectory and energy-optimal trajectory planning method were proceeded, respectively, in the condition of same pick point and place point. In the operational process, the actual output torque of the motors and actual speed of the joints can be real-time fed back using TwinCAT NCtoPLC function. Then the instantaneous power value of the motors can be recorded by Scope function. So the actual energy consumption can be calculated as follows:(23)
E
a
=
∑
i
=
1
n
P
i
Δ
t
,where E
a is the actual energy consumption of the robot, P
i is the instantaneous power value of the motors in the discrete pointi, and Δ
t is control period which is set to 1 ms in this work.The robot was controlled along the seven-degree polynomial trajectory and energy-optimal trajectory as Figure6, and the running time is set to 1000 microsecond. In order to avoid the occasionality in the process of experiment, the test was proceeded 10 times for the two trajectories, and the test results are shown in Table 2.Table 2
Energy consumption of the robot along the two trajectories.
Test number
Energy-optimal trajectory (kJ)
Seven-degree polynomial trajectory (kJ)
1
14.881
17.221
2
14.492
17.015
3
14.885
16.793
4
14.112
17.436
5
14.379
16.965
6
14.289
17.247
7
14.155
16.578
8
13.949
17.432
9
14.907
17.045
10
14.231
16.628The average values of the tests results were taken as the actual energy consumption of the robot, which were calculated as 14.428 kJ and 17.036 kJ under the conditions of energy-optimal trajectory and seven-degree polynomial trajectory, respectively. It shows that the energy-optimal trajectory planning method can make the energy consumption decrease by 15.309%. In addition, compared with the simulation results the actual energy consumption is slightly larger, which is because the friction is not considered in the process of simulation.The palletizing experiments for a whole stack of materials were proceeded also, while the pick point is set in the optimal location of the robotic workspace asP
0
∗
1.8275,0
,
1.9448. The total energy consumption was tested as 4.7293
×
10
2 kJ and 5.8269
×
10
2 kJ under the conditions of energy-optimal trajectory and seven-degree polynomial trajectory respectively. That is to say, under the same initial conditions, the total energy consumption for the whole stack of materials was reduced by 30.21% while using the optimal trajectory planning method.


## 5.1. Experimental System

A servo control system based on EtherCAT bus and palletizing robot body experimental platform were built in this work. Figure11 shows the entire experimental system which mainly consists of EtherCAT based servo control system and the robot mechanical system.Figure 11
Robotic system.Beckhoff IPC C6640 was used as industrial computer which installed TwinCAT3 programming software. TwinCAT software based on real-time operating system integrates the functionality of PLC and NC, and the real-time control cycle is only 50μs, which can meet the real-time requirements of the trajectory planning algorithm. The RS2 servo drives were applied in this work which supported EtherCAT bus communications. So real-time communication and motor control could be realized via high-speed EtherCAT bus.


## 5.2. Experimental Verification

The experimental verification for the energy-optimal trajectory planning algorithm was accomplished in this work. The seven-degree polynomial trajectory and energy-optimal trajectory planning method were proceeded, respectively, in the condition of same pick point and place point. In the operational process, the actual output torque of the motors and actual speed of the joints can be real-time fed back using TwinCAT NCtoPLC function. Then the instantaneous power value of the motors can be recorded by Scope function. So the actual energy consumption can be calculated as follows:(23)
E
a
=
∑
i
=
1
n
P
i
Δ
t
,where E
a is the actual energy consumption of the robot, P
i is the instantaneous power value of the motors in the discrete pointi, and Δ
t is control period which is set to 1 ms in this work.The robot was controlled along the seven-degree polynomial trajectory and energy-optimal trajectory as Figure6, and the running time is set to 1000 microsecond. In order to avoid the occasionality in the process of experiment, the test was proceeded 10 times for the two trajectories, and the test results are shown in Table 2.Table 2
Energy consumption of the robot along the two trajectories.
Test number
Energy-optimal trajectory (kJ)
Seven-degree polynomial trajectory (kJ)
1
14.881
17.221
2
14.492
17.015
3
14.885
16.793
4
14.112
17.436
5
14.379
16.965
6
14.289
17.247
7
14.155
16.578
8
13.949
17.432
9
14.907
17.045
10
14.231
16.628The average values of the tests results were taken as the actual energy consumption of the robot, which were calculated as 14.428 kJ and 17.036 kJ under the conditions of energy-optimal trajectory and seven-degree polynomial trajectory, respectively. It shows that the energy-optimal trajectory planning method can make the energy consumption decrease by 15.309%. In addition, compared with the simulation results the actual energy consumption is slightly larger, which is because the friction is not considered in the process of simulation.The palletizing experiments for a whole stack of materials were proceeded also, while the pick point is set in the optimal location of the robotic workspace asP
0
∗
1.8275,0
,
1.9448. The total energy consumption was tested as 4.7293
×
10
2 kJ and 5.8269
×
10
2 kJ under the conditions of energy-optimal trajectory and seven-degree polynomial trajectory respectively. That is to say, under the same initial conditions, the total energy consumption for the whole stack of materials was reduced by 30.21% while using the optimal trajectory planning method.


## 6. Conclusion

This work deals with trajectory planning of palletizing robots for stacking task; the main contents of the research include the following aspects:(1)
The rigid-flexible coupling dynamics model of the 4-DOF palletizing robot is deduced and verified by virtual prototype model and mathematical tools, which can be used for energy-optimal trajectory planning.
(2)
The mathematical description of the energy-optimal trajectory planning problem is proposed, which is transformed to a ternary functional extremum problem with mixed boundary conditions and solved by the fourth-order Runge-Kutta method and multiple shooting method. The experimental and simulating results show that the energy consumption is reduced obviously compared with seven-degree polynomial trajectory.
(3)
The energy-optimal pick point searching problem is introduced, which is transformed to a parameter optimal selection problem and solved by the traversing method. The calculation results validate that the materials pick point location has important effects on energy consumption of palletizing robots for stacking task.
(4)
The future work will be extended to deal with improved solving method of the energy-optimal problem for the conditions of the energy consumption and working speed are optimal simultaneously, which can improve the solution efficiency of the multiobjective optimization and enhance the applicability of the method.



---

*Source: 5862457-2017-02-23.xml*