Regeneration and Joining of the Learned Motion Primitives for Automated Vehicle Motion Planning Applications

TL;DR

This paper presents a novel method for regenerating and smoothly joining learned motion primitives in vehicle motion planning using modified dynamic movement primitives and SVD, enhancing adaptability and transition quality.

Contribution

It introduces a new representation algorithm that separates shape parameters, enabling accurate, smooth MP joining and reducing shape adjustment complexity without losing representation accuracy.

Findings

Effective reduction in shape adjustment parameters.

Improved smoothness in velocity transitions.

Maintains high accuracy in target point tracking.

Abstract

How to integrate human factors into the motion planning system is of great significance for improving the acceptance of intelligent vehicles. Decomposing motion into primitives and then accurately and smoothly joining the motion primitives (MPs) is an essential issue in the motion planning system. Therefore, the purpose of this paper is to regenerate and join the learned MPs in the library. By applying a representation algorithm based on the modified dynamic movement primitives (DMPs) and singular value decomposition (SVD), our method separates the basic shape parameters and fine-tuning shape parameters from the same type of demonstration trajectories in the MP library. Moreover, we convert the MP joining problem into a re-representation problem and use the characteristics of the proposed representation algorithm to achieve an accurate and smooth transition. This paper demonstrates that the proposed method can effectively reduce the number of shape adjustment parameters when the MPs are regenerated without affecting the accuracy of the representation. Besides, we also present the ability of the proposed method to smooth the velocity jump when the MPs are connected and evaluate its effect on the accuracy of tracking the set target points. The results show that the proposed method can not only improve the adjustment ability of a single MP in response to different motion planning requirements but also meet the basic requirements of MP joining in the generation of MP sequences.

Tables

Table 1. TABLE I: This table lists the average deviations in position and velocity when the number of fine-tuning parameters 𝒔 𝒔 \boldsymbol{s} changed. Type 1 refers to the sharp turn, and type 2 refers to the lane changing.

Type	$J$	$\Delta \overline{d}$(m)	$\Delta \overline{v}$(m/s)
1	1	2.12	0.43
1	3	2.09	0.41
1	5	1.49	0.29
2	1	1.78	0.41
2	3	1.72	0.32
2	5	1.46	0.25

Table 2. TABLE II: This table lists the maximum acceleration a m ​ a ​ x subscript 𝑎 𝑚 𝑎 𝑥 a_{max} and position deviations Δ ​ d 1 Δ subscript 𝑑 1 \Delta d_{1} , Δ ​ d 2 Δ subscript 𝑑 2 \Delta d_{2} during the joining process. Δ ​ d 1 Δ subscript 𝑑 1 \Delta d_{1} corresponds to the first target point, Δ ​ d 2 Δ subscript 𝑑 2 \Delta d_{2} corresponds to the second target point. The types listed in the table are consistent with the labels in Fig. 4 .

Type	$\Delta d_1$(m)	$\Delta d_2$(m)	$a_{max}$($m/s^2$)
Simple-1	0	0	16.65
Proposed-1	0.02	0.21	0.98
Simple-2	0	0	7.52
Proposed-2	0.32	0.07	0.34

Equations

$${\dot{z}}_m=-\tau {\alpha }_z{z}_m$$

$${\dot{\mathbf{v}}}_m=\tau {\alpha }_m({\beta }_m(\mathbf{g}-{\mathbf{d}}_m)-{\mathbf{v}}_m)+\tau (\mathbf{g}-\mathbf{b})f(z_m)$$

$${\dot{\mathbf{d}}}_m=\tau {\mathbf{v}}_m$$

$$f(z_m)=\frac{{\sum }_{n=1}^N{\omega }_n{\psi }_n(z_m)z_m}{{\sum }_{n=1}^N{\psi }_n(z_m)}$$

$${\dot{z}}_m=-\frac{{\alpha }_z{e}^{{\alpha }_z\cdot (\tau T-t)/\Delta t}}{{(1+e^{{\alpha }_z\cdot (\tau T-t)/\Delta t})}^2}$$

$${\dot{\mathbf{v}}}_m=\tau {\alpha }_m({\beta }_m({\mathbf{r}}_m-{\mathbf{d}}_m)-{\mathbf{v}}_m)+f(t,z_m)$$

$${\dot{\mathbf{d}}}_m=\tau {\mathbf{v}}_m$$

$$\displaystyle\tau\mathop{\boldsymbol{r}_{m}}\limits^{\cdot}=\left\{\begin{array}[]{l}\frac{{\Delta t}}{T}(\boldsymbol{g}_{m}-\boldsymbol{b}_{m}),{\rm{if}}{\ }t\leq T\\ 0,{\ }{\rm{else}}\end{array}\right.$$

$$f(t,z_m)={\alpha }_w\left(\sum _{j=1}^J\frac{{\sum }_{i=1}^N{\omega }_{ji}{\psi }_{ji}(t)s_j}{{\sum }_{i=1}^N{\psi }_{ji}(t)}\right)z_m$$

$$f_{dem}^m(t,z_m)={\ddot{x}}_q(t)/{\tau }^2-{\alpha }_y({\beta }_y(r-x_q(t))-{\dot{x}}_q(t))$$

$${\mathbf{F}}_{dem}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{\mathrm{T}}\approx \mathbf{s}{\mathbf{D}}_{basis}$$

$${\omega }_j^{\ast }\leftarrow \arg \underset{{\omega }_j}{\min }\sum _{c=1}^C\left(D_{basis}^j(t_c)-\frac{{\sum }_{i=1}^N{\omega }_{ji}{\psi }_{ji}(t_c)}{{\sum }_{i=1}^N{\psi }_{ji}(t_c)}{z}_m\right)$$

$$\displaystyle\mu_{k}^{i}=\left\{\begin{array}[]{l}\frac{{{T_{1}}\cdot(i-1)}}{{T^{\prime}\cdot(N-1)}},{\rm{if{\ }k=1}}\\ \frac{{{T_{k}}\cdot(i-1)}}{{T^{\prime}\cdot(N-1)}}+\frac{1}{{T^{\prime}}}\sum\limits_{l=1}^{k-1}{{T_{l}},{\rm{{\ }else}}}\end{array}\right.$$

$${\mathbf{\mu }}^{\prime }={\mu }_1^1,...,{\mu }_1^N;...;{\mu }_K^1,...,{\mu }_K^N$$

$${\bar{p}}_k^i=\frac{p_k^i\cdot T^{\prime }}{T_k}$$

$${\mathbf{p}}^{\prime }={\bar{p}}_1^1,...,{\bar{p}}_1^N;...;{\bar{p}}_K^1,...,{\bar{p}}_K^N$$

$${\mathbf{\omega }}^{\prime }={\omega }_1^1,...,{\omega }_1^N;...;{\omega }_K^1,...,{\omega }_K^N$$

$$\displaystyle\tau\dot{r}^{\prime}=\left\{\begin{array}[]{l}\frac{{\Delta{t_{k}}}}{{{T_{k}}}}({g_{k}}-{b_{k}}),{\rm{if{\ }}}\sum\limits_{l=1}^{k-1}{{T_{l}}\leq t\leq\sum\limits_{l=1}^{k}{{T_{l}}}}\\ 0,{\rm{{\ }otherwise}}\end{array}\right.$$

$${\dot{z}}^{\prime }=-\frac{{\alpha }_z{e}^{{\alpha }_z\cdot (\tau T^{\prime }-t)/\Delta t_k}}{{(1+e^{{\alpha }_z\cdot (\tau T^{\prime }-t)/\Delta t_k})}^2}$$

$${{\dot{v}}^{\prime }}_x=\tau {\alpha }_m({\beta }_m({r^{\prime }}_x-{d^{\prime }}_x)-{v^{\prime }}_x)+F_{tran\_x}$$

$$F_{tran\_x}=\cos {\delta }_j\cdot f_x(t,z)-sin{\delta }_j{f}_y(t,z)$$

$$\dot{x}=\tau v_x$$

$${\dot{v}}_y=\tau {\alpha }_m({\beta }_m(r_y-d_y)-v_y)+F_{tran\_y}$$

$$F_{tran\_y}=\sin {\delta }_j\cdot f_x(t,z)+\cos {\delta }_j{f}_y(t,z)$$

$$\dot{y}=\tau v_y$$

Full text

I Introduction

The integration of the human factors extracted from the collected driving data into the existing automated driving system has a significant contribution to improving the performance of the automated vehicles[1, 2, 3, 4, 5]. The motion planning system can generate appropriate trajectories according to the driving situations and is an important part of the automated driving system[6, 7]. One of the effective methods is to decompose the motion into primitives, and transform the motion planning problem into MP generation, selection and joining problems[8, 9, 10]. In order to further improve the practicability of the learned MP library for motion planning tasks, this paper mainly solves the following two fundamental issues: one is how to expand the adjustment ability further when the single MP is re-generated, and the other one is how to solve the smooth transition problem when MPs are connected.

In recent years, many methods have been applied to solve the problem of motion planning for automated vehicle in various driving situations. These methods mainly include the graph search-based planners, sampling-based planners, interpolating curve planners, and discrete optimization approaches[6]. Among these methods, the discrete optimization method uses a limited trajectory set to adapt to the changing driving environment and improves the efficiency of the algorithm by reducing the solution space. Therefore, it has been widely used in automated driving systems in recent years. Werling et al.[8] generated a trajectory set with several lateral offsets from a fixed-baseline based on quintic polynomials and then ensures a smooth transition between the trajectory segments through cost function. The proposed trajectory generation method was applied to achieve static obstacles avoidance in[9] and moving obstacles avoidance in[10]. However, the candidate trajectory set generated by the algorithm mentioned above is not based on the real driving data and does not consider the human factors in the motion planning system.

There are many ways to represent the human manipulation skills in the field of robotics, including spline-based via-point models, Hidden Markov Models (HMMs), Gaussian Mixture Models (GMMs) and DMPs[11]. Among these methods, DMP has attracted many researchers with its strong robustness and adaptability[12]. The application of DMP for human behavior representation includes biped walking[13], ball hitting[11], reaching a position[14], bimanual manipulation of clothes[15], etc. The collection of human demonstration data is often done by a motion capture device or by recording the joint angle feedback values of the guided robot arm. After learning one or several task-related MPs through the demonstration data, the motion planning system selects and regenerates the MPs according to the requirements. The above human skills collection experiments are often limited to specific repetitive scenes, but the driving data collection experiments are often carried out in a more general traffic environment. How to deal with the relative position data obtained from the general driving experiment, rather than the absolute joint angle data obtained from a specific scene, is a major challenge in this paper.

In the analysis and utilization of the collected vehicle trajectory, Zhao et al.[16] developed an on-road vehicle trajectory collection system to collect the driving data. A trajectory library for representing the driver’s lane change behavior was established by analyzing the collected driving data[17, 18]. The library was then used to match the set of candidate trajectories generated by the motion planning system to achieve generation of human-like lane-changing trajectories[19]. Schnelle et al.[20] proposed a trajectory generation method which can fit the road geometry and achieve the personalized driving, and validated the algorithm through lane change and double lane change scenarios. The authors further extended the method to the field of lane exchanging, avoiding collisions and achieving the integration of driving habits[21]. Although the methods mentioned above can implement human-like trajectory planning, they are specific to certain scenarios and therefore lack generality.

In this paper, we propose a modified trajectory representation method which can enhance the adjustable ability of a single MP and achieve the joining between multiple MPs in the library. In the authors’ early work[22, 23], the driving database under general traffic scene has been established, and different types of MPs have been extracted and identified to form the MP library (Fig.1a). Based on the previous work, this paper uses SVD to realize the further processing of multiple demonstrated trajectories in the same type, which not only extracts the main shape components but also reduces the dimension of the shape adjustment parameter set during regeneration. In addition, we implement the nonlinear function, goal function and canonical system association of each independent MP in the MP sequence through the modified representation method. Through the above association, we have completed a smooth connection between the MPs in the sequence (Fig.1b).

The main contributions of this paper are shown as follows:

• •

Providing a decomposition method for the shape representation parameters of the same type of MPs, which simplifies and constrains the shape adjustment during regeneration.

• •

Solving the problem of MPs joining by utilizing the modified representation method to achieve the association between independent MPs in the sequence.

The remains of the paper are organized as follows. Section II details the issues to be solved in this article. Section III presents the modified representation method and the joining method which are used to solve the problem. Section IV shows the results of MP regeneration and joining. Finally, the conclusions are given in Section V.

II Problem Statement

According to a learned MP library $\boldsymbol{M}=\{\boldsymbol{m}_{1},\ldots,\boldsymbol{m}_{M}\}$, the goal of this paper is to further separate the shape representation parameters of the same type of MPs, and generate the MP sequence $\boldsymbol{R}=\{\boldsymbol{r}_{1},\ldots,\boldsymbol{r}_{I}\}$ based on the initial condition $\boldsymbol{I}=\{\boldsymbol{I}_{1},\ldots,\boldsymbol{I}_{K}\}$.

The main parameters used in this paper are defined as follows:

• •

$\boldsymbol{m}_{m}(t)={[{x_{m}}(t),{y_{m}}(t)]^{\rm{T}}}\in{\mathbb{R}^{2\times 1}}$ is the definition of one MP point at time $t$, where $x_{m}$ and $y_{m}$ are the longitudinal and lateral position value in the $x_{m}oy_{m}$ coordinate. The initial position is the coordinate origin, and the initial heading direction is consistent with the positive direction of the $x_{m}$-axis.

• •

$\boldsymbol{\omega}_{m}={[\boldsymbol{\omega}_{x},\boldsymbol{\omega}_{y}]^{\rm{T}}}$ is the inherent attribute set of the selected MP which remains constant during the adjustment of the MP sequence generation, where $\boldsymbol{\omega}_{x}$ and $\boldsymbol{\omega}_{y}$ are the basic shape characterization parameters of the longitudinal and lateral trajectory shapes of the chosen MP type.

• •

$\boldsymbol{\gamma}_{m}={[\boldsymbol{b},\boldsymbol{g},T,\boldsymbol{s}]^{\rm{T}}}$ is the adjustment parameter set of the selected MP which can be changed during the generation process to satisfy different motion planning task requirements, where $\boldsymbol{b}$ is the initial position, $\boldsymbol{g}$ is the goal position $T$ is the time duration and $\boldsymbol{s}$ is the fine tuning parameter set for the trajectory shape which allows us to make further adjustments based on the basic shape of the selected type.

• •

$\boldsymbol{I}_{k}={[id,T,{x_{init}},{y_{init}},{x_{g}},{y_{g}}]^{\rm{T}}}\in{\mathbb{R}^{6\times 1}}$ defines the initial condition for the generation of MP sequence, where $id$ is the identification information of the ${k^{{\rm{th}}}}$ MP in the sequence, $T$ is the corresponding time duration of the selected MP, $x_{init}$ and $y_{init}$ are the initial position, $x_{g}$ and $y_{g}$ are the goal position of the ${k^{{\rm{th}}}}$ MP.

• •

$\boldsymbol{r}_{i}(t)={[x(t),y(t)]^{\rm{T}}}\in{\mathbb{R}^{2\times 1}}$ is the definition of the generated MP sequence point at time $t$, $x(t)$ and $y(t)$ are the generated position value in the $xoy$ coordinate. The $xoy$ coordinate system is consistent with the $x_{m}oy_{m}$ coordinate system of the first MP in the sequence.

The proposed method in this paper is trying to solve the following two challenges: First, how to extract the basic shape parameter set $\boldsymbol{\omega}_{m}$ and the fine-tuning shape parameter set $\boldsymbol{s}$ from multiple demonstration trajectories which are interrelated but not identical in each type of MP in the library. Second, how to ensure a smooth transition of the velocity at the switching points in the MP sequence given by the initial condition $\boldsymbol{I}$.

III Methodology

III-A Modified Representation Method of MPs

The original DMP algorithm only has the ability to represent a single demonstration trajectory[23], which is defined as

[TABLE]

where $\boldsymbol{d}_{m}=[{x_{m}},{y_{m}}]$. $\boldsymbol{v}_{m}=[{\dot{x}_{m}},{\dot{y}_{m}}]$, $\boldsymbol{g}=[{x_{m}}(T),{y_{m}}(T)]$ is the goal position of the selected MP and $\boldsymbol{b}=[{x_{m}}(0),{y_{m}}(0)]$ is the initial position. ${\psi_{n}}({z_{m}})=exp(-{p_{n}}{({z_{m}}-{\mu_{n}})^{2}})$ is the Gaussian basis function, $\omega_{n}$ is an element in the shape representation parameter set, and $N$ is the total number of the elements in the parameter set. The constants $\alpha_{m}$ and $\beta_{m}$ are pre-defined to ensure the spring-damper system is critically damped. and $\tau$ is the time scaling parameter.

In order to enable the proposed method to represent multiple trajectories of the same type in the MP library and provide an interface for subsequent MPs joining, we propose a modified DMP method. The modified canonical system is defined as

[TABLE]

where $T$ is the duration of the MP, ${\Delta t}$ is the sampling interval. Because multiple demonstration trajectories in the same type of MP do not necessarily have the same duration, in order to ensure that the vectors after resampling have the same size, the sampling interval $\Delta t=T/100$.

We introduce the goal function to replace the goal position value to avoid the acceleration jump caused by the sudden change of the goal position at the switching point. The modified form is proposed as follows:

[TABLE]

The nonlinear function $f(t,{z_{m}})$ is the core factor in representing the shape transformation of trajectories. In our modified representation algorithm, the definition of the nonlinear transformation function for one-DOF is as follows:

[TABLE]

where ${\psi_{ji}}(t)=exp(-{p_{ji}}{(\frac{t}{{\tau T}}-{\mu_{ji}})^{2}})$ is the Gaussian basis kernel of the modified DMP with $\mu_{ji}$ represents the center and $p_{ji}$ represents the bandwidth. $\boldsymbol{\omega}=[\boldsymbol{\omega}_{1},\cdots,\boldsymbol{\omega}_{J}]$ and $\boldsymbol{s}=[\boldsymbol{s}_{1},\cdots,\boldsymbol{s}_{J}]$ are the parameter matrices of one-DOF data in the inherent attribute set $\boldsymbol{\omega}_{m}$ and fine tuning parameter set $\boldsymbol{s}$ respectively. These two parameter sets $\boldsymbol{\omega}$ and $\boldsymbol{s}$ are combined to be used as the representation parameters of the trajectory shape.

III-B Learning Process of MP Parameter Set

The training data is a set of multiple demonstration trajectories with a similar shape obtained by the probabilistic segmentation algorithm. The learning process consists of two main processes: The first is to extract the basic shape representation data $\boldsymbol{D}_{baiss}$ from multiple demonstrations by using the SVD algorithm. The second is to obtain the inherent attribute set $\boldsymbol{\omega}_{m}$ by minimizing the errors between representation and demonstration. Since the training process of the longitudinal and lateral parameters is the same, we will only introduce the training process of the longitudinal part.

Assuming that there are $Q$ demonstration trajectories in the selected MP type, data resampling ensures that each segmented trajectory has $C$ sampling points, so all longitudinal trajectory data in this type can be represented as the same size of the vector $\boldsymbol{x}_{q}^{dem}\in{\mathbb{R}^{C\times 1}}$.

We can calculate the specific value to be represented by the nonlinear function in each demonstration trajectory by applying the following equation:

[TABLE]

The data set $\boldsymbol{F}_{dem}=[\boldsymbol{f}_{dem}^{1},\cdots,\boldsymbol{f}_{dem}^{Q}]^{\rm{T}}\in{\mathbb{R}^{Q\times C}}$ will be obtained after calculating all the demonstration trajectories of the selected type. Then we apply the SVD to decompose the $Q\times C$ matrix $\boldsymbol{F}_{dem}$, and the decomposed matrix is represented as follows:

[TABLE]

where the basic shape representation data ${\boldsymbol{D}_{basis}}={[\boldsymbol{D}_{basis}^{1},\cdots,\boldsymbol{D}_{basis}^{J}]^{\rm{T}}}\in{\mathbb{R}^{J\times C}}$ is the first $J$ columns of ${\boldsymbol{\Sigma}}{{\boldsymbol{V}}^{\rm{T}}}$. The fine tuning parameter set $\boldsymbol{s}=[{s^{1}},\cdots.{s^{Q}}]^{\rm{T}}\in{\mathbb{R}^{Q\times J}}$ is the first $J$ columns of $\boldsymbol{U}$. The dimension $J$ is determined by the singular value spectrum. In general, the value of $J$ is much smaller than $Q$ ($J\ll Q$).

The learning of inherent attribute set ${\boldsymbol{\omega}_{x}}=[{\boldsymbol{\omega}_{1}},\cdots,{\boldsymbol{\omega}_{J}}]^{\rm{T}}\in{\mathbb{R}^{J\times N}}$ is to solve the optimization problem as shown in Eq.14, and the problem can be solved by a regression algorithm mentioned in the previous study[23].

[TABLE]

Through the above process, the ${\boldsymbol{\omega}_{m}}={[{\omega_{x}},{\omega_{y}}]^{\rm{T}}}$ is learned to represent the separated basic shape data.

III-C Generating the MP Sequence

The simple joining method is to execute the next MP at the end of the previous MP, but this simple method only has a simple position correlation capability, i.e., the termination position of the previous MP is used as the starting position of the next MP, but the method does not have the velocity correlation ability in the process of generating MP sequences.

Therefore, we propose a novel joining method based on the modified MP representation algorithm to solve the problem of smooth transition. In the proposed joining method, we treat the MPs in the generated sequence as a whole rather than separate individual. A single set of overlapping kernels is established to construct the connections between different kinds of MPs in the sequence.

The centers $\boldsymbol{\mu}^{\prime}$ and width $\boldsymbol{p}^{\prime}$ of the Gaussian kernels are regenerated as follows:

[TABLE]

where $K$ is the total number of MPs in the generated sequence, and $T_{k}$ is the time duration of ${k^{{\rm{th}}}}$ MP in the sequence which is given by the initial condition $\boldsymbol{I}_{k}$. $T^{\prime}=\sum\limits_{k=1}^{K}{{T_{k}}}$ is the total time duration of the regenerated MP sequence. The inherent attribute set $\omega^{\prime}$ is a simple combination of each parameter set learned from the demonstration.

[TABLE]

According to the given initial condition $\boldsymbol{g_{k}}=[{x_{g}^{k}},{y_{g}^{k}}]$ and $\boldsymbol{b_{k}}=[{x_{init}^{k}},{y_{init}^{k}}]$, the goal function of the MP sequence is defined as:

[TABLE]

The canonical system $z^{\prime}$ is defined as

[TABLE]

Besides, since the coordinate system of a single MP is $x_{m}oy_{m}$, and the coordinate system of the MP sequence system is $xoy$, the coordinate transformation is required in the process of generating the MP sequence.

[TABLE]

where ${\delta_{j}}$ is the course angle at the end of the previous MP which is defined in the coordinate system $xoy$. $d_{x}$, $d_{y}$ are the position values and $v_{x}$, $v_{y}$ are the velocity values in the coordinate system $xoy$ of the generated MP sequence. ${f_{x}}(t,z)$ and ${f_{y}}(t,z)$ are the nonlinear functions which are based on the redefined overlapping Gaussian kernels.

Based on the given initial condition $\boldsymbol{I}$ and Eq.17 to Eq.31, we can achieve a smooth connection between multiple MPs in the generated MP sequence.

IV Experiments and Results

IV-A Data Collection and Preprocessing

The data set used in this paper is the same as the data set used in the previous study of MP representation and segmentation[23]. The established MP library which contains the segmented and classified observed trajectory data of a single human driver is selected as the training dataset.

The position values in the representation dataset $\boldsymbol{m}_{m}$ are calculated by integrating the vehicle speed and course angle acquired at equal time intervals. The vehicle speed is collected by the CAN bus, the course angle is obtained from the integrated navigation unit, and the time interval of the data collection system is 100ms. In order to maintain the consistency of the data dimension, multiple observed trajectories included in each type of MP need to have the same number of sampling points. Therefore, before the MP training process, the trajectory data need to be resampled so that each trajectory has 100 sampling points.

IV-B Evaluation of Typical MP Representation and Regeneration

The purpose of the MP training is to represent the driver’s various typical driving behaviors, including lane keeping, lane changing and other driving trajectory segments with different steering correction levels at different speeds. Considering that the MPs demonstrated here should be the most common driving behavior and contain the typical longitudinal-lateral cooperation relationship, the sharp turn MP at low speed and lane changing MP at medium speed were selected from the established MPs library.

The accuracy of the MP representation was evaluated by the average position deviation ${\Delta\bar{d}}$ and average velocity deviation ${\Delta\bar{v}}$, and the deviations were the absolute mean difference between all the demonstrated trajectories and the learned trajectories in the same type of MPs. We demonstrated the effect of different numbers of fine tuning parameter $\boldsymbol{s}$ (Eq.11) on the representation accuracy in Fig.2, and the corresponding average deviation values were presented in Table I.

In addition, we also demonstrated the adaptability of the selected two MPs to the change of goal position $\boldsymbol{g}$, time duration $T$, and fine-tuning parameter $\boldsymbol{s}$ in Fig.3.

IV-C Evaluation of MPs Joining

In order to demonstrate the benefits of the proposed joining method, we applied the simple joining method as a baseline and evaluated the performance of the joining algorithm at high speed and low speed driving situations. And the evaluation indexes were the maximum acceleration $a_{max}$ and position deviation $\Delta d_{k}$. The maximum acceleration $a_{max}$ referred to the maximum value of the acceleration generated during the transition of the MP combination, and the position deviation $\Delta d_{k}$ referred to the minimum distance between the generated trajectory and the target points set by the initial condition.

The generation results of the MP sequence based on the two methods were shown in Fig.4, and the evaluation indexes for the low-speed and high-speed situations were shown in Table II.

IV-D Discussion

Through the SVD, we not only extracted the main trajectory shape features from multiple demonstration trajectories but also significantly reduced the number of trajectory shape adjustment parameters when the MPs were regenerated. In the original DMP representation algorithm, the trajectory shape was completely represented and adjusted by the parameter set $\boldsymbol{\omega}$ (Eq.4). In order to obtain acceptable representation accuracy, the value of $N$ (Eq.4) was usually set to 20. In our proposed method, we still used the $\boldsymbol{\omega}$ to complete the basic shape representation task but used $\boldsymbol{s}$ to complete the shape adjustment task (Eq.11). Although the value of $N$ is still 20, the value of $J$ is greatly reduced. With this feature, we can fine-tune the shape of the trajectory with limited parameters without affecting the accuracy of the representation.

From Fig.2 and Table I, we can observe that for all the demonstration trajectories in the two selected types of MPs, the position deviation and the velocity deviation decreased with the increase of the value of $J$. However, the improvement of the representation accuracy was inconsistent for multiple demonstration trajectories in the same type. That is to say, the increase of the $J$ introduced more trajectory shape features in the chosen type, which improved the representation ability of the proposed method for different demonstration trajectories in the same type. In order to limit the number of shape adjustment parameters, we only selected some shape features, which also sacrificed the representation accuracy. However, this sacrifice in accuracy reduced the number of MPs shape adjustment parameters, and the reduction of adjustment parameters is of great significance for the future utilization of the MP library.

As for the MP regeneration as shown in Fig.3, we retain the ability of the original DMP representation method to adapt to the changes in goal position and time duration, and add fine-tuning capabilities of the trajectory shape to further improve the adaptability of MPs to different motion planning tasks. The joining method proposed in this paper transforms the problem of generating MP sequences into a re-presentation problem of a single MP. By associating the attribute information of each independent MP in the sequence, the kernel function, the goal function, and canonical system are redefined and the re-representation task is completed. Since the MP sequence can be considered as a whole after re-representation, the transition problem between MPs is converted into a single MP regeneration problem. And we can easily achieve a smooth transition by utilizing the properties of modified DMP representation algorithm. From the results in Fig.4 and Table II, we can find that although there were position deviations when tracking the set target points, the maximum acceleration of the proposed method in the joining process was only $5.8\%$ of the simple joining method in situation 1 and $4.6\%$ in situation 2.

V Conclusion

This paper proposed a modified MP representation algorithm to deal with the need for MPs regeneration and joining in the automated vehicle motion planning system. The proposed method can represent the multiple similar demonstrated trajectories with acceptable precision and reduce the number of shape adjustment parameters during regeneration. This improvement is due to the introduction of SVD, which enables us to separate the basic shape parameters and fine-tuning adjustment parameters and to extract the main features from multiple demonstration trajectories of the same type. Besides, the proposed MP representation method also has the ability to extend from a single MP representation to a MP sequence representation and uses its smoothing ability in the regeneration process to avoid the velocity jump under the premise of ensuring accuracy. The proposed method is to further improve the applicability of the previously established motion primitive library by enhancing the adjustment ability and achieving a smooth transition. With the above two aspects of improvement, the established MP library can better meet the needs of the motion planning system and help to complete personalized driving tasks.

In future work, we will further enhance the representation capabilities of the MPs so that different driving styles can be incorporated into the representation algorithm in the form of adjustment parameters, and combine the established MP library with the existing motion planning algorithm.