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

.e 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 ofmultiple forward and backwardmovements..e 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, andHC-FWP, can search a feasible path onmaps that require the AAV tomove back and forth.


Introduction
Autonomous articulated vehicle (AAV) path planning is usually divided into two stages: global static path planning [1][2][3][4] and local dynamic path planning [5][6][7][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. erefore, 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]. e two types of algorithms mentioned above fail to address vehicle motion constraints, so new methods must be applied to AAV path planning. e random sampling strategy adopted by the rapidlyexploring 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. erefore, it is often applied to path planning for multiple obstacle maps. However, the algorithm has high path cost and slow convergence. e 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. ese documents address vehicle motion constraints but still fail to meet the demand of multiple forward and backward movements. e motivation for this paper comes from unmanned road construction. e 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. Figure 1 shows 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) ird, IGDRRT, consisting of GDRRT, FNS, and HC-FWP, can search a feasible path on maps that require AAV to move back and forth globally. e rest of this paper is organized as follows. Section 2 defines the AAV model. Section 3 introduces GDRRT and the proposed IGDRRT in detail. e experimental results are given in Section 4, and discussion and conclusion are summarized in Section 5.

Autonomous Articulated Vehicle Model
e autonomous articulated vehicle (AAV) model can be simplified with two wheels and an articulated frame, as shown in Figure 2, and kinematic equation is expressed as [16,17] where x f and y f are the horizontal and vertical coordinates of the midpoint of the front axis, θ f , _ v f , and _ c 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. u 0 and u 1 are the acceleration and the rate of the change of the hinged steering angle.

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. e algorithm defines a directed graph G � 〈V, E〉 containing the vertex set V and edge set E. e vertex set stores the searched node position, and the edge set stores the index relationship between the vertices. Algorithm 1 shows that q ran d 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 q near ∈ Q near from V, and in function FindQNew, linear interpolation is adopted to detect obstacles between q near ∞ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * and q rand . If the new node q new is found, q new and q near , q new are added to V and E , respectively, and q new is added asa subset of Q near in the next cycle. Figure 3

Farthest Node Search. As shown in
with the GDRRTalgorithm. e 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(n 2 ).
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 Figure 4(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.

Head Correction with Fixed Wheel Position.
In general, the key nodes cause the designed path to fail to reach C 2 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         Mathematical Problems in Engineering 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.

e Experimental Scenarios and Parameter
Settings. e 100 m × 100 m experimental scenario near the construction area is used to verify the effectiveness of the proposed algorithm, as shown in Figures 7(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. e Dynapac CC6200 articulated road roller, a type of AAV, is selected for simulation experiment, and its parameters are set in Table 1. Table 2 shows IGDRRT parameter settings.

Farthest Node
Search. GDRRT regards AAV as a single point for path search. As shown in Figure 8, 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. e 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 9 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. e DeltatoTimes mapping matrix described in Algorithm 2 is designed depending on multiple experiments at a fixed-size sampling time and AAV speed, the mapping   Tables 3 and 4, respectively.

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. Figure 10 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.

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 Figure 10(h); the body of AAV turns almost 180°i n 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.

IGDRRT with HC-FWP Using the Experimental Scenario near the Construction Area.
e experimental scenario near the construction area, shown in Figure 7, is designed for simulation. As shown in Figure 12, the UAV final state of the following four cases is all set to [10,90] Table 6 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. e 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. erefore, the algorithm proposed in this paper is significantly less time consuming than the other three algorithms using 95% confidence interval.

Discussion and Conclusion
Most traditional path planning algorithms do not consider C 2 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.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that they have no conflicts of interest.