A Hybrid Path Planning Method Based on Articulated Vehicle Model

: Due to the unique steering mechanism and driving characteristics of the articulated vehicle, a hybrid path planning method based on the articulated vehicle model is proposed to meet the demand of obstacle avoidance and searching the path back and forth of the articulated vehicle. First, Support Vector Machine (SVM) theory is used to obtain the two-dimensional optimal zero potential curve and the maximum margin, and then, several key points are selected from the optimal zero potential curves by using Longest Accessible Path (LAP) method. Next, the Cubic Bezier (CB) curve is adopted to connect the curve that satisfies the curvature constraint of the articulated vehicle between every two key points. Finally, Back and Forth Rapidly-exploring Random Tree with Course Correction (BFRRT-CC) is designed to connect paths that do not meet articulated vehicle curvature requirements. Simulation results show that the proposed hybrid path planning method can search a feasible path with a 90-degree turn, which meets the demand for obstacle avoidance and articulated vehicle back-and-forth movement.


Introduction
In the past 50 years, path planning technology has developed rapidly, and many planning algorithms have emerged, which include the Genetic Algorithm (GA), Simulated Annealing Algorithm (SAA), Ant Colony Optimization (ACO), and so on. These algorithms have their superiority in solving the problem of the general path planning problems. However, they need to build each obstacle into a specific model, which leads to executable algorithms with high time complexity and does not apply to many obstacles or uneven distribution of a sophisticated environment. Besides, graph search algorithms like A* algorithm, D* algorithm, and Artificial Potential Field (APF) algorithm, although they meet the requirements of optimality and real-time performance in path planning, it does not meet the vehicle kinematic constraints, which makes it impossible for the vehicle to track the planned path. Scholars at home and abroad put forward many traditional algorithms for vehicle path planning. Some researchers present vehicle motion planning with RRT-based theory ; Karaman, Walter, Perez et al. (2011); Kuwata, Teo, Fiore et al. (2009)], while others come up with algorithms based on one or several curves, such as Bezier curve [Jolly, Kumar, Vijayakumar et al. (2009)], B-spline [Fauser, Chadda, Goergen et al. (2019); Wan, Xu, Ye et al. (2018)] and Dubins curve [Héerissé and Pepy (2013)], which all have good effects on their vehicle models. In recent years, a novel path planning algorithm using SVM is proposed. As a classic theory in the field of machine learning, this algorithm has great progress in reducing the search space, and sometimes can directly plan an excellent path [Chen, Jiang, Zhao et al. (2017)]. In this paper, we propose a hybrid path planning method using SVM, CB curve and BFRRT-CC. First, SVM theory is used to obtain the two-dimensional optimal zero potential curve and the maximum margin, and then, several key points are selected from the optimal zero potential curve by using LAP. Next, we use CB curve to connect the curve that satisfies the curvature constraint of the articulated vehicle between every two key points. Finally, BFRRT-CC is designed to connect paths that does not meet articulated vehicle curvature requirements [Xia, Hu, Luo et al. (2017)].

Nonholonomic constraint articulated vehicle model
The concept of "incomplete" comes from modern analytical mechanics, which first appeared in the scholarly work 'Die Prinzipen der Mechanik' by a German scholar. The non-holonomic constraint refers to the constraint that contains the generalized coordinate derivative of the system and is non-integrable, and expression of the constrained system subject to Eqs. (1) and (2) (2) where, n s ∈  is system's state variables, n u ∈  is system control variables, t is time variable, C( , d /d , ) s s t t is the vehicle constraint. The holonomic system can be expressed as dG( , )/d C ( , d /d , ) s t t s s t t = , if G( , ) s t exists, the corresponding constraint is holonomic constraint. On the contrary, we call it nonholonomic constraint. Nonholonomic constraint system includes all kinds of vehicles and mobile robots with limited movement. Therefore, researches on path planning under nonholonomic constraint has a broad application and important application value. In this paper, the articulated vehicle model is simplified as two wheels and an articulated frame, as shown in Fig. 1. Take the forward motion as an example, we view the middle point of the front bridge f P as the reference point of the roller, and view the velocity direction of this point f v as the forward direction of the roller , and the coordinate change of the middle point of the front bridge can be expressed as Eqs.
(3) and (4): where f v is the speed of the middle point of the front bridge, f x and f y are the horizontal and vertical coordinates of the midpoint of the front bridge in the coordinate system separately, f θ is the course angle of the front bridge, and the gradient of the course angle can be expressed as Eq. (5): where f l is the distance from the front bridge to the hinge point, r l is the distance from the rear bridge to the hinge point and γ is the articulated steering angle, define the right as γ + and the left asγ when the roller moves forward, and define the right asγ and the left as γ + when the roller moves backward.
where the control variable 0 u is acceleration and the control variable 1 u is the rate of the change of hinged steering angle. These constrain conditions should be satisfied: 0 0 max The maximum curvature is expressed as Eq. (7): 3 The hybrid path planning method The improved algorithms are presented in detail in this part, and it mainly divides into four sections. Section 3.1 briefly introduces the principle of SVM and the way to obtain SVM model parameters. Section 3.2 introduces how to search key points with LAP theory. In Section 3.3, a suitable CB curve is selected to connect the curve segment that satisfies the curvature constraint of the articulated vehicle between every two key points. In Section 3.4, BFRRT-CC is designed to connect paths that does not meet articulated vehicle curvature requirements.

Optimal zero-potential decision boundary and free space extraction based on SVM theory
SVM is a binary model and the idea of the model is to map vectors to a higher dimensional space and establish an optimal classification hyper-plane in this space, which maximizes margin between two classes [Chen, Xiong, Xu et al. (2019)].
Since the state of start and end points including articulated vehicle position and course is given initially, in order to meet the actual needs, we add three positive virtual obstacles and three negative ones on both sides, and extract optimal zero-potential decision boundary and obstacle-free space based on SVM theory [Yuan, Yao, Tan et al. (2018); Tang is the normal vector, which determines the direction of the hyper-plane, b is displacement, which indicates the distance between the hyper-plane and the origin, n is the number of support vector, i ξ are the slack variables, c is the error penalty factor, 2 ( ) : H p φ ℜ → ℜ maps data points from two-dimensional spatial to highdimensional reproducing kernel Hilbert space, to separate the data set linearly. Through the Lagrange multiplier method, we can get the dual problem in Eq. (9) of the Eq. (8). According to the basic theory of SVM, the error penalty factor and the kernel parameter play a crucial role. This paper adopts 10-fold cross-validation to find the optimal parameters.

Key points searching with longest accessible path theory
After drawing the zero-potential decision boundary on the two dimensional map, we need to find several key points and three key points are selected, as shown in Fig. 2. We define the connection between two points as the path, and when the path does not go through any obstacles, we call it an accessible path, otherwise it is termed as an illegal path. Then try to connect the latest point with the starting point and so on, and find the longest accessible path, and that point is set to a key point and so on until the end point is connected.
The course and position of the start and the end points are shown in the Eq. (14), and the position of the other two vertices are determined by the Eq. (15): where 1 step and 2 step are the given constant value, and we use the linear interpolation to test whether the generated curve can avoid obstacles. Curvature at any point on the curve is calculated as Eq. (16): , and meet requirements of τ(0)= init q and τ(1)= goal q .

Back and forth RRT
Vehicle motion planning with RRT-based theory mentioned in the latest documents does not consider the demand of articulated vehicle back-and-forth movement, this paper puts forward BFRRT algorithm and its cost function can be expressed as Eq. (17): where E  is normalized euclidean distance, C  is normalized course metric, and the expressions are Eqs. (18) and (19): The Min-Max Normalization is used to realize the normalization, which is expressed as Eqs. (20) and (21): The algorithm proposed is shown in Algorithm 1 in detail.

Course correction
In general, a feasible path cannot be searched only by using BFRRT theory, in which case CC with with fixed position of the front wheel is presented to deal with it [Wang, Willianms, Angley et al. (2019)]. CC algorithm is shown in Algorithm 2 in detail. Calculate the absolute value of the difference between the target angle arg t et θ and the roller's course angle real θ at the position to be corrected, and judge the relationship between that angular deviation | | θ ∆ and preset

The experimental scene and parameters of the articulated vehicle
As shown in Fig. 3, the start and end point of the articulated vehicle front wheel is [20,85] with 0° and [85, 20] with -90 o respectively, and the shaded part indicates the obstacle. Parameters of the articulated vehicle are used in this experiment, shown in Tab. 1 (i.e., Dynapac CC6200 vibratory roller).

Experimental results of improved BFRRT-CC
We first determine the SVM model and draw the free space using the experimental scene shown in Fig. 4, and the best kernel parameter and the best error penalty factor c are 0.0059208 and 36.7583 respectively, thus, the zero-potential decision boundary is drawn and three key points are searched by LAP. Then we use CB curve to connect every two key points in turn and divide the fitted curve into 1000 segments to calculate the curvature at each point. As shown in Fig. 5, the generated CB curve is not suitable for the articulated vehicle to follow from about the point 900 to the point 1100, and in this case, BFRRT-CC is used to connect this segment. Fig. 6 shows CB curve segment with curvature meeting vehicle constraints and in Fig. 7, the hybrid path planning method based on articulated vehicle model is shown and the number from 1 to 8 indicate the best BFRRT-CC searching path.
Zero-potential decision boundary

Conclusion
In this paper, a hybrid path planning method based on articulated vehicle model is proposed to meet the demand of obstacle avoidance and searching the path back and forth. Simulation experiments show that the proposed algorithm can search a feasible path with a 90 degree turn. In future research, we will use the articulated vehicle to follow the planned path and solve the real-time dynamic obstacle avoidance problem on the planned path. When the articulated vehicle detects that a person or a vehicle is moving on the planned path, it can also stop in time and wait for the person or the vehicle to leave the path.
Funding Statement: This work was supported by the Jiangsu Natural Science Foundation Project BK20170681, and National Natural Science Foundation of China 51675281.

Conflicts of Interest:
The authors declare that they have no conflicts of interest to report regarding the present study.