Generation and Selection of Driver-Behavior-Based Transferable Motion Primitives

Abstract

To integrate driver experience and heterogeneous vehicle platform characteristics in a motion-planning algorithm, based on the driver-behavior-based transferable motion primitives (MPs), a general motion-planning framework for offline generation and online selection of MPs is proposed. Optimal control theory is applied to solve the boundary value problems in the process of generating MPs, where the driver behaviors and the vehicle motion characteristics are integrated into the optimization in the form of constraints. Moreover, a layered, unequal-weighted MP selection framework is proposed that utilizes a combination of environmental constraints, nonholonomic vehicle constraints, trajectory smoothness, and collision risk as the single-step extension evaluation index. The library of MPs generated offline demonstrates that the proposed generation method realizes the effective expansion of MP types and achieves diverse generation of MPs with various velocity attributes and platform types. We also present how the MP selection algorithm utilizes a unique MP library to achieve online extension of MP sequences. The results show that the proposed motion-planning framework can not only improve the efficiency and rationality of the algorithm based on driving experience but can also transfer between heterogeneous vehicle platforms and highlight the unique motion characteristics of the platform.

1 Introduction

The ever-increasing advancement of unmanned vehicle technology, especially multivehicle cooperative technology, will significantly promote the development of future intelligent transportation systems and unmanned combat systems and will affect future travel modes and combat patterns [1,2,3]. Motion planning is a crucial component in the unmanned vehicle system framework. Its main function is to generate a reference trajectory that satisfies the constraints of the environment and the vehicle itself [4,5,6]. Building a unified motion-planning algorithm for heterogeneous vehicle platforms and using the driving experience to guide and accelerate the completion of planning tasks is a fundamental task for the further development of unmanned vehicle motion-planning algorithms. In both the motion-planning algorithm and the driving behavior representation algorithm, the decomposition of complex motion into motion primitives (MPs) can effectively improve the efficiency of the algorithm [7,8,9,10].

The essence of MP generation is to solve a set of boundary value problems, that is, to generate a set of paths connecting different target states [11]. The difficulty in solving the above problems lies in the segmentation strategy of the state space and the form of the curve connecting the start and end states. Graph search-based motion-planning algorithms, such as the typical A* [12, 13], D* [14, 15], and state lattice [16, 17], divide the environment space into basic grids and then generate MPs based on center points, corner points, or edges. To overcome the disadvantages of a grid-based MP generation method that cannot consider nonholonomic vehicle constraints, the Hybrid A* algorithm completes the generation of MPs based on the vehicle kinematic model [7, 18]. Each primitive used in the Hybrid A* algorithm is an arc generated with a fixed time scale and a fixed front wheel angle. The discrete optimization-based method uses spline curves or polynomial curves to generate MP candidate sets based on the offset points of the road centerline, which effectively limits the search space and improves the overall efficiency of the algorithm [19,20,21]. In addition, some methods use numerical optimization to realize the generation of MPs while considering vehicle dynamic constraints [22,23,24]. Although the MPs generated by using the above methods can meet the needs of a complex driving environment, they fail to integrate the driver's behavior information into the MP generation algorithm. Moreover, these methods generally lack consideration of the differences and connections of the heterogeneous platform motion characteristics.

The quality of the final path generated by the motion-planning algorithm depends not only on the quality of the MP itself but also on the rationality of the MP selection and connection [25]. The selection of MPs is the process of first considering the cost function represented by different constraints, then combining the cost according to a particular weight factor, and finally selecting the MP with the minimum cost [26]. The Dijkstra algorithm takes the shortest path as the extension cost of the MP [27]. The A* algorithm and its variants add heuristics based on the target state and Voronoi diagram to the original Dijkstra algorithm [28]. The sampling-based RRT path planning algorithm [29], on the one hand, takes the target point as a guide to the selection and extension of MPs; on the other hand, it randomly extends in any direction with a certain probability to achieve a comprehensive search of accessible areas. The discrete optimization-based method [19] utilizes the linear combination of the three extension cost values by considering static obstacles, dynamic obstacles, and path smoothness as the basis for the selection of MPs. Although the MP selection and extension method mentioned above can generate the desired driving trajectory, no attention is paid to the guiding role of the driver’s experience.

The learning and generalization of the driving experience and characteristics have attracted the attention of many researchers in recent years [30,31,32]. Specific to trajectory-level driving behavior research, Guo et al. [33] researched the generation of human-like trajectories with the leader vehicle as an attractive force. Although this method can cope with complex dynamic environments, the learning object of its behavior is the guide vehicle; that is, it is a follow-up human-like trajectory-generation method. Regarding the representation of human-like trajectories that target the behavior of the ego vehicle, Schnelle et al. [34] proposed a personalized trajectory optimization generation method in two scenarios: one with a single lane change and another with a double lane change. Zhao et al. also conducted a series of studies on lane-change scenes. First, they used the support vector machine algorithm to extract lane-change primitives from the lane-change trajectory data of surrounding vehicles [35, 36]. Subsequently, using the principle of trajectory matching, the generated human-like lane-change trajectory was integrated into a real-time motion-planning system [37, 38]. However, the aforementioned research has limitations in the application of scenes. It is often aimed at a specific scene, and there is no further study on how to combine multiple driving behaviors with the vehicle motion-planning system in a general driving environment.

In this paper, for the two heterogeneous platforms of wheeled Ackermann-steering vehicles and tracked skid-steering vehicles, a unified vehicle motion differential constraint is proposed. In addition, five types of equality constraints for typical driving behaviors and inequality constraints for vehicle platform characteristics are set, and an offline optimization method for driver-behavior-based transferable motion primitive (DBTMP) generation is formed. Based on the unique MP library established for different vehicle platforms, research on the selection of MPs was conducted under the Hybrid A* algorithm framework. However, there are two significant differences in the construction of the selection framework owing to the large difference between the offline generated MP library and the online generated MP library used by the Hybrid A* algorithm. First, the MP library generated offline is a collection of multiple MP sets containing the entire speed interval; therefore, a hierarchical structure is applied to restrict the MP candidate set. Second, because each candidate set contains both behavior primitives and general primitives, different weight coefficients are applied to the above two types of primitives when selecting and evaluating a single extended MP. Finally, a motion-planning algorithm based on offline generation and online selection of the MP library is proposed.

The main contributions of this study are the following:

• A unified motion-planning algorithm framework is proposed to complete the motion-planning tasks of heterogeneous platforms, achieving a balance between the general solution framework and the characteristics of heterogeneous vehicle platforms.

• An offline MP generation method combining discrete driving behaviors and a vehicle kinematics model is proposed, and the guiding role of behavior primitives in the online selection layer of the motion-planning algorithm is highlighted.

The remainder of this paper is organized as follows. Section 1 details the problems to be solved in this study and explains the main parameters. Section 2 introduces the offline generation method for the MP library. Section 3 introduces an online selection framework for MPs. Section 4 demonstrates the unique MP libraries of two heterogeneous vehicle platforms and the results of motion planning in two typical scenarios. Finally, the conclusions and future work are presented in Section 5.

2 Problem Statement

The offline MP generation solves the optimization problem with driver behavior B and platform motion characteristics as constraints and trajectory smoothness as the objective function g. The selection of MPs is first to select the MP set from the MP library based on the passable area and velocity connection, and then utilize the cost function J as the evaluation index to choose the corresponding single extended MP from the MP set.

The definitions and explanations of the main parameters used in this study are as follows:

• ${\varvec{s}}(t)=[x(t),y(t),\theta (t){]}^{\mathrm{T}}\in {R}^{3\times 1}$ is the unified state parameter set of each heterogeneous platform at time t, where x(t) and y(t) are the coordinate positions of the x and y axes, respectively, and $\theta (t)$ is the course angle. The defined coordinate system is the geodetic coordinate system xoy.

• ${\varvec{u}}(t)=[{v}_{x}(t),{v}_{y}(t),{\omega }_{z}(t){]}^{\mathrm{T}}\in {R}^{3\times 1}$ is the unified control variable set of each heterogeneous platform at time t, where v_x(t) and v_y(t) are the velocities of the platform along the x and y axes, respectively, and ${\omega }_{z}(t)$ is the yaw rate around the z axis. The defined coordinate system is the vehicle body coordinate system x_aoy_a or x_toy_t.

• ${{\varvec{u}}}_{\mathrm{a}}(t)=[{v}_{{w}_{x}}(t),\alpha (t){]}^{\mathrm{T}}\in {R}^{2\times 1}$ is the unique control variable set of the wheeled Ackermann-steering vehicle at time t, where ${v}_{{w}_{x}}(t)$ is the velocity of the rear axle along the x_a axis and $\alpha (t)$ is the front wheel angle of the vehicle. The defined coordinate system is the vehicle body coordinate system x_aoy_a.

• ${{\varvec{u}}}_{t}(t)=[{v}_{{l}_{x}}(t),{v}_{{r}_{x}}(t){]}^{\mathrm{T}}\in {R}^{2\times 1}$ is the unique control variable set of the tracked skid-steering vehicle at time t, where ${v}_{{l}_{x}}(t)$ and ${v}_{{r}_{x}}(t)$ are the velocities of the left and right tracks, respectively, along the x axis. The defined coordinate system is the vehicle body coordinate system x_toy_t.

• ${\varvec{D}}={{\varvec{D}}}_{s}\cdot {{\varvec{D}}}_{\text{a/t}}$ represents the mapping relationship between the platform state parameters and control variables, where ${{\varvec{D}}}_{s}$ is the correlation between the unified state parameter set s and control variable set u. ${{\varvec{D}}}_{\text{a/t}}$ is the correlation between u and the unique control variable set u_a/t.

• ${\varvec{B}}=\left\{{B}_{\text{SD}},{B}_{\text{LC}},{B}_{\text{UT}},{B}_{\text{RT}},{B}_{\text{TA}}\right\}$ are the five defined types of driving behavior constraints: straight driving behavior ${B}_{\text{SD}}$, lane-changing behavior ${B}_{\text{LC}}$, U-shaped turn behavior ${B}_{\text{UT}}$, right-angle turn behavior ${B}_{\text{RT}}$, and turn-around behavior ${B}_{\text{TA}}$.

• U is the inequality constraint condition set according to the motion limit of the platform. U_a and U_t are for wheeled Ackermann-steering vehicles and tracked skid-steering vehicles, respectively.

• ${\varvec{J}}=\left\{{J}_{\mathrm{e}},{J}_{\mathrm{n}},{J}_{\mathrm{s}},{J}_{\mathrm{c}}\right\}$ is the cost function when selecting a single MP, where J_e is the environmental constraint heuristic, J_n is the nonholonomic vehicle constraint heuristic, J_s is the evaluation function considering the smoothness of the curve, and J_c is the evaluation function considering the collision risk.

The method proposed in this study is dedicated to solving the following two challenges: (1) determining how to generate an MP library that can be transferred between heterogeneous platforms, considering the overall versatility and highlighting the specificity of the vehicle platform, and (2) integrating the driving behavior factors into the generation and selection algorithm of the MPs and then realizing the guidance of the driver’s experience with the motion-planning algorithm.

3 Generation of MPs

The generation process of the MP library is shown in Figure 1. The entire processing flow can be decomposed into two main components: (1) the creation and solution of optimal control problems and (2) the rotation transformation and velocity expansion of a single MP cluster. The definition of the optimal control problem for a single MP generation lies at the core of the generation method.

Figure 1

MP offline generation algorithm flow

The specific form of the optimal control problem is as follows:

$$\begin{gathered} \mathop {{\text{min}}}\limits_{{{\varvec{u}}_{{\text{a/t}}} }} {: }g({\varvec{s}},{\varvec{u}}) \hfill \\ {\text{subject to: }} \hfill \\ \left\{ \begin{gathered} {\varvec{s}}(t) = {\varvec{B}}, \, t \in [t_{1} ,t_{{\text{g}}} ], \hfill \\ {\dot{\varvec{s}}}(t) = f({\varvec{s}}(t),{\varvec{u}}(t)){, }t \in [t_{1} ,t_{{\text{g}}} ], \hfill \\ {\varvec{U}}(t) \in {\varvec{U}}, \, t \in [t_{1} ,t_{{\text{g}}} ], \hfill \\ \end{gathered} \right. \hfill \\ \end{gathered}$$

(1)

where $g({\varvec{s}},{\varvec{u}})$ is the objective function of the trajectory smoothness optimization based on the state and control variables; ${\varvec{s}}(t)={\varvec{B}}$ is the constraint condition of the vehicle state based on the driving behavior; $\dot{{\varvec{s}}}(t)=f({\varvec{s}}(t),{\varvec{u}}(t))$ is the motion differential constraint describing the relationship between the control variables and the state parameters of the vehicle platform; ${\varvec{U}}(t)\in {\varvec{U}}$ is the inequality constraint condition set, and t₁ and t_g are the start and end times of the MP generation, respectively.

3.1 Vehicle Motion Differential Constraint

In this study, we ignore the impact of the suspension system on the vehicle motion response and regard the vehicle platform as a rigid body moving in a two-dimensional plane. The relationship between the unified state parameter set s and the unified control variable set u in a two-dimensional space is

$${\dot{\varvec{s}}}(t) = {\varvec{D}}_{{\text{s}}} \cdot {\varvec{u}}(t) = \left[ {\begin{array}{*{20}c} {\cos \theta (t)} & { - \sin \theta (t)} & 0 \\ {\sin \theta (t)} & {\cos \theta (t)} & 0 \\ 0 & 0 & 1 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {v_{x} (t)} \\ {v_{y} (t)} \\ {\omega_{z} (t)} \\ \end{array} } \right].$$

(2)

According to the characteristics of heterogeneous platform actuators, the following mapping relationship between the unique control variable set u_a/t and the unified control variable set u of different types of platforms can be established:

$${\varvec{u}}(t) = {\varvec{D}}_{{\text{a/t}}} \cdot {\varvec{u}}_{{\text{a/t}}} (t).$$

(3)

The relationship between the control variable u_a of a typical wheeled Ackermann-steering vehicle and the unified state parameter set s is

$$\dot{\user2{s}}(t) = {\varvec{D}}_{{\text{s}}} \cdot {\varvec{D}}_{{\text{a}}} \cdot {\varvec{u}}_{{\text{a}}} (t) = \left[ \begin{gathered} v_{{w_{x} }} (t)\cos \theta (t) \hfill \\ v_{{w_{x} }} (t)\sin \theta (t) \hfill \\ v_{{w_{x} }} (t)\tan \alpha (t)/L \hfill \\ \end{gathered} \right],$$

(4)

where L is the distance between the front and rear axles of the vehicle. The related coordinate system and simplified model of the wheeled vehicle platform are shown in Figure 2.

Figure 2

Wheeled Ackerman-steering vehicle

The relationship between the control variable u_t of a typical tracked vehicle and the unified state parameter set s is

$${\dot{\varvec{s}}}(t) = {\varvec{D}}_{{\text{s}}} \cdot {\varvec{D}}_{{\text{t}}} \cdot {\varvec{u}}_{{\text{t}}} (t) = \left[ {\begin{array}{*{20}c} {(v_{{l_{x} }} (t) + v_{{r_{x} }} (t))\cos \theta (t)/2} \\ {(v_{{l_{x} }} (t) + v_{{r_{x} }} (t))\sin \theta (t)/2} \\ {(v_{{r_{x} }} (t) - v_{{l_{x} }} (t))/B} \\ \end{array} } \right],$$

(5)

where B is the distance between the two tracks. The related coordinate system and simplified model of the tracked vehicle platform are shown in Figure 3.

Figure 3

Tracked skid-steering vehicle

3.2 Equality Constraints of Driving Behavior

This study defines five typical driving behaviors, as shown in Figure 4. These five behaviors can be classified into two main types: behaviors related to typical road structures (B_SD, B_UT, and B_RT) and behaviors related to typical driving behavior patterns (B_LC and B_TA). The MPs related to the road structure highlight the restraint effect of typical road structure factors on behavior, and the MPs related to the driving pattern highlight the restraint effect of the driver's behavior preferences. The remaining general MPs in the library are used as a supplement to the behavior primitives to improve the planning algorithm's adaptability to different driving environments. By increasing the priority of behavior primitives in the subsequent selection logic, we hope to better adapt to road structures and driving habits.

Figure 4

Driver behavior MPs

Among the above behavior primitives, B_SD and B_LC exist throughout the velocity range, and B_UT, B_RT, and B_TA exist only in the low-velocity range. Owing to the specificity of the vehicle structure, B_TA can be subdivided into ${{B}_{\text{TA}}}_{\mathrm{a}}$ for wheeled vehicles and ${{B}_{\text{TA}}}_{\mathrm{t}}$ for tracked vehicles.

For straight driving behavior B_SD, U-shaped turn behavior B_UT, and right-angle turn behavior B_RT, only the course angle change between the start state t₁ and the end state t_g is constrained:

$$B_{{{\text{SD}}}} = \theta (t_{{\text{g}}} ) - \theta (t_{1} ) = 0,$$

(6)

$$B_{{{\text{UT}}}} = \theta (t_{{\text{g}}} ) - \theta (t_{1} ) = {\uppi ,}$$

(7)

$$B_{{{\text{RT}}}} = \theta (t_{{\text{g}}} ) - \theta (t_{1} ) =\uppi /2.$$

(8)

For lane-changing behavior B_LC, in addition to restraining the course angle change deviation, the lateral distance change deviation d should also be constrained:

$$B_{{{\text{LC}}}} = \left[ {\begin{array}{*{20}c} {\theta (t_{{\text{g}}} ) - \theta (t_{1} )} \\ {[\cos \theta (t_{{\text{g}}} ),\sin \theta (t_{{\text{g}}} )][x(t_{{\text{g}}} ),y(t_{{\text{g}}} )]^{{\text{T}}} - d} \\ \end{array} } \right] = 0.$$

(9)

For the turn-around behavior ${{B}_{\text{TA}}}_{\mathrm{a}}$ of a wheeled vehicle, it is necessary to restrict not only the beginning and end states but also the intermediate states accordingly:

$$B_{{{\text{TA}}_{{\text{a}}} }} = \left[ {\begin{array}{*{20}c} {\theta (t_{{\text{g}}} /3) - \theta (t_{1} ) -\uppi /3} \\ {\theta (2t_{{\text{g}}} /3) - \theta (t_{{\text{g}}} /3) + 2\uppi /3} \\ {\theta (t_{{\text{g}}} ) - \theta (2t_{{\text{g}}} /3) -\uppi /3} \\ \end{array} } \right] = 0.$$

(10)

Because the tracked vehicle can complete the task of turning around through the pivot turn movement, its constraint relationship ${{B}_{\text{TA}}}_{\mathrm{t}}$ is as follows:

$$B_{{{\text{TA}}_{{\text{t}}} }} = \left[ {\begin{array}{*{20}c} {x(t_{{\text{g}}} ) - x(t_{1} )} \\ {y(t_{{\text{g}}} ) - y(t_{1} )} \\ {\theta (t_{{\text{g}}} ) - \theta (t_{1} ) -\uppi } \\ \end{array} } \right] = 0.$$

(11)

3.3 Inequality Constraints of Vehicle Platform Characteristics

The constraints of vehicle platform characteristics mainly include four aspects (the input range of control variable u, the limitation of yaw rate ${\omega }_{z}$, the limitation of lateral acceleration a_y, and the limitation of the current generated MP velocity attribute v):

$${\varvec{U}} = \left\{ \begin{gathered} {\varvec{u}} \in [ - {\varvec{u}}_{\max } ,{\varvec{u}}_{\max } ], \hfill \\ \omega_{z} \le 0.8, \hfill \\ a_{y} \le 0.4g, \hfill \\ v \in \left[ {v_{{{\text{low}}}} ,v_{{{\text{up}}}} } \right]. \hfill \\ \end{gathered} \right.$$

(12)

The inequality constraint relationship U_a of the wheeled Ackermann-steering vehicle is

$${\varvec{U}}_{{\text{a}}} { = }\left\{ \begin{gathered} \alpha \in \left[ { - \alpha_{\max } ,\alpha_{\max } } \right], \hfill \\ v_{{\omega_{x} }} \tan \alpha /L \le 0.8, \hfill \\ v_{{_{{\omega_{x} }} }}^{2} \tan \alpha /L \le 0.4g, \hfill \\ v_{{w_{x} }} \in \left[ {v_{{{\text{low}}}} ,v_{{{\text{up}}}} } \right], \hfill \\ \end{gathered} \right.$$

(13)

where ${\alpha }_{\mathrm{max}}$ is the maximum front wheel angle of the vehicle platform.

The inequality constraint relationship U_t of the tracked skid-steering vehicle is

$${\varvec{U}}_{{\text{t}}} = \left\{ \begin{gathered} v_{{l_{x} }} \in \left[ { - v_{{l_{x\_} \rm{max}}} ,v_{{l_{x\_} \rm{max} }} } \right], \hfill \\ v_{{r_{x} }} \in \left[ { - v_{{r_{x\_} \rm{max} }} ,v_{{r_{x\_} \rm{max}}} } \right], \hfill \\ \left| {v_{{l_{x} }} - v_{{r_{x} }} } \right|/B \le 0.8, \hfill \\ \left| {v_{{l_{x} }}^{2} - v_{{r_{x} }}^{2} } \right|/2B \le 0.4g, \hfill \\ (v_{{l_{x} }} + v_{{r_{x} }} )/2 \in \left[ {v_{{{\text{low}}}} ,v_{{{\text{up}}}} } \right], \hfill \\ \end{gathered} \right.$$

(14)

where ${v}_{{l}_{x-max}}$ and ${v}_{{r}_{x-max}}$ are the maximum speed limits of the left and right driving wheels, respectively.

3.4 Objective Function and Optimization Problem Solving

The objective function g mainly reflects the smoothness requirements of MP generation and includes the steering control input c_s and yaw rate of the vehicle platform ${\omega }_{z}$:

$$g = \int_{0}^{{t_{{\text{g}}} }} {(c_{{\text{s}}}^{2} { + }\omega_{{\text{z}}}^{2} } ){\text{d}}t.$$

(15)

These two parameters should be as small as possible because they correspond to the curvature of the generated curve and its rate of change, which directly affects the smoothness of the curve.

According to the difference in platform characteristics, the objective function g_a for the wheeled Ackermann-steering platform is given by

$$g_{{\text{a}}} = \int_{0}^{{t_{{\text{g}}} }} {\alpha^{2} (t)} + (v_{{w_{x} }} (t)\frac{\tan \alpha (t)}{{\text{L}}})^{2} {\text{d}}t,$$

(16)

and the objective function g_t for the tracked skid-steering vehicle platform is given by

$$g_{{\text{t}}} = \int_{0}^{{t_{g} }} {(\frac{{2(v_{{l_{x} }} (t) - v_{{r_{x} }} (t))}}{{v_{{l_{x} }} (t) + v_{{r_{x} }} (t)}})^{2} } + (\frac{{v_{{l_{x} }} (t) - v_{{r_{x} }} (t)}}{B})^{2} {\text{d}}t.$$

(17)

In this study, the IPOPT [39] and CVODES [40] were applied to solve the optimal control problem.

3.5 Generation of the MP Library

Owing to the complexity of the real driving environment, having only the defined driving behavior MPs cannot fully satisfy the motion-planning problem in a complex environment. Therefore, to improve the applicability of the MP library, general MPs are generated as a supplement with the target course angle uniformly distributed on $[0, 2\uppi ]$ as the constraint.

In addition, in the online MP extension process, it is necessary to make the final heading of the MP selected in the previous MP cluster consistent with the heading of the straight driving behavior primitive in the next primitive cluster. This process can be performed either online or offline. To improve the efficiency of the algorithm, we used offline generation and online selection to complete the above process. After the MP cluster containing behavior primitives and general primitives is generated, an MP set covering the entire annular space with 36 evenly distributed MP clusters is generated by rotation transformation. The alignment between the MP clusters is completed by selecting the most appropriate angle from the MP set during online extension.

A simplified schematic of the MP set generation is shown in Figure 5; only a portion of the typical MPs and the rotation transformation at three positions are shown in the figure. The resulting MP library is a combination of multiple MP sets under different MP velocity attribute settings.

Figure 5

MP cluster rotation transformation

4 Selection of MPs

The MP selection process is shown in Figure 6. The entire processing flow can be decomposed into the selection of the MP sets, calculation of heuristics and costs, selection of a single MP, and extension of the MP sequence. The selection of MP sets refers to selecting the appropriate MP sets from the MP library based on passable areas in the environment map and the velocity attribute of the last extended periodic MP set. The heuristics include both the environmental constraint heuristic and the nonholonomic vehicle constraint heuristic. The environmental constraint heuristic is calculated from the starting and target poses in the grid map. The nonholonomic vehicle constraint heuristic is calculated based on the final state of every MP in the MP set and the target pose set by the planning system. The extension costs include the trajectory smoothing cost represented by the curve energy and collision risk cost, which is characterized by the distance from the obstacle. Finally, the linear combination of heuristics and costs is selected as the basis for single-step MP extension, and the selected multiple MPs are combined into an MP sequence to complete the motion-planning task.

Figure 6

MP online selection algorithm flow

4.1 Selection of MP Sets

The extraction of the single-step extended passable area is shown in Figure 7. The radius of the passable area, r_area, was calculated using the closest distance d_min=min{d_{obs_1}, d_{obs_2},…, d_{obs_n}} between the current position and the obstacle. The candidate set of MPs must satisfy the essential condition that the final state of every MP is within the passable area.

Figure 7

Single-step extended passable area extraction

In addition, by considering the velocity attribute association of two adjacent periodic MPs, the MP set with a large velocity gap from the previous periodic is eliminated from the candidate set. Finally, further narrowing of the candidate set was achieved.

4.2 Heuristics Considering Environmental and Nonholonomic Constraints

The calculation of the environmental constraint heuristic J_e takes the target position as the initial point and utilizes the breadth-first-search algorithm to realize the iterative expansion of the distance cost until it expands to the starting position. The specific algorithm flow is presented in Algorithm 1. $\Delta J$ is the distance measurement value during the iterative process. According to the difference between the adjacent positions of the current grid, $\Delta J$ has two values: $\left\{d,\sqrt{2}d\right\}$, where d is the grid width.

Figure 8 shows the comparison results of the environmental constraint heuristics calculated based on the Euclidean distance and the breadth-first algorithm used in this study. It can be observed from Figure 8(a) that the calculated heuristics will first guide the MP to extend to the obstacle area, as shown by the dotted blue arrow in the figure. In contrast, in Figure 8(b), the MP is guided to the passable area, as indicated by the solid blue arrow in the figure. Therefore, the environmental constraint heuristic proposed in this study can avoid invalid extended searches and effectively improve the efficiency of the algorithm.

Figure 8

Comparison of environmental constraint heuristics

We also introduce the vehicle nonholonomic constraint heuristic J_n, taking the length of the Reeds-Shepp curve as the heuristic value:

$$J_{{\text{n}}} = \int_{{t_{{{\text{start}}}} }}^{{t_{{{\text{goal}}}} }} {\sqrt {\dot{x}_{{\text{r}}} (t)^{2} + \dot{y}_{{\text{r}}} (t)^{2} } } {\text{d}}t.$$

(18)

The Reeds-Shepp curve is composed of a fixed curvature arc and a straight line. The curve can be defined as a set of poses $\Pi =\left({x}_{r}\left(t\right),{y}_{r}\left(t\right),{\theta }_{r}\left(t\right)\right)\subset {R}^{2}\times \left.\mathrm{0,2\uppi }\right), t\in \left[{t}_{\text{start}},{t}_{\text{goal}}\right]$. It can quickly generate a curve connecting the start and end points while meeting the position and course constraints of the two points.

The nonholonomic constraint heuristic introduces the evaluation of the target point’s course reachability under the premise of considering the vehicle’s motion characteristics, which makes up for the defect in which the environmental constraint heuristic only considers the target position’s reachability.

Finally, a larger value of the environmental constraint heuristic and nonholonomic constraint heuristic is selected as the final heuristic value of the grid, that is, ${J}_{1}=\mathrm{max}\{{J}_{\mathrm{e}},{J}_{\mathrm{n}}\}$.

4.3 Extension Costs Considering Smoothness and Collision

The extension cost proposed in this study mainly includes two aspects: trajectory smoothness and possible collision risk.

The extension cost value of smoothness, J_s, is calculated using the curve energy function as follows:

$$J_{{\text{s}}} = \sum\limits_{i = 2}^{N} {\frac{{(\kappa_{i - 1}^{2} + \kappa_{i}^{2} )\Delta s_{i} }}{2}} ,$$

(19)

where N is the number of discrete points of the curve, ${\kappa }_{i}$ is the corresponding curvature of the point, and $\Delta {s}_{i}$ is the distance between two adjacent points. The essence of this function is the discrete representation of the curvature integral on the curve.

The extension cost value of collision risk, J_c, is calculated using the function

$$J_{{\text{c}}} = \frac{{\sum\limits_{i = 1}^{6} {(d_{{{\text{obs}}\_i}} } - r_{i} )}}{6},$$

(20)

where d_{obs_i} is the distance from the center of each coverage circle to the nearest obstacle and r_i is the radius of each coverage circle. The vehicle platform is approximated by the six coverage circles shown in Figure 9 during the calculation process.

Figure 9

Six covered circles of the vehicle platform

The above extension costs J_s and J_c are given different weights ${\omega }_{\mathrm{s}}$ and ${\omega }_{\mathrm{c}}$. The linear combination after weighting is the final extension value:

$$J_{2} = \omega_{{\text{s}}} J_{{\text{s}}} + \omega_{{\text{c}}} J_{{\text{c}}} .$$

(21)

The weighting coefficient ${\omega }_{\mathrm{s}}$ is differentiated according to the type of MP as follows:

$$\omega_{{\text{s}}} { = }\left\{ \begin{gathered} \omega_{{{\text{s}}_{{\text{B}}} }} {\text{ if mp}}_{i} {\text{ = = Behavior MP,}} \hfill \\ \omega_{{{\text{s}}_{{\text{G}}} }} {\text{ if mp}}_{i} {\text{ = = General MP,}} \hfill \\ \omega_{{{\text{s}}_{{\text{D}}} }} {\text{ if mp}}_{i} {\text{ = = Reverse Direction}}{. } \hfill \\ \end{gathered} \right.$$

(22)

In principle, the relationships among the weight values of behavior MPs ${\omega }_{{\mathrm{s}}_{\mathrm{B}}}$, general MPs ${\omega }_{{\mathrm{s}}_{\mathrm{G}}}$, and MPs in reverse direction, ${\omega }_{{\mathrm{s}}_{\mathrm{D}}}$, is ${\omega }_{{\mathrm{s}}_{\mathrm{B}}}<{\omega }_{{\mathrm{s}}_{\mathrm{G}}}\ll {\omega }_{{\mathrm{s}}_{\mathrm{D}}}$.

4.4 Evaluation Function of MP Selection

The total cost of selecting a single MP mp_i in the MP candidate set should be the sum of the corresponding heuristic and extension costs:

$$J({\text{mp}}_{i} ) = J_{1} ({\text{mp}}_{i} ) + J_{2} ({\text{mp}}_{i} ),\quad i \in [0,N_{{\text{p}}} ],$$

(23)

where N_p is the number of independent MPs in the MP candidate set. In the MP extension process at each step, the MP with the lowest total cost in the MP candidate set is selected as an extension. Finally, the desired trajectory generation from the start position to the end position in the environment map can be realized.

5 Experimental Results and Discussion

To verify the effect of the DBTMP A* motion-planning algorithm proposed in this study, the wheeled Ackermann-steering vehicle and tracked skid-steering vehicle were chosen, and the corresponding tests were conducted in both the simulation environment and the real environment. In the simulation environment, we conducted an in-depth comparative analysis of the algorithm performance in low-speed and high-speed scenes. In the real environment, we mainly focused on the applicability of the algorithm in real situations, especially the compatibility with the platform and the corresponding autonomous driving module. In addition, the classic Hybrid A* method was selected for comparison. The specific platform parameters are listed in Table 1, where P_a represents the wheeled Ackerman-steering platform and P_t represents the tracked skid-steering platform.

Table 1 Platform geometric parameters and actuator constraints

5.1 MP Library Offline Generation

The MP libraries for both platforms used a unified optimization framework during the generation process, and only the details of the vehicle characteristic constraints were changed during the platform transformation process.

The overall situation of the two platform MP libraries is given in Table 2, where v_m (m/s) is the velocity attribute of the MP cluster, d_m (m/s) is the longest distance covered by the MP cluster, N_B is the number of behavioral primitives in the MP cluster, N_m is the number of general primitives in the MP cluster, and $\overline{J}_{{\text{s}}}$ is the average curve energy value of each MP in the cluster. At the end of Table 2, the overall situation of the MP library used by the Hybrid A* method, which does not distinguish between vehicle platforms and velocity attributes, is presented. In Table 2, listed are the composition structures of the MP library generated offline for the wheeled Ackermann-steering platform and tracked skid-steering platform. The structure of the online generated MP library used by the Hybrid A* algorithm is also introduced as a comparison reference.

Table 2 Generated MP library

To show the generated MP clusters more intuitively, Figure 10 presents low-speed and high-speed typical MP clusters with wheeled platform velocity attributes of 5 and 18 m/s and tracked platform velocity attributes of 5 and 16 m/s. It also includes the MP cluster used by the Hybrid A* method.

Figure 10

Comparison of MP cluster generation results

The offline generation result of the MP library demonstrated that, regardless of the type of vehicle platform, the number of MPs gradually decreased with an increase in velocity. This is because the vehicle can achieve more diversified steering movements at low speeds, but the number of MPs at high speeds is greatly reduced owing to the weakening of the steering ability. Because the MPs are defined in discrete velocity ranges, an additional velocity-planning algorithm needs to be introduced in the actual application process to achieve a smooth velocity level transition.

In addition, when the velocity is 5 m/s, the MP cluster composition is significantly different from the composition at other velocities. The number of behavior primitives, N_B, is reduced, while the number of general primitives, N_m, is significantly increased (including reverse primitives). When the vehicle speed is low, it is often necessary to address a more complicated environment, and general primitives can effectively improve the success rate of the planning algorithm in the corresponding situation.

Table 2 indicates that the average energy of the wheeled platform MPs in the low-speed range was lower than that of the tracked platform. Although the trajectory of the wheeled platform is smoother, and the adjustment of the course is gentle, the tracked platform has stronger and more aggressive steering adjustment ability at low speeds. In the high-speed range, the average energy of the MPs of the two platforms was basically the same, and the steering adjustment tended to be the same.

Hybrid A* utilized a circular arc generated by a fixed curvature as the basic MP. It realized neither the differentiation of the characteristics of heterogeneous platforms nor the separation of different speed intervals. In general, the types of MPs are relatively limited and do not have the diversity of MPs in the MP library proposed in this study.

5.2 Comparative Analysis of Online Selection of MPs in the Simulation Environment

To demonstrate the performance of the MP online selection algorithm proposed in this study, a low-speed scene and a high-speed scene were designed to evaluate the motion-planning algorithm. In each scene, the motion-planning results of Hybrid A*, DBTMP A*-P_a, and DBTMP A*-P_t were compared. The planning results were solved using a laptop Lenovo IdeaPad Y700-15ISK with 16 gigabytes of memory and an eight-core Intel Core i7-6700 operating at 2.6 GHz, running under 64-bit Ubuntu 16.04. The entire motion-planning algorithm was written in C++ in the ROS system.

Figure 11 shows the results of motion planning in low-speed scenes. Table 3 presents the comparison results of the corresponding evaluation indicators, including the average curve energy ${\bar{J}}_{s}$, total number of MP extensions, N_e, number of behavior primitives during extension, N_be, and the solution time of the corresponding motion-planning algorithm, T (ms).

Figure 11

Comparison of motion-planning results in low-speed scenes: (a) Comparison of the three motion-planning algorithms under the same initial and final state conditions (The blue and purple arrows indicate the position and heading of the initial and end conditions separately), (b)–(d) Details of the MP extension of the three motion-planning methods (The final selected single MP in the MP cluster is represented by a thick solid line)

Table 3 Comparison of MP online selection results in a low-speed scene

From the perspective of the smoothness of the motion-planning results, regardless of the platform used, the trajectory generated by the DBTMP A* algorithm is smoother. One of the reasons for this is that the overall MP library used by the DBTMP A* algorithm is smoother than that of the Hybrid A* method. Another reason is that the straight driving behavior primitives and general primitives use different weighting coefficients in the evaluation (as shown in Eq. (22)); therefore, the straight driving behavior primitives occupy a larger proportion of the entire trajectory, which makes the evaluation index of smoothness significantly lower than that of the Hybrid A* algorithm.

From the perspective of the extension times of MPs, the extension times of DBTMP A*-P_a and DBTMP A*-P_t were only 69.56% and 78.26%, respectively, of that of the Hybrid A* method. This is mainly because the introduction of behavior primitives increases the extension category of a single MP and improves its adaptability to the environment so that it can complete the trajectory-planning task with fewer MP combinations. This is most evident in the local scene of the parking scene. Regardless of the vehicle platform, the DBTMP A* algorithm used only one MP to complete the planning task, whereas Hybrid A* achieved the task by joining five independent MPs and did not highlight the characteristics of the two platforms.

From the perspective of the solution time of motion-planning tasks, the DBTMP A* algorithm only accounted for 35% of the overall solution time of the Hybrid A* algorithm. The main reason for this difference is that the MP sets used by the Hybrid A* algorithm are generated online based on the passable radius, while the MP sets selected by the DBTMP A* algorithm are generated offline.

The comparison results of motion planning in high-speed scenes are shown in Figure 12, and Table 4 presents the corresponding evaluation indicators.

Figure 12

Comparison of motion-planning results in high-speed scenes

Table 4 Comparison of MP online selection results in a high-speed scene

From the motion-planning results of Figure 12 and Table 4, one can observe that the algorithm proposed in this study took advantage of the DBTMP library to achieve lower curve energy, fewer extension times, shorter solution time, and more reasonable planning results. This is mainly because the DBTMP library restricts and distinguishes the MPs that can be selected according to the velocity attribute and introduces the standard lane-change driving behavior primitives in high-speed scenes as supplements. This enables the driving experience to be successfully integrated into the MP generation and selection process proposed in this study. Although the Hybrid A* algorithm can also plan the corresponding collision-free trajectory, it does not conform to the driver’s driving experience in high-speed scenes. The several imperfections of the Hybrid A* algorithm in high-speed scenes are mainly due to the composition of primitive clusters, which do not distinguish between high-speed and low-speed scenes.

5.3 Applicability of the Algorithm in a Real Environment

The performance of the proposed algorithm in a real environment was verified by using two vehicle platforms the wheeled Ackermann-steering platform shown in Figure 13(a) and the tracked skid-steering platform shown in Figure 13(b). The two platforms are equipped with the same equipment, including a RoboSense 32-lidar system and two RoboSense 16-lidar systems, a Simpak982 GNSS receiver, an FOSN2 inertial navigation system, an AVT-1290c camera, and two industrial personal computers.

Figure 13

Unmanned vehicle test platforms

Before the experiment, we obtained the environmental map required for the entire planning task using a multisource simultaneous localization and mapping algorithm. In the experiment, the fusion positioning system provided centimeter-level real-time positioning accuracy at a frequency of 50 Hz. Based on an a priori map, the environment perception equipment carried by the vehicle added real-time environmental information to the map at a frequency of 5 Hz. Model predictive control was selected as the vehicle motion control algorithm, and the comprehensive tracking error in the off-road environment was < 0.5 m.

The purpose of the experiment in real scenes is to test the performance and efficiency of the algorithm under complex environmental conditions. Figure 14 shows the comparison results of the motion-planning algorithm at two time points during unmanned driving. These two moments were the starting moment of planning and the trigger moment of re-planning after turning. Because the Hybrid A* algorithm only made extremely limited adaptability adjustments for the two platforms, only the wheeled Ackermann-steering platform was selected as the verification platform. Ti represents the initial moment of the planning, and Tr represents the moment of replanning. Figure 15 shows the efficiency indices of the three motion-planning algorithms. Where the single-step extension time refers to the time required to select the most appropriate MP from the MP library at each MP extension step.

Figure 14

Comparison results of the three motion-planning algorithms in real environments

Figure 15

Time consumption of single-step extension of MPs

Judging from the experimental results in real scenes, the algorithm proposed in this study can not only be applied to unmanned systems in real time but can also achieve higher efficiency and more reasonable planning results. The experimental conclusions in the real environment are consistent with the simulation results.

6 Conclusions

A unified motion-planning algorithm framework based on MP generation and selection for heterogeneous vehicle platforms is proposed.

• (1) An optimized generation method is applied to realize the offline construction of a heterogeneous platform MP library. The generated MP library not only distinguishes MPs according to the velocity attributes and platform characteristics, but it also introduces five types of behavior primitives to expand the types of MPs.

• (2) The layered unequal-weighted MP online selection framework makes full use of the characteristics of the platform-specific MP library generated offline. The velocity connection relationship of the MP set is limited, and the selection weight of the single extended behavior primitive and general primitive is distinguished.

• (3) The DBTMP A* motion-planning algorithm proposed in this study not only retains the strong environmental adaptability of the original Hybrid A* algorithms but also highlights the characteristic differences between heterogeneous vehicle platforms. Moreover, the proposed method effectively utilizes the driving experience to complete the reasonable guidance of the planning results, improving the trajectory smoothness and significantly reducing the required solution time.