# Path Planning for Autonomous Articulated Vehicle Based on Improved Goal-Directed Rapid-Exploring Random Tree

Authors: Tong Xu; Yang Xu; Dong Wang; Siwei Chen; Weigong Zhang; Lihang Feng
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

The special steering characteristics and task complexity of autonomous articulated vehicle (AAV) make it often require multiple forward and backward movements during autonomous driving. In this paper, we present a simple yet effective method, named head correction with fixed wheel position (HC-FWP), for the demand of multiple forward and backward movements. The goal-directed rapid-exploring random tree (GDRRT) algorithm is first used to search for a feasible path in the obstacle map, and then, the farthest node search (FNS) algorithm is applied to obtain a series of key nodes, on which HC-FWP is used to correct AAV heading angles. Simulation experiments with Dynapac CC6200 articulated road roller parameters show that the proposed improved goal-directed rapid-exploring random tree (IGDRRT), consisting of GDRRT, FNS, and HC-FWP, can search a feasible path on maps that require the AAV to move back and forth.


---

## Body

## 1. Introduction

Autonomous articulated vehicle (AAV) path planning is usually divided into two stages: global static path planning [1–4] and local dynamic path planning [5–8]. Global static indicates that the map is known and has no moving obstacles, and a feasible path without collisions can be designed at this stage. Although the vehicle may encounter moving obstacles while following the planned path, AAV will recognize moving obstacles through various types of sensors and stop to maintain a safe distance. Therefore, the core of the AAV path plan is to design a global static path that meets vehicle kinematics and dynamic constraints.Many graph-based search algorithms have been proposed for global static path planning. Artificial potential field, one type of graph-based search algorithms, treats each obstacle as a specific model, leading to high time complexity and poor real-time performance in a multiobstacle environment [9]. A ∗ and D ∗ ， the other type of graph-based search algorithms,are computationally expensive because the completeness and optimality are given priority [10]. The two types of algorithms mentioned above fail to address vehicle motion constraints, so new methods must be applied to AAV path planning.The random sampling strategy adopted by the rapidly-exploring random tree (RRT) has a wide search range, fast search speed and high computational efficiency, and there is no need to model obstacles during path planning. Therefore, it is often applied to path planning for multiple obstacle maps. However, the algorithm has high path cost and slow convergence. The goal-directed method proposed by Kuffner and LaValle [11] makes RRT branches move towards the target position with a certain probability, which greatly reduces its convergence speed. RRT∗ algorithm [12] reduces the path cost by searching for newly generated nodes and adjacent nodes. Based on [12], Nasir et al. [13] use triangle inequality for intelligent sampling and optimize the path through beacon nodes. In response to vehicle motion constraints, [14] designs a variety of kinematic constraint models, including cars, flat panel, and tricycle, and uses the proposed improved RRT algorithm for simulation verification. Du et al. [15] improves RRT cost function based on vehicle kinematic constraints and uses the pruning function and B-spline algorithm for subsequent processing. These documents address vehicle motion constraints but still fail to meet the demand of multiple forward and backward movements.The motivation for this paper comes from unmanned road construction. The trailer transports and drops the articulated road roller, a kind of AAV, near its construction area and where the roller is placed determines its starting state. To prepare the road construction, the end state of AAV generally locates on the extension of the first predetermined path. Figure1 shows a possible experimental scenario near the construction area.Figure 1
A possible experimental scenario near the construction area.Compared to the above-related work, the present paper addresses three main contributions that are given as follows:(1)
First, farthest node search (FNS) is proposed to reduce the computational complexity of GDRRT and select key nodes for subsequent processes.(2)
Second, head correction with fixed wheel position (HC-FWP) is proposed for the demand of multiple forward and backward movements.(3)
Third, IGDRRT, consisting of GDRRT, FNS, and HC-FWP, can search a feasible path on maps that require AAV to move back and forth globally.The rest of this paper is organized as follows. Section2 defines the AAV model. Section 3 introduces GDRRT and the proposed IGDRRT in detail. The experimental results are given in Section 4, and discussion and conclusion are summarized in Section 5.


## 2. Autonomous Articulated Vehicle Model

The autonomous articulated vehicle (AAV) model can be simplified with two wheels and an articulated frame, as shown in Figure2, and kinematic equation is expressed as [16, 17](1)x˙fy˙fθ˙fv˙fγ˙=vfcosθfvfsinθfvfsinγ+lrγ˙/lfcosγ+lr00+00010u0+00001u1,where xf and yf are the horizontal and vertical coordinates of the midpoint of the front axis, θf, v˙f, and γ˙ are the heading angle, AAV speed, and the articulated steering angle of the front axis, respectively, and derivatives represent changes in AAV motion over time. u0 and u1 are the acceleration and the rate of the change of the hinged steering angle.Figure 2
Autonomous articulated vehicle model.


## 3. Improved Goal-Directed RRT

### 3.1. Goal-Directed RRT

Goal-directed RRT, presented in [11], is a single query tree structure algorithm that moves towards the target position with a certain probability. The algorithm defines a directed graph G=V,E containing the vertex set V and edge set E. The vertex set stores the searched node position, and the edge set stores the index relationship between the vertices. Algorithm 1 shows that qrand moves towards the target position with a certain probability. In function FindQNear, Euclidean distance is selected as the cost function to pick the least cost qnear∈Qnear from V, and in function FindQNew, linear interpolation is adopted to detect obstacles between qnear and qrand. If the new node qnew is found, qnew and qnear,qnew are added to V and E , respectively, and qnew is added asa subset of Qnear in the next cycle.


### 3.2. Improved GDRRT

#### 3.2.1. Farthest Node Search

As shown in Figure3(a), the search path is V1 ⟶ V3 ⟶ V4 ⟶ V8 ⟶ V9 ⟶ V12 with the GDRRT algorithm. The numerical values next to the line segment represents GDRRT function cost, 3(a). Figure 3(b) shows the adjacency matrix calculated by the Dijkstra algorithm, and its time complexity is T(n) = O(n2).Figure 3
(a) Undirected graph network, where V1, V3, V4, V8, V9, and V12 are the preserved nodes determined by the GDRRT algorithm. (b) Adjacency matrix with preserved nodes updated by the Dijkstra algorithm.
(a)(b)To reduce the computational complexity, farthest node search (FNS) is proposed in this paper. Unlike the Dijkstra algorithm, FNS first connects the end node to postorder nodes in turn until the cost reaches infinity, and then the preorder node of the node with infinite cost, we call it the farthest node, is set as the key node; next, we view the key node as the end node and find the next key node until the starting node is reached, as shown in Figure4(a), and FNS time complexity is T(n) = O(n). Figure 4(b) details the process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key nodes confirmation.Figure 4
(a) Searched key nodes from the end node to the starting node by FNS. (b) The process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key node confirmation.
(a)(b)


#### 3.2.2. Head Correction with Fixed Wheel Position

In general, the key nodes cause the designed path to fail to reachC2 continuity, which makes it impossible for AAV to directly follow the planned path by the algorithm mentioned in Section 3.2.1. In view of this problem, this paper proposes an algorithm called head correction with fixed wheel position (HC-FWP), which includes head correction with fixed front wheel position (HC-FFWP) and head correction with fixed rear wheel position (HC-FRWP). Take HC-FFWP as an example, when the front wheel arrives at a certain key node, we make the head angle gradually approach the target angle at each key node. Figures 5 and 6 display the process of HC-FFWP and HC-FRWP, expressed by ForwardBackward and BackwardForward function in simplified pseudocode, respectively, and the pseudocode is shown in Algorithm 2.Algorithm 1: Goal-directed RRT (GDRRT).
Input:K∈ℕ,qinit∈Cfree,qgoal∈Qgoal∈CfreeOutput:GV⟵qinit,E⟵ϕ,G⟵V,Efork⟵1toKdoifrand<p1, thenqrand⟵qgoalelseqrand⟵RANDOMSTATEqnear⟵FindQNearqrand,Vqnew,success⟵FindQNewqnear,qrandifsuccess, thenV⟵V∪qnew,E⟵E∪qnear,qnewifqnew==qgoalorqgoalisontheedgethenifqnew∼=qgoal, thenV⟵V\qnew,V⟵V∪qgoalpath⟵TRUEG⟵V,EReturn G if path==TRUE;else failureAlgorithm 2: Head correction with fixed wheel position.
Input:θH,θT,θmaxerror,GOutput:GV⟵G.V,E⟵G.EΔθ⟵θH−θTifΔθ<=θthandΔθ>θmaxerror, thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π/2andΔθ>θth, thendoV⟵FowardBackwardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θthandΔθ>π/2, thendoV⟵BackwardFowardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θmaxerrorandΔθ>π−θth, thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewG⟵V,EReturn GFigure 5
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FFWP.
(a)(b)(c)(d)Figure 6
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FRWP.
(a)(b)(c)(d)


## 3.1. Goal-Directed RRT

Goal-directed RRT, presented in [11], is a single query tree structure algorithm that moves towards the target position with a certain probability. The algorithm defines a directed graph G=V,E containing the vertex set V and edge set E. The vertex set stores the searched node position, and the edge set stores the index relationship between the vertices. Algorithm 1 shows that qrand moves towards the target position with a certain probability. In function FindQNear, Euclidean distance is selected as the cost function to pick the least cost qnear∈Qnear from V, and in function FindQNew, linear interpolation is adopted to detect obstacles between qnear and qrand. If the new node qnew is found, qnew and qnear,qnew are added to V and E , respectively, and qnew is added asa subset of Qnear in the next cycle.


## 3.2. Improved GDRRT

### 3.2.1. Farthest Node Search

As shown in Figure3(a), the search path is V1 ⟶ V3 ⟶ V4 ⟶ V8 ⟶ V9 ⟶ V12 with the GDRRT algorithm. The numerical values next to the line segment represents GDRRT function cost, 3(a). Figure 3(b) shows the adjacency matrix calculated by the Dijkstra algorithm, and its time complexity is T(n) = O(n2).Figure 3
(a) Undirected graph network, where V1, V3, V4, V8, V9, and V12 are the preserved nodes determined by the GDRRT algorithm. (b) Adjacency matrix with preserved nodes updated by the Dijkstra algorithm.
(a)(b)To reduce the computational complexity, farthest node search (FNS) is proposed in this paper. Unlike the Dijkstra algorithm, FNS first connects the end node to postorder nodes in turn until the cost reaches infinity, and then the preorder node of the node with infinite cost, we call it the farthest node, is set as the key node; next, we view the key node as the end node and find the next key node until the starting node is reached, as shown in Figure4(a), and FNS time complexity is T(n) = O(n). Figure 4(b) details the process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key nodes confirmation.Figure 4
(a) Searched key nodes from the end node to the starting node by FNS. (b) The process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key node confirmation.
(a)(b)


### 3.2.2. Head Correction with Fixed Wheel Position

In general, the key nodes cause the designed path to fail to reachC2 continuity, which makes it impossible for AAV to directly follow the planned path by the algorithm mentioned in Section 3.2.1. In view of this problem, this paper proposes an algorithm called head correction with fixed wheel position (HC-FWP), which includes head correction with fixed front wheel position (HC-FFWP) and head correction with fixed rear wheel position (HC-FRWP). Take HC-FFWP as an example, when the front wheel arrives at a certain key node, we make the head angle gradually approach the target angle at each key node. Figures 5 and 6 display the process of HC-FFWP and HC-FRWP, expressed by ForwardBackward and BackwardForward function in simplified pseudocode, respectively, and the pseudocode is shown in Algorithm 2.Algorithm 1: Goal-directed RRT (GDRRT).
Input:K∈ℕ,qinit∈Cfree,qgoal∈Qgoal∈CfreeOutput:GV⟵qinit,E⟵ϕ,G⟵V,Efork⟵1toKdoifrand<p1, thenqrand⟵qgoalelseqrand⟵RANDOMSTATEqnear⟵FindQNearqrand,Vqnew,success⟵FindQNewqnear,qrandifsuccess, thenV⟵V∪qnew,E⟵E∪qnear,qnewifqnew==qgoalorqgoalisontheedgethenifqnew∼=qgoal, thenV⟵V\qnew,V⟵V∪qgoalpath⟵TRUEG⟵V,EReturn G if path==TRUE;else failureAlgorithm 2: Head correction with fixed wheel position.
Input:θH,θT,θmaxerror,GOutput:GV⟵G.V,E⟵G.EΔθ⟵θH−θTifΔθ<=θthandΔθ>θmaxerror, thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π/2andΔθ>θth, thendoV⟵FowardBackwardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θthandΔθ>π/2, thendoV⟵BackwardFowardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θmaxerrorandΔθ>π−θth, thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewG⟵V,EReturn GFigure 5
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FFWP.
(a)(b)(c)(d)Figure 6
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FRWP.
(a)(b)(c)(d)


## 3.2.1. Farthest Node Search

As shown in Figure3(a), the search path is V1 ⟶ V3 ⟶ V4 ⟶ V8 ⟶ V9 ⟶ V12 with the GDRRT algorithm. The numerical values next to the line segment represents GDRRT function cost, 3(a). Figure 3(b) shows the adjacency matrix calculated by the Dijkstra algorithm, and its time complexity is T(n) = O(n2).Figure 3
(a) Undirected graph network, where V1, V3, V4, V8, V9, and V12 are the preserved nodes determined by the GDRRT algorithm. (b) Adjacency matrix with preserved nodes updated by the Dijkstra algorithm.
(a)(b)To reduce the computational complexity, farthest node search (FNS) is proposed in this paper. Unlike the Dijkstra algorithm, FNS first connects the end node to postorder nodes in turn until the cost reaches infinity, and then the preorder node of the node with infinite cost, we call it the farthest node, is set as the key node; next, we view the key node as the end node and find the next key node until the starting node is reached, as shown in Figure4(a), and FNS time complexity is T(n) = O(n). Figure 4(b) details the process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key nodes confirmation.Figure 4
(a) Searched key nodes from the end node to the starting node by FNS. (b) The process of FNS with ① to ⑬, where ①, ④, ⑦, ⑪, and ⑬ are the process of key node confirmation.
(a)(b)


## 3.2.2. Head Correction with Fixed Wheel Position

In general, the key nodes cause the designed path to fail to reachC2 continuity, which makes it impossible for AAV to directly follow the planned path by the algorithm mentioned in Section 3.2.1. In view of this problem, this paper proposes an algorithm called head correction with fixed wheel position (HC-FWP), which includes head correction with fixed front wheel position (HC-FFWP) and head correction with fixed rear wheel position (HC-FRWP). Take HC-FFWP as an example, when the front wheel arrives at a certain key node, we make the head angle gradually approach the target angle at each key node. Figures 5 and 6 display the process of HC-FFWP and HC-FRWP, expressed by ForwardBackward and BackwardForward function in simplified pseudocode, respectively, and the pseudocode is shown in Algorithm 2.Algorithm 1: Goal-directed RRT (GDRRT).
Input:K∈ℕ,qinit∈Cfree,qgoal∈Qgoal∈CfreeOutput:GV⟵qinit,E⟵ϕ,G⟵V,Efork⟵1toKdoifrand<p1, thenqrand⟵qgoalelseqrand⟵RANDOMSTATEqnear⟵FindQNearqrand,Vqnew,success⟵FindQNewqnear,qrandifsuccess, thenV⟵V∪qnew,E⟵E∪qnear,qnewifqnew==qgoalorqgoalisontheedgethenifqnew∼=qgoal, thenV⟵V\qnew,V⟵V∪qgoalpath⟵TRUEG⟵V,EReturn G if path==TRUE;else failureAlgorithm 2: Head correction with fixed wheel position.
Input:θH,θT,θmaxerror,GOutput:GV⟵G.V,E⟵G.EΔθ⟵θH−θTifΔθ<=θthandΔθ>θmaxerror, thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π/2andΔθ>θth, thendoV⟵FowardBackwardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes1ΔθV⟵FowardBackwardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θthandΔθ>π/2, thendoV⟵BackwardFowardV,Δθ,Times1andE⟵E∪qend,qnewuntilΔθ<=θth,thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewelse ifΔθ<=π−θmaxerrorandΔθ>π−θth, thenTimes2⟵DeltatoTimes2ΔθV⟵BackwardFowardV,Δθ,Times2E⟵E∪qend,qnewG⟵V,EReturn GFigure 5
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FFWP.
(a)(b)(c)(d)Figure 6
AAV position changes from (a) state 1 to 2, (b) state 2 to 3, (c) state 3 to 4, and (d) display of HC-FRWP.
(a)(b)(c)(d)


## 4. Experiments

### 4.1. The Experimental Scenarios and Parameter Settings

The 100 m × 100 m experimental scenario near the construction area is used to verify the effectiveness of the proposed algorithm, as shown in Figures7(a)–7(d), where gray areas indicate obstacles, blue stars and circles indicate AAV front and rear wheels at the starting position, and the black star and circle indicate AAV front and rear wheels at the end position, respectively.Figure 7
Experimental scenario near the construction area.The Dynapac CC6200 articulated road roller, a type of AAV, is selected for simulation experiment, and its parameters are set in Table1. Table 2 shows IGDRRT parameter settings.Table 1
Dynapac CC6200 articulated road roller parameter settings.
Simulation parametersValueCompaction width of the steel wheelB/m2.13Distance from hinge point to front axle midpointlf/m1.845Distance from hinge point to rear axle midpointlr/m1.845Maximum speedvf/m⋅s−22Sampling timeΔt/s0.1Maximum steering angleγmax/°14Maximum accelerationu0/m⋅s−21The gradient of articulated steering angleu1/°⋅s−110Table 2
IGDRRT parameter settings.
Simulation parametersValueSearch stepΔq/m5AAV maximum curvature/m−10.0661Steptime120θth/°15θmaxerror/°1


### 4.2. Experimental Results

#### 4.2.1. Farthest Node Search

GDRRT regards AAV as a single point for path search. As shown in Figure8, this algorithm searches for the path in the fixed-size step from the starting point marked by the dark blue star, the path and single-point position are marked with blue line segments and cyan stars, respectively, and the final path is shown in red. The path optimized by FNS is drawn as a black line. Compared with the original path generated by the traditional GDRRT, the path length is shortened and the number of key nodes is also reduced, which reduces the operation of subsequent algorithms.Figure 8
GDRRT with farthest node search.


#### 4.2.2. Head Correction with Fixed Front Wheel Position

Figure9 shows one-time head correction with fixed front wheel position implementation, where the dark blue and red rhombus indicate AAV front and rear wheel position, respectively, the black curve and the cyan curve are AAV trajectories. From AB to EF, the coordinates of AAV front wheel hardly change,but the heading changes to some extent.Figure 9
One-time HC-FFWP implementation.The DeltatoTimes mapping matrix described in Algorithm2 is designed depending on multiple experiments at a fixed-size sampling time and AAV speed, the mapping matrices, DeltatoTimes1 and DeltatoTimes2, decribe the relationship between the articulated steering angle and times, depicted in Tables 3 and 4, respectively.Table 3
DeltatoTimes1 mapping matrix.
Δθ/°12345678Times134578911Δθ/°9101112131415Times12141516171820Table 4
DeltatoTimes2 mapping matrix.
Δθ/°179178177176175Times13457Δθ/°174173172171170Times89111214Δθ/°169168167166165Times1516171820


#### 4.2.3. IGDRRT with HC-FFWP under Different Goal-Directed Probabilities

In the existing documents, goal-directed probability is generally set between 0.01 and 0.3. In order to explore the impact of probability values on the proposed algorithm, we choose four different goal-directed probability values of 0.01, 0.1, 0.2, and 0.3, respectively. Figure10 shows the experiment results intuitively, and Table 5 indicates that the change in goal-directed probability parameter does not have a great impact on algorithm processing time.Figure 10
IGDRRT with goal-directed probability of 0.01(a) (b), 0.1(c) (d), 0.2(e) (f), and 0.3(g) (h), respectively.
(a)(b)(c)(d)(e)(f)(g)(h)Table 5
Algorithm processing time with 95% confidence interval under 20 different goal-directed probabilities.
Goal-directed probabilityNumber of case (a)/(c)/(e)/(g)95% confidence interval for case (a)/(c)/(e)/(g)Number of case (b)/(d)/(f)/(h)95% confidence interval for case (b)/(d)/(f)/(h)0.01 (a) (b)10(19.0572s, 23.8877s)10(19.8248s, 28.1721s)0.1 (c) (d)12(21.0483s, 26.0185s)8(17.0006s, 26.0764s)0.2 (e) (f)16(18.3258s, 24.2589s)4(24.5175s, 32.4981s)0.3 (g) (h)16(19.6846s, 26.7702s)4(17.9538s, 23.5815s)


#### 4.2.4. Optimization of Head Correction

Although the designed algorithm has acceptable processing time, the head correction strategy of some key nodes can still be optimized, as shown in Figure10(h); the body of AAV turns almost 180° in the starting node, which is not in line with general driving behavior. Since AAV uses the same steering strategy when moving forward and backward, it may be a better way to use both the front and the rear wheel to correct the course. Figure 11(a) shows the heading correction for fixed front wheel only, and Figure 11(b) shows the heading correction using both the front and the rear wheel, and experiments intuitively show that the method used in the latter experiments can be significantly improved in some cases.Figure 11
The comparison between (a) HC-FFWP and (b) HC-FWP.
(a)(b)


#### 4.2.5. IGDRRT with HC-FWP Using the Experimental Scenario near the Construction Area

The experimental scenario near the construction area, shown in Figure7, is designed for simulation. As shown in Figure 12, the UAV final state of the following four cases is all set to [10, 90] with 90° heading angle, and the AAV initial state is [25, 20] with 135° heading angle (a), [50, 20] with 90° heading angle (b), [80, 20] with 45° heading angle (c), and [80, 80] with 180° heading angle (d), respectively.Figure 12
(a) AAV final state set to [10, 90] with 90° heading angle and AAV initial state set to [25, 20] with 135° heading angle, (b) [50, 20] with 90° heading angle, (c) [80, 20] with 45° heading angle, and (d) [80, 80] with 180° heading angle, respectively.
(a)(b)(c)(d)Table6 shows processing time of the four algorithms with 95% confidence interval under 20 experiments. Due to the single-point model of the traditional GDRRT algorithm, AAV cannot search for a feasible path. The time complexity of GDRRT with HC-FWP using the Dijkstra algorithm consumes relatively much time. Although the algorithm mentioned in [16] and the algorithm proposed in this paper take almost the same time, the algorithm presented in [16] requires a few minutes of preprocessing under each different initial state to determine the model parameters. Therefore, the algorithm proposed in this paper is significantly less time consuming than the other three algorithms using 95% confidence interval.Table 6
95% confidence interval for four algorithms.
AlgorithmsInitial states in Figure7Algorithm processing timeTraditional GDRRT algorithm(a)–(d)UnreachableGDRRT with HC-FWP using Dijkstra algorithm(a)(4.5762s, 5.8423s)(b)(6.3485s, 7.4384s)(c)(6.9432s, 8.1357s)(d)(4.9132s, 6.0157s)Document [16](a)(1.4571s, 1.5831s)(b)(2.3578s, 2.5439s)(c)(2.7936s, 2.9128s)(d)(1.6257s, 1.6527s)The proposed algorithm(a)(1.2559s, 1.7180s)(b)(2.1972s, 3.0762s)(c)(2.4013s, 3.1109s)(d)(1.5985s, 1.7127s)


## 4.1. The Experimental Scenarios and Parameter Settings

The 100 m × 100 m experimental scenario near the construction area is used to verify the effectiveness of the proposed algorithm, as shown in Figures7(a)–7(d), where gray areas indicate obstacles, blue stars and circles indicate AAV front and rear wheels at the starting position, and the black star and circle indicate AAV front and rear wheels at the end position, respectively.Figure 7
Experimental scenario near the construction area.The Dynapac CC6200 articulated road roller, a type of AAV, is selected for simulation experiment, and its parameters are set in Table1. Table 2 shows IGDRRT parameter settings.Table 1
Dynapac CC6200 articulated road roller parameter settings.
Simulation parametersValueCompaction width of the steel wheelB/m2.13Distance from hinge point to front axle midpointlf/m1.845Distance from hinge point to rear axle midpointlr/m1.845Maximum speedvf/m⋅s−22Sampling timeΔt/s0.1Maximum steering angleγmax/°14Maximum accelerationu0/m⋅s−21The gradient of articulated steering angleu1/°⋅s−110Table 2
IGDRRT parameter settings.
Simulation parametersValueSearch stepΔq/m5AAV maximum curvature/m−10.0661Steptime120θth/°15θmaxerror/°1


## 4.2. Experimental Results

### 4.2.1. Farthest Node Search

GDRRT regards AAV as a single point for path search. As shown in Figure8, this algorithm searches for the path in the fixed-size step from the starting point marked by the dark blue star, the path and single-point position are marked with blue line segments and cyan stars, respectively, and the final path is shown in red. The path optimized by FNS is drawn as a black line. Compared with the original path generated by the traditional GDRRT, the path length is shortened and the number of key nodes is also reduced, which reduces the operation of subsequent algorithms.Figure 8
GDRRT with farthest node search.


### 4.2.2. Head Correction with Fixed Front Wheel Position

Figure9 shows one-time head correction with fixed front wheel position implementation, where the dark blue and red rhombus indicate AAV front and rear wheel position, respectively, the black curve and the cyan curve are AAV trajectories. From AB to EF, the coordinates of AAV front wheel hardly change,but the heading changes to some extent.Figure 9
One-time HC-FFWP implementation.The DeltatoTimes mapping matrix described in Algorithm2 is designed depending on multiple experiments at a fixed-size sampling time and AAV speed, the mapping matrices, DeltatoTimes1 and DeltatoTimes2, decribe the relationship between the articulated steering angle and times, depicted in Tables 3 and 4, respectively.Table 3
DeltatoTimes1 mapping matrix.
Δθ/°12345678Times134578911Δθ/°9101112131415Times12141516171820Table 4
DeltatoTimes2 mapping matrix.
Δθ/°179178177176175Times13457Δθ/°174173172171170Times89111214Δθ/°169168167166165Times1516171820


### 4.2.3. IGDRRT with HC-FFWP under Different Goal-Directed Probabilities

In the existing documents, goal-directed probability is generally set between 0.01 and 0.3. In order to explore the impact of probability values on the proposed algorithm, we choose four different goal-directed probability values of 0.01, 0.1, 0.2, and 0.3, respectively. Figure10 shows the experiment results intuitively, and Table 5 indicates that the change in goal-directed probability parameter does not have a great impact on algorithm processing time.Figure 10
IGDRRT with goal-directed probability of 0.01(a) (b), 0.1(c) (d), 0.2(e) (f), and 0.3(g) (h), respectively.
(a)(b)(c)(d)(e)(f)(g)(h)Table 5
Algorithm processing time with 95% confidence interval under 20 different goal-directed probabilities.
Goal-directed probabilityNumber of case (a)/(c)/(e)/(g)95% confidence interval for case (a)/(c)/(e)/(g)Number of case (b)/(d)/(f)/(h)95% confidence interval for case (b)/(d)/(f)/(h)0.01 (a) (b)10(19.0572s, 23.8877s)10(19.8248s, 28.1721s)0.1 (c) (d)12(21.0483s, 26.0185s)8(17.0006s, 26.0764s)0.2 (e) (f)16(18.3258s, 24.2589s)4(24.5175s, 32.4981s)0.3 (g) (h)16(19.6846s, 26.7702s)4(17.9538s, 23.5815s)


### 4.2.4. Optimization of Head Correction

Although the designed algorithm has acceptable processing time, the head correction strategy of some key nodes can still be optimized, as shown in Figure10(h); the body of AAV turns almost 180° in the starting node, which is not in line with general driving behavior. Since AAV uses the same steering strategy when moving forward and backward, it may be a better way to use both the front and the rear wheel to correct the course. Figure 11(a) shows the heading correction for fixed front wheel only, and Figure 11(b) shows the heading correction using both the front and the rear wheel, and experiments intuitively show that the method used in the latter experiments can be significantly improved in some cases.Figure 11
The comparison between (a) HC-FFWP and (b) HC-FWP.
(a)(b)


### 4.2.5. IGDRRT with HC-FWP Using the Experimental Scenario near the Construction Area

The experimental scenario near the construction area, shown in Figure7, is designed for simulation. As shown in Figure 12, the UAV final state of the following four cases is all set to [10, 90] with 90° heading angle, and the AAV initial state is [25, 20] with 135° heading angle (a), [50, 20] with 90° heading angle (b), [80, 20] with 45° heading angle (c), and [80, 80] with 180° heading angle (d), respectively.Figure 12
(a) AAV final state set to [10, 90] with 90° heading angle and AAV initial state set to [25, 20] with 135° heading angle, (b) [50, 20] with 90° heading angle, (c) [80, 20] with 45° heading angle, and (d) [80, 80] with 180° heading angle, respectively.
(a)(b)(c)(d)Table6 shows processing time of the four algorithms with 95% confidence interval under 20 experiments. Due to the single-point model of the traditional GDRRT algorithm, AAV cannot search for a feasible path. The time complexity of GDRRT with HC-FWP using the Dijkstra algorithm consumes relatively much time. Although the algorithm mentioned in [16] and the algorithm proposed in this paper take almost the same time, the algorithm presented in [16] requires a few minutes of preprocessing under each different initial state to determine the model parameters. Therefore, the algorithm proposed in this paper is significantly less time consuming than the other three algorithms using 95% confidence interval.Table 6
95% confidence interval for four algorithms.
AlgorithmsInitial states in Figure7Algorithm processing timeTraditional GDRRT algorithm(a)–(d)UnreachableGDRRT with HC-FWP using Dijkstra algorithm(a)(4.5762s, 5.8423s)(b)(6.3485s, 7.4384s)(c)(6.9432s, 8.1357s)(d)(4.9132s, 6.0157s)Document [16](a)(1.4571s, 1.5831s)(b)(2.3578s, 2.5439s)(c)(2.7936s, 2.9128s)(d)(1.6257s, 1.6527s)The proposed algorithm(a)(1.2559s, 1.7180s)(b)(2.1972s, 3.0762s)(c)(2.4013s, 3.1109s)(d)(1.5985s, 1.7127s)


## 4.2.1. Farthest Node Search

GDRRT regards AAV as a single point for path search. As shown in Figure8, this algorithm searches for the path in the fixed-size step from the starting point marked by the dark blue star, the path and single-point position are marked with blue line segments and cyan stars, respectively, and the final path is shown in red. The path optimized by FNS is drawn as a black line. Compared with the original path generated by the traditional GDRRT, the path length is shortened and the number of key nodes is also reduced, which reduces the operation of subsequent algorithms.Figure 8
GDRRT with farthest node search.


## 4.2.2. Head Correction with Fixed Front Wheel Position

Figure9 shows one-time head correction with fixed front wheel position implementation, where the dark blue and red rhombus indicate AAV front and rear wheel position, respectively, the black curve and the cyan curve are AAV trajectories. From AB to EF, the coordinates of AAV front wheel hardly change,but the heading changes to some extent.Figure 9
One-time HC-FFWP implementation.The DeltatoTimes mapping matrix described in Algorithm2 is designed depending on multiple experiments at a fixed-size sampling time and AAV speed, the mapping matrices, DeltatoTimes1 and DeltatoTimes2, decribe the relationship between the articulated steering angle and times, depicted in Tables 3 and 4, respectively.Table 3
DeltatoTimes1 mapping matrix.
Δθ/°12345678Times134578911Δθ/°9101112131415Times12141516171820Table 4
DeltatoTimes2 mapping matrix.
Δθ/°179178177176175Times13457Δθ/°174173172171170Times89111214Δθ/°169168167166165Times1516171820


## 4.2.3. IGDRRT with HC-FFWP under Different Goal-Directed Probabilities

In the existing documents, goal-directed probability is generally set between 0.01 and 0.3. In order to explore the impact of probability values on the proposed algorithm, we choose four different goal-directed probability values of 0.01, 0.1, 0.2, and 0.3, respectively. Figure10 shows the experiment results intuitively, and Table 5 indicates that the change in goal-directed probability parameter does not have a great impact on algorithm processing time.Figure 10
IGDRRT with goal-directed probability of 0.01(a) (b), 0.1(c) (d), 0.2(e) (f), and 0.3(g) (h), respectively.
(a)(b)(c)(d)(e)(f)(g)(h)Table 5
Algorithm processing time with 95% confidence interval under 20 different goal-directed probabilities.
Goal-directed probabilityNumber of case (a)/(c)/(e)/(g)95% confidence interval for case (a)/(c)/(e)/(g)Number of case (b)/(d)/(f)/(h)95% confidence interval for case (b)/(d)/(f)/(h)0.01 (a) (b)10(19.0572s, 23.8877s)10(19.8248s, 28.1721s)0.1 (c) (d)12(21.0483s, 26.0185s)8(17.0006s, 26.0764s)0.2 (e) (f)16(18.3258s, 24.2589s)4(24.5175s, 32.4981s)0.3 (g) (h)16(19.6846s, 26.7702s)4(17.9538s, 23.5815s)


## 4.2.4. Optimization of Head Correction

Although the designed algorithm has acceptable processing time, the head correction strategy of some key nodes can still be optimized, as shown in Figure10(h); the body of AAV turns almost 180° in the starting node, which is not in line with general driving behavior. Since AAV uses the same steering strategy when moving forward and backward, it may be a better way to use both the front and the rear wheel to correct the course. Figure 11(a) shows the heading correction for fixed front wheel only, and Figure 11(b) shows the heading correction using both the front and the rear wheel, and experiments intuitively show that the method used in the latter experiments can be significantly improved in some cases.Figure 11
The comparison between (a) HC-FFWP and (b) HC-FWP.
(a)(b)


## 4.2.5. IGDRRT with HC-FWP Using the Experimental Scenario near the Construction Area

The experimental scenario near the construction area, shown in Figure7, is designed for simulation. As shown in Figure 12, the UAV final state of the following four cases is all set to [10, 90] with 90° heading angle, and the AAV initial state is [25, 20] with 135° heading angle (a), [50, 20] with 90° heading angle (b), [80, 20] with 45° heading angle (c), and [80, 80] with 180° heading angle (d), respectively.Figure 12
(a) AAV final state set to [10, 90] with 90° heading angle and AAV initial state set to [25, 20] with 135° heading angle, (b) [50, 20] with 90° heading angle, (c) [80, 20] with 45° heading angle, and (d) [80, 80] with 180° heading angle, respectively.
(a)(b)(c)(d)Table6 shows processing time of the four algorithms with 95% confidence interval under 20 experiments. Due to the single-point model of the traditional GDRRT algorithm, AAV cannot search for a feasible path. The time complexity of GDRRT with HC-FWP using the Dijkstra algorithm consumes relatively much time. Although the algorithm mentioned in [16] and the algorithm proposed in this paper take almost the same time, the algorithm presented in [16] requires a few minutes of preprocessing under each different initial state to determine the model parameters. Therefore, the algorithm proposed in this paper is significantly less time consuming than the other three algorithms using 95% confidence interval.Table 6
95% confidence interval for four algorithms.
AlgorithmsInitial states in Figure7Algorithm processing timeTraditional GDRRT algorithm(a)–(d)UnreachableGDRRT with HC-FWP using Dijkstra algorithm(a)(4.5762s, 5.8423s)(b)(6.3485s, 7.4384s)(c)(6.9432s, 8.1357s)(d)(4.9132s, 6.0157s)Document [16](a)(1.4571s, 1.5831s)(b)(2.3578s, 2.5439s)(c)(2.7936s, 2.9128s)(d)(1.6257s, 1.6527s)The proposed algorithm(a)(1.2559s, 1.7180s)(b)(2.1972s, 3.0762s)(c)(2.4013s, 3.1109s)(d)(1.5985s, 1.7127s)


## 5. Discussion and Conclusion

Most traditional path planning algorithms do not considerC2 continuity, so AAV cannot track the path planned by these algorithms. In recent years, some path planning algorithms for the car model are proposed, but the special steering characteristics and task complexity of AAV make it often require multiple forward and backward movements, and the algorithm designed for the car model fails to meet the demand. In regard to this, we present an algorithm called IGDRRT, which introduces the FNS optimization algorithm with T(n) = O(n) time complexity on the basis of GDRRT, changes the single-point model of GDRRT to the two-point model, and incorporates the HC-FWP algorithm to meet the needs of forward and backward movements. Simulation results show that the proposed IGDRRT can search a feasible path on maps that require AAV to move back and forth.Other application scenarios of the presented algorithm including articulated shovel trucks for earthquake rescue and articulated mining trucks for transport.



---

*Source: 7123164-2020-05-20.xml*