# Multi-Objective Trajectory Planning with Dual-Encoder

Beibei Zhang1,∗, Tian Xiang1,∗, Chentao Mao2, Yuhua Zheng1,
Shuai Li2, Haoyi Niu1, Xiangming Xi1, Wenyuan Bai1, Feng Gao1,† 1 Beibei
Zhang, Tian Xiang, Yuhua Zheng, Haoyi Niu, Xiangming Xi, Wenyuan Bai, and Feng
Gao are with Zhejiang Lab, Hangzhou, China.2 Chentao Mao and Shuai Li are with
Hangzhou Institute for Advanced Study, University of Chinese Academy of
Sciences, Hangzhou, China.∗ Beibei Zhang and Tian Xiang have equally
contributed to this work.${\dagger}$ Feng Gao is the corresponding author.

###### Abstract

Time-jerk optimal trajectory planning is crucial in advancing robotic arms’
performance in dynamic tasks. Traditional methods rely on solving complex
nonlinear programming problems, bringing significant delays in generating
optimized trajectories. In this paper, we propose a two-stage approach to
accelerate time-jerk optimal trajectory planning. Firstly, we introduce a
dual-encoder based transformer model to establish a good preliminary
trajectory. This trajectory is subsequently refined through sequential
quadratic programming to improve its optimality and robustness. Our approach
outperforms the state-of-the-art by up to 79.72% in reducing trajectory
planning time. Compared with existing methods, our method shrinks the
optimality gap with the objective function value decreasing by up to 29.9%.

##  I Introduction

Driven by the imperative for increased productivity in future smart factories,
we demand robotic arms to efficiently and reliably accomplish a sequence of
dynamic tasks. As an example, aircraft assembly lines employ robotic arms to
place components with varying shapes and sizes, adjusting to the changing
requirements of the assembly process [1]. A robotic arm trajectory, commonly
quoted as a robot configuration as a function of time, is pivotal for a robot
manipulator to fulfill the tasks. To optimize productivity and enhance the
stability of a robot manipulator, we consider time and the time derivative of
acceleration, known as a jerk, as optimization objectives when proposing the
trajectory. Our time objective targets to minimize the overall duration of a
trajectory and the jerk objective prioritizes diminishing the abrupt changes
in trajectory accelerations.

The trajectory can be carried out in the joint space with a series of
successive joint values that each robot joint should assume over time,
referred to as waypoints. Trajectory planning seeks to create interpolation
functions, such as polynomial [2], Bezier [3], and B-spline [4], to accurately
traverse the waypoints as illustrated in Fig. 1. Fundamentally, obtaining a
time-jerk optimal trajectory amounts to solving a nonlinear programming
problem, which is NP-hard [5]. The inherent computational demands of
trajectory planning result in prolonged trajectory generation time, making it
difficult to cope with the dynamic task requirements of future smart
factories. In this paper, we focus on speeding up the time-jerk optimal
trajectory planning process.

Figure 1: We show a 4DOFs robot manipulator. A joint space trajectory composes
waypoints for each joint and an interpolation function passes through the
waypoints.

Current research addresses the time-jerk optimal trajectory planning through
two classes of methods. A first group of work tackles the problem by
optimizing a system of equations that consists of the interpolation function
type and the constraints imposed on it. The problem can be solved through
genetic algorithms (GA) [4, 6, 7] that iteratively improve the approximation
set or the sequential quadratic programming (SQP) algorithm [8] that
decomposes the nonlinear and non-convex problem into a sequential convex
problem. Another line of research resorts to devising supervised learning
models, such as artificial neural networks [9] and long short-term memory
[10], trained on time-jerk optimal trajectory datasets. The supervised
learning models take kinematic configurations of robotic manipulators and
waypoints as inputs to generate a time-jerk optimal trajectory.

Upon investigating and experimenting on the released code of state-of-the-art
methods, we empirically observe that: First, the result optimality and
computation time of the GA or the SQP methods are highly sensitive to how good
the initial estimate is. As an example [11], the SQP-based solution takes an
average 2.6 seconds to generate a trajectory and requires multiple times of
optimizations with different initial estimates to get a time-jerk optimal
trajectory. Second, although it is faster to generate a trajectory with
supervised learning methods, the black-box nature of such approaches leads to
poor robustness when faced with kinematic constraints and joint configurations
not represented in the training data. This lack of robustness renders these
methods unsuitable for future smart factories [1].

To sum up, the optimization-based methods fall short of delivering efficiency.
This gap not only affects the throughput of production lines but also limits
the potential for real-time adjustments, which are essential features in
future smart factories. The supervised learning-based methods show promising
efficiency that caters to the demands of future smart manufacturing. However,
the methods struggle with robustness and fail to achieve optimal results.

In response to these challenges, our paper introduces a two-stage trajectory
planning method. First, with the observation that a good initial estimate of
the trajectory noticeably improves the convergence speed of the SQP
optimization routine, we propose a dual-encoder based transformer model [12]
to predict initial trajectory parameters. Next, based on the initial
parameters, we further optimize the trajectory with SQP to derive a time-jerk
optimal trajectory, thereby enhancing the robustness of the method without
compromising efficiency. Technically, we design a dual-encoder based
transformer model that consists of a source encoder and a context encoder. The
source encoder represents the dynamic characteristics of the current joint.
Meanwhile, the context encoder depicts the motion relationships between the
current joint and other joints.

Thereafter, we conduct extensive experiments on five counterparts including
SQP [8], NSGA-II [4], IPTP [13], TOTG [14], and LSTM [10]. Empirically, our
approach consistently and significantly outperforms state-of-the-art methods.
Interestingly, we observe that, unlike other methods, our approach is less
impacted by trajectory length, measured by the number of waypoints, in terms
of trajectory planning time.

To summarize, our key contributions are as follows: 1) A two-stage solution to
expedite the generation of time-jerk optimal trajectory, where we design a
dual-encoder based transformer model trained on time-jerk optimal trajectories
to predict the initial estimate of the SQP. 2) Our method achieves new state-
of-the-art performance over optimization-based approaches and supervised
learning methods, particularly highlighting the balance between computational
efficiency and robustness in trajectory planning. The implementations are
released, hoping to facilitate future research in robust and efficient multi-
objective trajectory planning.

##  II Problem Statement

Suppose a robotic manipulator with $K$ degrees of freedom (DOFs) is employed
to follow a path consisting of $I$ waypoints. The joint values of all joints
are denoted as $\mathbfcal{Q}=\\{\bm{q}^{k}\\}_{k=1}^{K}$. Further, we
describe the joint values of the $k$-th joint as
$\bm{q}^{k}=\\{q_{1}^{k},\dots,q_{i}^{k},\dots,q_{I}^{k}\\}$, where
$q_{i}^{k}$ represents the joint value of the $k$-th joint at the $i$-th
waypoint. For each $k$-th joint, our objective is to propose a three-times
differentiable function $\bm{q}^{k}(t)$ that depends on time $t\geq 0$ and
interpolates $\\{q_{1}^{k},\dots,q_{i}^{k},\dots,q_{I}^{k}\\}$. Building on
recent research [15], we represent $\bm{q}^{k}(t)$ using a 5th-order B-spline
function $f(h^{k},t)$, where $\bm{q}^{k}(t)=f(h^{k},t)$. The set $h^{k}$
comprises coefficients and knot vectors associated with the B-spline of the
$k$-th joint. Collectively, we denote the simultaneous joint values of $K$
joints at time $t$ as $\bm{q}(t)=\\{\bm{q}^{k}(t)\\}_{k=1}^{K}=f(\bm{h},t)$.

Suppose that the trajectory starts from time $0$ and $T$ is the timestamp of
the last waypoint in the trajectory, where $\bm{q}^{k}(T)=q_{I}^{k}$ for all
$k\in\\{1,2,\dots,K\\}$. Given the joint limit $\bm{q}_{\text{max}}$, maximum
velocity $\bm{\dot{q}}_{\text{max}}$, maximum acceleration
$\bm{\ddot{q}}_{\text{max}}$ of the joints, and maximum jerk
$\bm{\dddot{q}}_{\text{max}}$, $\bm{q}(t)$ is subject to satisfying kinematic
constraints and boundary conditions listed in Eq. 1 and Eq. 2,

$\begin{cases}|\bm{q}(t)|<\bm{q}_{\text{max}},&\\\
|\bm{\dot{q}}(t)|<\bm{\dot{q}}_{\text{max}},&\\\
|\bm{\ddot{q}}(t)|<\bm{\ddot{q}}_{\text{max}},&\\\
|\bm{\dddot{q}}(t)|<\bm{\dddot{q}}_{\text{max}},\end{cases}\forall t\in[0,T],$
(1)

$\bm{\dot{q}}(0)=\bm{\dot{q}}(T)=\bm{\ddot{q}}(0)=\bm{\ddot{q}}(T)=0.$ (2)

The total jerk $\mathbfcal{J}$ of $K$ joints along the trajectory is defined
as:

$\mathbfcal{J}=\sum_{k=1}^{K}\sqrt{\int_{0}^{T}\bm{\dddot{q}}^{k}(t)^{2}\,dt/T}$
(3)

With the above settings, we formulate the time-jerk optimal trajectory
planning problem as

$\displaystyle\min_{\bm{h}}\ \ \ $ $\displaystyle\lambda J+(1-\lambda)T,$ (4)
s.t. $\displaystyle\text{Eq.(1) and Eq. (2)},$ (5)

where $\lambda$ is a balancing parameter. The two objectives are inherently
contradictory due to their opposing effects. Minimizing time tends to increase
jerk fluctuation and disrupt the smoothness of the trajectory. Conversely,
prioritizing jerk reduction can extend the execution time and compromise
overall work efficiency.

To expedite the optimal trajectory planning time, we propose a dual-encoder
based transformer model trained on time-jerk optimal trajectories to predict
the initial trajectory that will be further optimized by SQP. Next, we will
delve into the details of the proposed model.

Figure 2: Dual-Encoder based Transformer architecture.

##  III Proposed Method

Fig. 2 depicts the overall structure of the model that predicts the initial
trajectory. Given the waypoints of $k$-th joint, we take the joint values
$\mathbfcal{Q}_{src}=\bm{q}^{k}=\\{q_{1}^{k},\dots,q_{i}^{k},\dots,q_{I}^{k}\\}$
as the input to the source encoder. Typically, a robotic arm has multiple
degrees of freedom, corresponding to multiple joints. The motion of each joint
is constrained by other joints, confining its movement within a certain range.
To capture the kinematic relations between different joints, we introduce a
context encoder that extracts the kinematic information among joints.
Specifically, we employ a multi-head self-attention encoder to compute the
representation matrix of $\mathbfcal{Q}_{ctx}$, where
$\mathbfcal{Q}_{ctx}=\\{\bm{q}^{1},\dots,\bm{q}^{k-1},\bm{q}^{k+1},\dots,\bm{q}^{K}\\}$,
consisting of the concatenation of the joint values of the other $K-1$ joints.

Suppose the number of joint values in $\mathbfcal{Q}_{src}$ is $I$, then the
number of joint values in $\mathbfcal{Q}_{ctx}$ is $(K-1)\times I$. To adapt
to the structure of the Transformer, we propose an embedding layer with a
dimension of $D=32$ to encode the joint values in $\mathbfcal{Q}_{src}$ and
$\mathbfcal{Q}_{ctx}$. Consequently, we have
$\mathbfcal{Q}_{src}\in\mathbb{R}^{I\times D}$, and
$\mathbfcal{Q}_{ctx}\in\mathbb{R}^{(K-1)I\times D}$. The number of waypoints
in different trajectories varies, causing a variable number of elements in
$\mathbfcal{Q}_{src}$ and $\mathbfcal{Q}_{ctx}$. To address this, we obtain
the maximum number of waypoints in our trajectory dataset, termed as
$I_{max}$. Therefore, the number of elements in $\mathbfcal{Q}_{src}$ and
$\mathbfcal{Q}_{ctx}$ is $L=(K-1)\times I_{max}$ that contains $I$,
$(K-1)\times I$ joint values respectively. The number of padding elements in
$\mathbfcal{Q}_{src}$ and $\mathbfcal{Q}_{ctx}$ are $(K-1)\times I_{max}-I$
and $(K-1)\times(I_{max}-I)$. Next, we incorporate positional encoding to
characterize the relative orders of waypoints along the trajectory. We refer
to the output of $\mathbfcal{Q}_{src}$ and $\mathbfcal{Q}_{ctx}$ after
applying positional encoding as $\mathbfcal{Q}_{src\\_emb}$ and
$\mathbfcal{Q}_{ctx\\_emb}$ respectively. We have
$\mathbfcal{Q}_{src\\_emb}\in\mathbb{R}^{L\times D}$ and
$\mathbfcal{Q}_{ctx\\_emb}\in\mathbb{R}^{L\times D}$.

###  III-A Context Encoder

The context encoder comprises a stack of $N_{c}$ identical context encoding
layers, each of which has two sublayers. The first sublayer operates as a
multi-head self-attention mechanism (MultiHead), which allows the direct
modeling of dependencies between every two joints. The operation of the first
sublayer is given by

$\mathbfcal{Z}_{c}^{1}=\text{MultiHead}(\bm{Q}_{c},\bm{K}_{c},\bm{V}_{c}),$
(6)

where $\bm{Q}_{c}=\bm{K}_{c}=\bm{V}_{c}=\mathbfcal{Q}_{ctx\\_emb}$. That is

$\mathbfcal{Z}_{c}^{1}=\text{MultiHead}(\mathbfcal{Q}_{ctx\\_emb},\mathbfcal{Q}_{ctx\\_emb},\mathbfcal{Q}_{ctx\\_emb}).$
(7)

The multi-head self-attention layer incorporates a residual connection and a
layer normalization (LayerNorm) to enhance model training stability [12]. We
denote the output of the first sublayer as

$\mathbfcal{O}_{c}^{1}=\text{LayerNorm}(\mathbfcal{Z}_{c}^{1}+\mathbfcal{Q}_{ctx\\_emb}).$
(8)

We design the second sublayer as a position-wise feed-forward network (FFN),
which takes the output of the multi-head attention sublayer as input and
undergoes a nonlinear transformation through a two-layer fully connected
network with a ReLU activation function. We describe the procedure as

$\mathbfcal{F}_{c}^{1}=\text{FFN}(\mathbfcal{O}_{c}^{1})=\text{ReLU}(\mathbfcal{O}_{c}^{1}\mathbf{W}_{c1}^{1}+\mathbf{b}_{c1}^{1})\mathbf{W}_{c2}^{1}+\mathbf{b}_{c2}^{1},$
(9)

where $\mathbfcal{O}_{c}^{1}\in\mathbb{R}^{L\times{D}}$ is the output of the
first sublayer, $\mathbf{W}_{c1}^{1},\mathbf{b}_{c1}^{1}\text{ and
}\mathbf{W}_{c2}^{1},\mathbf{b}_{c2}^{1}$ represent the the first and the
second layer parameters of the FFN. Likewise, we append a residual connection
and a layer normalization to the second sublayer producing the output of the
first context encoding layer, referred to as

$\mathbfcal{C}^{1}=\text{LayerNorm}(\mathbfcal{F}_{c}^{1}+\mathbfcal{O}_{c}^{1}).$
(10)

We repeat the above process $N_{c}$ times deriving

$\displaystyle\mathbfcal{Z}_{c}^{N_{c}}$
$\displaystyle=\text{MultiHead}(\mathbfcal{C}^{N_{c}-1},\mathbfcal{C}^{N_{c}-1},\mathbfcal{C}^{N_{c}-1}),$
(11) $\displaystyle\mathbfcal{O}_{c}^{N_{c}}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{Z}_{c}^{N_{c}}+\mathbfcal{C}^{N_{c}-1}),$
(12) $\displaystyle\mathbfcal{F}_{c}^{N_{c}}$
$\displaystyle=\text{FFN}(\mathbfcal{O}_{c}^{N_{c}})$
$\displaystyle=\text{ReLU}(\mathbfcal{O}_{c}^{N_{c}}\mathbf{W}_{c1}^{N_{c}}+\mathbf{b}_{c1}^{N_{c}})\mathbf{W}_{c2}^{N_{c}}+\mathbf{b}_{c2}^{N_{c}},$
(13) $\displaystyle\mathbfcal{C}^{N_{c}}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{F}_{c}^{N_{c}}+\mathbfcal{O}_{c}^{N_{c}}),$
(14)

where $\mathbfcal{Z}_{c}^{N_{c}}\in\mathbb{R}^{L\times{D}}$ and
$\mathbfcal{C}^{N_{c}}\in\mathbb{R}^{L\times{D}}$, which correspond to the
hidden state and output of the $N_{c}$-th layer, respectively.

###  III-B Source Encoder and Context Integration

Up to this point, we have presented the design of context encoder that
extracts cues from $\mathbfcal{Q}_{ctx\\_emb}$. Next, we introduce our source
encoder with $N_{s}$ identical source encoding layers to encode
$\mathbfcal{Q}_{src\\_emb}$. Each source encoding layer comprises three
consecutive sublayers: a multi-head self-attention layer, a context attention
layer, and a feed-forward layer. Technically, we propose a multi-head self-
attention layer to encapsulate the relative motion change among waypoints in a
trajectory. Formally, we apply the multi-head self-attention to the source
embedding $\mathbfcal{Q}_{src\\_emb}$:

$\displaystyle\mathbfcal{Z}_{s}^{1}$
$\displaystyle=\text{MultiHead}(\mathbfcal{Q}_{src\\_emb},\mathbfcal{Q}_{src\\_emb},\mathbfcal{Q}_{src\\_emb}),$
(15) $\displaystyle\mathbfcal{O}_{s1}^{1}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{Z}_{s}^{1}+\mathbfcal{Q}_{src\\_emb}),$
(16)

where $\mathbfcal{O}_{s1}^{1}$ is the output of the multi-head self-attention
layer.

To capture the kinematic relations between the current joint and other joints,
we integrate the output of the context encoder into the source encoder with a
context attention layer:

$\displaystyle\mathbfcal{G}_{s}^{1}$
$\displaystyle=\text{MultiHead}(\mathbfcal{O}_{s1}^{1},\mathbfcal{C}^{N_{c}},\mathbfcal{C}_{c}^{N_{c}}),$
(17) $\displaystyle\mathbfcal{O}_{s2}^{1}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{G}_{s}^{1}+\mathbfcal{O}_{s1}^{1}).$
(18)

Then, the position-wise feed-forward network (FFN) is applied, which can be
expressed as:

$\displaystyle\mathbfcal{F}_{s}^{1}$
$\displaystyle=\text{FFN}(\mathbfcal{O}_{s2}^{1})$
$\displaystyle=\text{ReLU}(\mathbfcal{O}_{s2}^{1}\mathbf{W}_{s1}^{1}+\mathbf{b}_{s1}^{1})\mathbf{W}_{s2}^{1}+\mathbf{b}_{s2}^{1},$
(19) $\displaystyle\mathbfcal{S}^{1}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{F}_{s}^{1}+\mathbfcal{O}_{s2}^{1})$
(20)

We iterate the same process $N_{s}$ times:

$\displaystyle\mathbfcal{Z}_{s}^{N_{s}}$
$\displaystyle=\text{MultiHead}(\mathbfcal{S}^{N_{s}-1},\mathbfcal{S}^{N_{s}-1},\mathbfcal{S}^{N_{s}-1}),$
(21) $\displaystyle\mathbfcal{O}_{s1}^{N_{s}}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{Z}_{s}^{N_{s}}+\mathbfcal{S}^{N_{s}-1}),$
(22) $\displaystyle\mathbfcal{G}_{s}^{N_{s}}$
$\displaystyle=\text{MultiHead}(\mathbfcal{O}_{s1}^{N_{s}},\mathbfcal{C}^{N_{c}},\mathbfcal{C}_{c}^{N_{c}}),$
(23) $\displaystyle\mathbfcal{O}_{s2}^{N_{s}}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{G}_{s}^{N_{s}}+\mathbfcal{O}_{s1}^{N_{s}}),$
(24) $\displaystyle\mathbfcal{F}_{s}^{N_{s}}$
$\displaystyle=\text{FFN}(\mathbfcal{O}_{s2}^{N_{s}})$
$\displaystyle=\text{ReLU}(\mathbfcal{O}_{s2}^{N_{s}}\mathbf{W}_{s1}^{N_{s}}+\mathbf{b}_{s1}^{N_{s}})\mathbf{W}_{s2}^{N_{s}}+\mathbf{b}_{s2}^{N_{s}},$
(25) $\displaystyle\mathbfcal{S}^{N_{s}}$
$\displaystyle=\text{LayerNorm}(\mathbfcal{F}_{s}^{N_{s}}+\mathbfcal{O}_{s2}^{N_{s}})$
(26)

where $\mathbfcal{S}^{N_{s}}\in\mathbb{R}^{L\times{D}}$ is the representation
of source joint $\mathbf{q}^{k}$ at the ${N_{s}}$-th layer.

To predict the coefficients and knot vector of a B-spline curve that fits a
robotic arm trajectory, we link the source encoder’s output to two distinct
two-layer fully connected networks. This process yields separate outputs for
each branch, enabling us to derive the predictions for the coefficients and
knot vector as outlined below:

$\displaystyle\mathbfcal{R}_{coef}=\text{ReLU}(\mathbfcal{S}^{N_{s}}\mathbf{W}_{coef1}+\mathbf{b}_{coef1})\mathbf{W}_{coef2}+\mathbf{b}_{coef2},$
(27)
$\displaystyle\mathbfcal{R}_{knot}=\text{ReLU}(\mathbfcal{S}^{N_{s}}\mathbf{W}_{knot1}+\mathbf{b}_{knot1})\mathbf{W}_{knot2}+\mathbf{b}_{knot2},$
(28)

where $\mathbfcal{R}_{coef}\in\mathbb{R}^{1\times{M}}$,
$\mathbfcal{R}_{knot}\in\mathbb{R}^{1\times{N}}$, $M$ denotes the number of
coefficients and $N$ denotes the number of knot vectors.

###  III-C Training

Our training data consists of the coefficients and knot vector of the
B-splines that fit the time-jerk optimal trajectory. Our training objective is
to minimize the error between the model output and the ground truth in the
training dataset. Given that the output of the dual-encoder bifurcates into
two branches, one predicting the coefficients of the B-spline and the other
predicting the knot vector of the B-spline, we design two different loss
functions to cater to two branches. The composite loss function can be
described as:

$L=\theta_{1}\cdot L_{\text{coef}}+\theta_{2}\cdot L_{\text{knot}}$

where $L_{\text{coef}}$ is the L1 smooth loss for the B-spline coefficients,
$L_{\text{knot}}$ is the L1 loss for the knot vector, $\theta_{1}$ and
$\theta_{2}$ are weighting parameters that balance the loss components.

For the branch that predicts the B-spline coefficients, we employ the L1
smooth loss function, which can be defined as:

$L_{\text{coef}}(\mathbf{y},\mathbf{\hat{y}})=\frac{1}{N}\sum_{i=1}^{N}\text{smooth}_{L1}(y_{i}-\hat{y}_{i})\
$

where $\mathbf{y}$ represents the ground truth coefficient vector,
$\mathbf{\hat{y}}$ represents the predicted coefficient vector, $N$ is the
number of coefficients, and $\text{smooth}_{L1}$ is the smooth L1 loss
function applied element-wise, referred to as:

$\text{smooth}_{L1}(x)=\begin{cases}0.5\cdot x^{2}&\text{if }|x|<1\\\
|x|-0.5&\text{otherwise}\end{cases}$

This choice is motivated by the need for a loss function that not only
facilitates robustness but also smooths out predictions to avoid abrupt
changes, which are critical for accurately modeling the trajectory’s
curvature.

For the branch predicting the knot vector, we utilize the L1 loss function,
which can be defined as:

$L_{\text{knot}}(\bm{\eta},\bm{\hat{\eta}})=\frac{1}{M}\sum_{i=1}^{M}|\eta_{i}-\hat{\eta}_{i}|$

where $\bm{\eta}$ represents the ground truth knot vector, $\bm{\hat{\eta}}$
represents the predicted knot vector, and $M$ is the number of knots in the
vector.

The adoption of the L1 loss for the knot vector prediction addresses the
sparse changes in knot vectors, focusing on precise knot placement vital for
trajectory fidelity.

We also employ a learning rate decay strategy to fine-tune model weights more
precisely in later training stages.

(a) Worst-case scenario.

(b) Best-case scenario.

Figure 3: Comparison of trajectories generated by our dual-encoder model
(dashed) to the ground truth generated by 5th-order BSpline (solid).

(a) path length=6

(b) path length=12

(c) path length=24

(d) path length=48

Figure 4: Convergence trends of SQP, our method, IPTP, TOTG, NSGA-II, LSTM
with various numbers of waypoints over time.

Figure 5: Time-jerk optimal trajectory data collection with MoveIt2.

Figure 6: Comparison of training loss between our method and source encoder-
only model.

(a) path length=6

(b) path length=12

(c) path length=24

(d) path length=48

Figure 7: Time variations (seconds) comparison among SQP, Transformer+SQP, and
Transformer across different path lengths: a boxplot analysis.

##  IV Experiment

###  IV-A Setup

Datasets. We leverage a Kinova Gen3 6DOFs robotic arm to generate time-jerk
optimal trajectories in Moveit2 as depicted in Fig. 6. The dataset consists of
trajectories with lengths ranging from 6 to 48 waypoints. These waypoints are
uniformly distributed in the Cartesian space that falls within the
manipulator’s operational range, with inverse kinematics employed to derive
the corresponding joint values. We collect a total of 80532 trajectories,
where 56372 trajectories are used for training, 16106 trajectories are used
for validation, and 7784 trajectories are used for testing. Every trajectory
data contains six data items, each of which contains the joint values of a
Kinova Gen3 joint at the waypoints and the joint values of the other Kinova
Gen3 joints at the waypoints. We feed the joint values of the current joint to
the source encoder and the joint values of the other joints to the context
encoder. We train and serve the dual-encoder based transformer model on an
NVIDIA TITAN RTX GPU.

Training Configs. The dual-encoder model features a 32-dimensional embedding
for joint values, 8 attention heads, and 6 stacked layers to enhance learning
capabilities. It incorporates a dropout rate of 0.1 to prevent over-fitting.
We employ an Adam optimizer [16] with the learning rate set to 0.001 and
incorporate a weight decay of 1e-4 for regularization. An adaptive learning
rate scheduler, $\mathtt{ReduceLROnPlateau}$, is used to adjust the learning
rate based on performance.

We showcase the performance of our proposed model with a worst-case scenario
and a best-case scenario from our test data. As illustrated in Fig. 3, we plot
the ground truth trajectory B-splines and the B-splines generated by our
model. In the worst-case scenario, the generated B-spline exhibits a joint
value error margin of less than 23.47% in comparison to the ground truth. In
the best-case scenario, the generated B-spline exhibits a joint value error
margin of less than 5.26% in comparison to the ground truth.

Counterparts. We benchmark our approach against several established
algorithms, including SQP [8], NSGA-II [4], Iterative Parabolic Time
Parametrization (IPTP) [13], Time-Optimal Trajectory Generation (TOTG) [14],
and LSTM [10]. NSGA-II starts with a preliminary feasible set, iteratively
enhancing it through a process that involves sorting solutions based on non-
domination, selection, crossover, and mutation operations to generate new
solutions while ensuring compliance with velocity and acceleration
constraints. IPTP adjusts time intervals based on the most constraining
velocity across joints, incrementally modifying them until acceleration
constraints are met. TOTG smoothens paths with circular blends, establishes a
maximum velocity profile in the phase plane, and accomplishes time
parameterization through numerical integration. LSTM is utilized for
trajectory planning by first generating paths with the Rapidly Exploring
Random Tree (RRT) method, then refining these paths using LSTM networks
trained on environmental data. We take the output of LSTM as an initial value
for SQP optimization in our experiment. The next subsection highlights the
efficiency of our proposed method in trajectory planning and its superiority
in reducing the objective function values.

###  IV-B Results

In our experiments, we conduct an ablation study by removing the context
encoder, leaving only the source encoder in the transformer model. As depicted
in Fig. 6, our proposed method has achieved a loss reduction to 0.01 after
just 5 epochs of training and further converged to below 0.007 within 15
epochs. In contrast, the loss of the source encoder only model plateaued at
0.035 after 5 epochs, demonstrating inferior performance in comparison to the
dual-encoder model. This demonstrates the effectiveness of the context
encoder.

Fig. 4 illustrates the comparison of time consumed by various optimization
algorithms in solving the time-jerk optimal trajectory planning problem, with
path lengths of 6, 12, 24, and 48. All methods commence optimization from
identical initial values. However, both Transformer+SQP and LSTM+SQP
incorporate a preliminary inference step to refine the initial values before
further optimization through SQP. In contrast, the other methods proceed the
optimization process directly on the original initial values. This analysis
selects a set of results that balance time efficiency and trajectory
optimality. It can be observed that, after obtaining the initial estimate
through our well-trained transformer model and then carrying out subsequent
optimizations, the optimal trajectory planning time improves up to 77.01%,
79.72%, 80.47%, 47.32%, and 52.13% in comparison to SQP, IPTP, TOTG, NSGA-II,
and LSTM+SQP respectively. In terms of results optimality, the proposed
methods outperform SQP, IPTP, TOTG, NSGA-II, and LSTM+SQP by up to 8.43%,
16.71%, 22.2%, 29.9%, and 29.7% respectively. From the above analysis, we
empirically observed that LSTM+SQP performance is greatly affected by the path
length, whereas our proposed model keeps its performance across varying path
lengths.

Furthermore, we employ a boxplot analysis to compare the time variations
across different path lengths for SQP, Transformer+SQP, and Transformer, as
shown in Fig. 7. Due to SQP’s high dependency on initial values, it exhibits
the greatest fluctuation in processing time, with a difference of up to 29.64
seconds between the maximum and minimum values at the path length of 48. In
contrast, the Transformer+SQP approach, benefiting from accurate initial
values inferred by our dual-encoder based Transformer, demonstrates a more
stable time variation as path length increases. Notably, the Transformer model
shows the shortest inference time, appearing almost as a constant line in the
figure. This indicates the efficiency of our proposed method to predict
initial values for SQP optimization and highlights its ability to maintain
performance consistency across different trajectory lengths.

##  V Conclusion

In this article, we have proposed a two-stage approach to address the
challenges associated with time-jerk optimal trajectory planning for robotic
arms. Initially, a dual-encoder based transformer model, trained on datasets
of time-jerk optimized trajectories, is utilized to quickly generate initial
values. These initial values serve as a foundation for further refinement
through sequential quadratic programming optimization, leading to enhanced
trajectory planning. Our extensive experimental evaluation validates the
effectiveness of this method, demonstrating its potential to improve the
efficiency and performance of multi-objective trajectory planning.

## References

  * [1] E. Masehian and S. Ghandi, “Assembly sequence and path planning for monotone and nonmonotone assemblies with rigid and flexible parts,” _Robotics and Computer-Integrated Manufacturing_ , vol. 72, p. 102180, 2021.
  * [2] M. Švejda and T. Čechura, “Interpolation method for robot trajectory planning,” in _2015 20th International Conference on Process Control (PC)_. IEEE, 2015, pp. 406–411.
  * [3] S. G. Manyam, D. W. Casbeer, I. E. Weintraub, and C. Taylor, “Trajectory optimization for rendezvous planning using quadratic bézier curves,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2021, pp. 1405–1412.
  * [4] J. Huang, P. Hu, K. Wu, and M. Zeng, “Optimal time-jerk trajectory planning for industrial robots,” _Mechanism and Machine Theory_ , vol. 121, pp. 530–544, 2018.
  * [5] A. Gasparetto and V. Zanotto, “A technique for time-jerk optimal planning of robot trajectories,” _Robotics and Computer-Integrated Manufacturing_ , vol. 24, no. 3, pp. 415–426, 2008.
  * [6] F. Wang, Z. Wu, and T. Bao, “Time-jerk optimal trajectory planning of industrial robots based on a hybrid woa-ga algorithm,” _Processes_ , vol. 10, no. 5, p. 1014, 2022.
  * [7] J. Lin, N. Somani, B. Hu, M. Rickert, and A. Knoll, “An efficient and time-optimal trajectory generation approach for waypoints under kinematic constraints and error bounds,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 5869–5876.
  * [8] Y. Zhang, Z. Sun, Q. Sun, Y. Wang, X. Li, and J. Yang, “Time-jerk optimal trajectory planning of hydraulic robotic excavator,” _Advances in Mechanical Engineering_ , vol. 13, no. 7, p. 16878140211034611, 2021.
  * [9] S. Yin, W. Ji, and L. Wang, “A machine learning based energy efficient trajectory planning approach for industrial robots,” _Procedia CIRP_ , vol. 81, pp. 429–434, 2019.
  * [10] A. Molina-Leal, A. Gómez-Espinosa, J. A. Escobedo Cabello, E. Cuan-Urquizo, and S. R. Cruz-Ramírez, “Trajectory planning for a mobile robot in a dynamic environment using an lstm neural network,” _Applied Sciences_ , vol. 11, no. 22, p. 10689, 2021.
  * [11] I. Gukov and A. Logins, “Real-time multi-objective trajectory optimization,” in _2022 Sixth IEEE International Conference on Robotic Computing (IRC)_. IEEE, 2022, pp. 391–394.
  * [12] A. Vaswani, N. Shazeer, N. Parmar, J. Uszkoreit, L. Jones, A. N. Gomez, Ł. Kaiser, and I. Polosukhin, “Attention is all you need,” _Advances in neural information processing systems_ , vol. 30, 2017.
  * [13] Q.-C. Pham, “A general, fast, and robust implementation of the time-optimal path parameterization algorithm,” _IEEE Transactions on Robotics_ , vol. 30, no. 6, pp. 1533–1540, 2014.
  * [14] T. Kunz and M. Stilman, “Time-optimal trajectory generation for path following with bounded acceleration and velocity,” in _Robotics: Science and Systems VIII, University of Sydney, Sydney, NSW, Australia, July 9-13, 2012_ , 2012\.
  * [15] A. Artuñedo, J. Villagra, and J. Godoy, “Jerk-limited time-optimal speed planning for arbitrary paths,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 7, pp. 8194–8208, 2021.
  * [16] D. P. Kingma and J. Ba, “Adam: A method for stochastic optimization,” _arXiv preprint arXiv:1412.6980_ , 2014.