Hybrid A*-Based Reverse Path-Planning of a Vehicle with Trailer System

1. Introduction

There have been many advancements in the field of autonomous driving vehicles in recent years [1,2,3]. A basic functionality of an autonomous vehicle is its capability to plan and track its own path [4,5]. Reverse parking, particularly in restrictive spaces, remains one of the most challenging maneuvers to complete for an autonomous vehicle and such difficulty increases considerably when the autonomous vehicle is coupled to a trailer unit during this process. A vehicle with a trailer attached needs to be steered in the opposite direction of intended trailer orientation which can be intuitively difficult for inexperienced drivers, requiring the development and utilization of advisory systems to aid drivers [6]. Another challenge is the fact that different steering inputs at the vehicle will be required to orientate the trailer the same way depending on the current pose of the vehicle-trailer combination. Things get more complicated due to the presence of other vehicles that the trailer and its connected vehicle must avoid during the reverse parking maneuver. While path planning methods in reverse motion for vehicles with trailers exist, there is a lack of results that also offer collision avoidance as part of the algorithm which is the motivation underlying the research reported in this paper.

Two types of models have typically been used for vehicles with trailer systems: dynamic models that consider the forces involved and kinematic models that focus on only geometric relationships.

While dynamic models provide more accurate descriptions of the tractor-trailer behaviors, they are typically more complicated in construction. As a result, many studies opt to derive the simpler kinematic model instead, particularly when the maneuvers being studied are low speed in nature, where significant tire deformations do not typically occur [7,8,9,10]. The concept of ‘virtual tractor’, where the last trailer unit in the vehicle-trailer system chain is regarded as a ‘virtual tractor’ unit has been used in [6,8,9,11,12].

A collision-free feasible path is the prerequisite of the effective completion of parking maneuvers. Some approaches opt to plan the path that starts from the terminal/goal states and ends at the starting states [13]. Similarly, [14] uses a tree-based path planner while [15] presents a Pontryagin’s Minimum Principle (PMP) based optimal path-planning routine with obstacle avoidance through the artificial potential field for forward motion. [16] presents a lattice-based path planner that uses kinematically feasible motion primitives. [17] uses differential flatness and also uses an RRT-based motion planner. [18] explores the minimum parking space for a vehicle with a one-axle trailer. [19] presents feasible paths for the tractor-trailer system by assembling simple path constructs such as rotations, translations, stretches and bends. [20] applies semi-supervised where a deep neural network is used to generate paths that minimize off-track portions of the area swept by the tractor-trailer system. [21] proposes a cooperative trajectory planning algorithm for tractor-trailer wheeled robots.

The above-mentioned path-planning approaches, while effective, are not sufficiently simple in formulation and do not always contain collision avoidance considerations. This paper, thus, proposes a Hybrid A*-based path-planning algorithm that is simple in construction while also providing kinematically feasible and collision-free paths for the automation of vehicle-trailer system reverse parking maneuvers. The outline of the rest of this paper is as follows. Section 2 presents the kinematic vehicle-trailer model. Section 3 illustrates the inverse kinematics of the vehicle-trailer system. Section 4 explains the Hybrid A*-based reverse parking path-planning design. Section 5 subsequently presents simulation case study results. The paper ends with conclusions.

2. Kinematic Vehicle-Trailer Model

Given the low-speed nature of the vehicle-trailer system during reverse parking motion, a kinematic model is used here due to its simplicity and sufficient accuracy. This section aims to derive such a kinematic vehicle-trailer model. The schematic of a generic vehicle-trailer system with one-trailer configuration is shown in Figure 1, and its parameters are listed in Table 1. $L_H$, which is the distance between the vehicle rear axle center and the hitch point, determines the type of tractor vehicle (semi-tractor or passenger vehicle), and is a positive value here corresponding to a typical vehicle-trailer system where the vehicle is a passenger car, SUV or pickup truck with a hitch connected to the rear bumper of the vehicle. A front wheel steering vehicle is considered as shown in Figure 1.

The kinematic model is summarized in the following equations:

$$\dot{X_R}=V_R·\cos \left({\psi }_1\right)$$

(1)

$$\dot{Y_R}=V_R·\sin \left({\psi }_1\right)$$

(2)

$$\dot{{\psi }_1}={\displaystyle \frac{V_R}{L}}\tan \left({\delta }_f\right)$$

(3)

$$\dot{X_T}=V_R\cos \left({\psi }_2\right)\left[\cos \left(\mathsf{\Delta }\psi \right)+{\displaystyle \frac{L_H}{L}}\sin \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_f\right)\right]$$

(4)

$$\dot{Y_T}=V_R\sin \left({\psi }_2\right)\left[\cos \left(\mathsf{\Delta }\psi \right)+{\displaystyle \frac{L_H}{L}}\sin \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_f\right)\right]$$

(5)

$$\dot{{\psi }_2}={\displaystyle \frac{V_R}{L_T}}\left[\sin \left(\mathsf{\Delta }\psi \right)-{\displaystyle \frac{L_H}{L}}\cos \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_f\right)\right]$$

(6)

and its derivation is given in reference [6]. In this model, the inputs are the vehicle front axle steer angle ${\delta }_f$ and vehicle rear axle center speed $V_R$.

3. Inverse Kinematics

3.1. Inverse Kinematics Derivation

One of the main challenges of vehicle-trailer reverse parking maneuvers comes from the fact that the system tends to demonstrate unintuitive yaw behavior for inexperienced drivers. As a result, it would be helpful to decouple this multi-body system into individual components, where the trailer unit is treated as a standalone vehicle. One can hence calculate a ‘virtual’ steering angle at the trailer that would orientate it properly and then map this ‘virtual’ angle to the actual steer angle at the vehicle steerable axle through kinematic derivation.

The inverse kinematic vehicle-trailer model under consideration is identical to the model illustrated in Figure 1 except for the addition of a ‘virtual’ steerable axle at the trailer hitch, as demonstrated in Figure 2 that shows only the trailer portion of the model to reduce visual clutter. With this ‘virtual’ steerable axle, the trailer unit can be regarded as a standalone vehicle, and its ‘virtual’ steer angle is denoted as ${\delta }_T$.

The inverse kinematic model is given by:

$$\dot{{\psi }_2}={\displaystyle \frac{V_T}{L_T}}\tan \left({\delta }_T\right)$$

(7)

$$\dot{{\psi }_1}={\displaystyle \frac{V_T}{L_H}}\left[\sin \left(\mathsf{\Delta }\psi \right)-\cos \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_T\right)\right]$$

(8)

$$V_T={\displaystyle \frac{V_R}{\cos \left(\mathsf{\Delta }\psi \right)+\sin \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_T\right)}}$$

(9)

$${\delta }_f=\mathrm{atan}\left({\displaystyle \frac{L}{L_H}}·{\displaystyle \frac{\sin \left(\mathsf{\Delta }\psi \right)-\cos \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_T\right)}{\cos \left(\mathsf{\Delta }\psi \right)+\sin \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_T\right)}}\right)$$

(10)

where Equations (7) and (8) represent the ‘desired’ yaw rates of the trailer and the vehicle, respectively, given a ‘virtual’ steer angle ${\delta }_T$ while Equation (9) represents the mapping from vehicle rear axle center speed to trailer axle center speed given a ‘virtual’ steer angle ${\delta }_T$. Finally, Equation (10) is the mapping equation from the ‘virtual’ steer angle at the trailer hitch to the actual steer angle at the vehicle steerable (front) axle. The derivation of this inverse kinematic model is given in references [6].

3.2. Inverse Kinematics Validation

A simulation study is carried out to demonstrate the effects of the actual-virtual steering angle mapping equation derived in the previous sub-section. The simulation procedure is illustrated in Figure 3. A ‘desired’ ${\delta }_T$ profile is first generated and fed into the inverse kinematics calculation block that invokes the actual-virtual steering angle mapping equation, and the resulting vehicle steer angle required is plugged into the kinematic vehicle-trailer model. An additional block is also included to calculate the ‘actual’ ${\delta }_T$ based on the outputs of the kinematic vehicle-trailer model, which is then compared to the ‘desired’ ${\delta }_T$ profile. Equation (11) illustrates how this ‘actual’ ${\delta }_T$ can be calculated.

$$Actual {\delta }_T=\mathrm{atan}\left({\displaystyle \frac{\dot{Y_H}}{\dot{X_H}}}\right)-{\psi }_2=\mathrm{atan}\left({\displaystyle \frac{V_T\sin \left({\psi }_2\right)+L_T\dot{{\psi }_2}\cos \left({\psi }_2\right)}{V_T\cos \left({\psi }_2\right)-L_T\dot{{\psi }_2}\sin \left({\psi }_2\right)}}\right)-{\psi }_2$$

(11)

The parameter value choices used in this simulation study are detailed in Table 2. It is worth noting that this simulation study focuses on the case of reverse motion with the trailer hitch located behind the vehicle rear axle, as this paper limits its scope on vehicle-trailer system backup maneuvers with a car-like (passenger car, SUV or pickup truck) tractor unit. Figure 4 displays the result of the simulation. It can be observed that the vehicle steering inputs generated by the inverse kinematics calculation can accurately re-create the ‘desired’ ${\delta }_T$ profile.

4. Multi-Body Hybrid A*-Based Path-Planning Design

4.1. Modified Hybrid A* Algorithm Overview

A typical scenario of vehicle-trailer system reverse path-planning operation is shown in Figure 5, where the planning goal is to position the vehicle-trailer system into the desired parking space while avoiding all obstacles that are usually located in neighboring parking spaces. The satisfactory completion of this planning procedure requires both of the following criteria to be met: 1) the generated path must be kinematically feasible for the vehicle with trailer system in reverse motion; 2) the generated path must be collision-free. This is a reverse motion path planning system for a coupled multi-body systems consisting of two bodies, the vehicle and the trailer, with the connection hinge being part of both bodies forming the constraint. To this end, the Hybrid A* algorithm can be used as a beginning of the path-planning approach and extended into a constrained multi-body system with built in collision avoidance. This approach is used as it takes the system kinematics into account. This sub-section hence provides a general overview of a modified Hybrid A* algorithm while later sub-sections dive into design details regarding the multi-body vehicle-trailer system accommodations and collision avoidance considerations.

The flowchart of the modified Hybrid A* algorithm is provided in Figure 6. The algorithm works in an iterative manner where several kinematically feasible path branches, called motion primitives, are constructed during each iteration by simulating the kinematic system for a set duration. Assuming that several partially completed paths have already been generated in the previous iterations, the newly generated motion primitives will spawn from the node located at the end of the existing partial path that is closest to the goal. The criteria to determine how close a particular partial path is to the goal is encoded in a metric defined as the cost, where a lower cost value corresponds to an increased proximity to the goal. Once the motion primitive branches are constructed, they are checked for collision, and the end points of the collision-free branches are assigned as the new terminal nodes of their respective partial paths. The terminal node of the partial path with the lowest cost value is subsequently used as the spawning point of additional motion primitive branches in the next iteration, and the cycle continues until a partial branch gets sufficiently close to the goal, in which case this path is regarded as the completed solution path that is both kinematically feasible and also collision-free.

Details of the partial paths, including system states at the terminal nodes and their corresponding cost values, are stored in a priority queue. This queue is ordered by cost values so that the node with the lowest cost can easily be located for motion primitive branch expansion. It should be noted that once the new admissible nodes are added to the queue at the end of each algorithm iteration, the entry used to spawn these new nodes is removed from the queue, as their information will already have been encoded into the new node entries. It is also worth noting that if the lowest cost node entry in the queue cannot yield any collision-free branches, the node entry corresponding to the second lowest cost will be attempted for node expansion operation.

4.2. Cost Function Design

As indicated in the algorithm overview, cost values are assigned to the end nodes of partial paths to evaluate their proximity to the goal state. This sub-section aims to explain the design of the cost function in detail.

In general, the cost function in this proposed path-planning algorithm has two components: a heuristic cost element and an accumulated action cost element. The heuristic cost describes how close the trailer state at the partial path end node is to the desired trailer goal state. It is of quadratic form and is displayed in Equation (12).

$$J_H={\left(\left[\begin{array}{c}{X}_T\\ Y_T\\ {\psi }_2\end{array}\right]-\left[\begin{array}{c}{X}_{T,goal}\\ Y_{T,goal}\\ {\psi }_{2,goal}\end{array}\right]\right)}^{T}Q\left(\left[\begin{array}{c}{X}_T\\ Y_T\\ {\psi }_2\end{array}\right]-\left[\begin{array}{c}{X}_{T,goal}\\ Y_{T,goal}\\ {\psi }_{2,goal}\end{array}\right]\right),\mathrm{where}Q\in {ℝ}^{3\times 3}\succ 0$$

(12)

Apart from the heuristic cost, additional considerations should be given to the selection of the partial path to be expanded. The necessity of this step is demonstrated in Figure 7 where path 1 reaches a query node while path 2 passes through the query node and completes a complete loop before returning to the query node. In this case, path 1 and path 2 have the same heuristic cost values since they both end up at the same terminal states. However, it is more efficient to choose path 1 for further path expansion since it has used less prior expansion steps. As a result, an additional cost function component, referred to as the accumulated action cost $J_A$, is defined in Equation (13), where $N_A$ refers to the number of prior actions already taken and $K_A$ is a scaling factor. Combining this accumulated action cost and the heuristic cost, the overall cost function can be written as Equation (14). It should be remarked that the scaling factor $K_A$ in Equation (13) must be adjusted such that the value of the accumulated action cost is much smaller than that of the heuristic cost since the presence of $J_A$ in the cost function is mainly to differentiate partial paths that have the same terminal states and should not interfere with the metrics of the heuristic cost.

$$J_A=K_A·N_A$$

(13)

$$J=J_H+J_A$$

(14)

4.3. Motion Primitives

As mentioned in a previous sub-section, kinematically feasible motion primitives are generated during each algorithm iteration where several branches spawning from the same node will be constructed. Since this paper focuses on the vehicle with trailer system, the branches are obtained by simulating the kinematic vehicle-trailer system for a set duration. While the number of branches can be custom-defined, this paper presents a three-branch design. The three branches correspond to the following behaviors respectively: 1) backward motion with maximum trailer orientation to the left; 2) backward motion with maximum trailer orientation to the right; 3) backward motion with intermediate trailer orientation (between maximum left and maximum right). All three branches feature the same trailer unit reverse speed $V_T$, which coupled with the same simulation duration, ensures the terminal nodes of these branches being comparable to each other using the cost function mentioned in the previous sub-section. It must also be remarked that each motion primitive branch features constant vehicle-trailer system steering input, the calculation of which is covered in this sub-section as follows.

The trailer orientations in maximum left and maximum right directions are determined by the virtual steering limits, denoted as (${\delta }_{T,max}$ and ${\delta }_{T,min}$), which should be carefully designed such that its virtual-actual steering angle mapping under the current hitch angle will not yield an actual steering angle that is unfeasible for the vehicle front axle. At the same time, the virtual steering angle will require its own feasible value range so that it can ensure reasonable trailer orientation behaviors. If we denote the vehicle front wheel steering angle range as $\left[{\delta }_{f,min},{\delta }_{f,max}\right]$, then Equation (15) as shown below that maps ${\delta }_f$ to ${\delta }_T$ can be used to generate the mapped upper and lower bounds of the virtual steering angle, denoted as $\left[{\delta }_{T,lb1},{\delta }_{T,ub1}\right]$, under the current hitch angle.

$${\delta }_T=\mathrm{atan}\left({\displaystyle \frac{\mathrm{L}·\sin \left(\mathsf{\Delta }\psi \right)-L_H·\cos \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_f\right)}{\mathrm{L}·\cos \left(\mathsf{\Delta }\psi \right)+L_H·\sin \left(\mathsf{\Delta }\psi \right)\tan \left({\delta }_f\right)}}\right)$$

(15)

If we further define the upper and lower bounds of the virtual steering angle that guarantee reasonable trailer orientation behaviors as $\left[{\delta }_{T,lb2},{\delta }_{T,ub2}\right]$, then the virtual steering input constraints (${\delta }_{T,min}$ and ${\delta }_{T,max}$) can be defined as shown in Equation (16).

$$\left[{\delta }_{T,min},{\delta }_{T,max}\right]=\left[{\delta }_{T,lb1},{\delta }_{T,ub1}\right]{\displaystyle \cap }\left[{\delta }_{T,lb2},{\delta }_{T,ub2}\right]$$

(16)

Once the maximum and minimum virtual steering inputs have been obtained, an intermediate virtual steering input, denoted as ${\delta }_{T,int}$ can be calculated with Equation (17) shown below to generate the motion primitive branch that features the intermediate trailer orientation.

$${\delta }_{T,int}={\displaystyle \frac{1}{2}}\left({\delta }_{T,max}+{\delta }_{T,min}\right)$$

(17)

Finally, since the motion primitive branches are generated by simulating the kinematic vehicle-trailer system that takes actual inputs at the vehicle, the above-mentioned virtual inputs at the trailer $\left(V_T,{\delta }_T\right)$ must be mapped to the actual inputs at the vehicle $\left(V_R,{\delta }_f\right)$ by invoking Equation (9) and Equation (10).

A simple numeric example is presented here to demonstrate the proposed motion primitive design. Model parameter values listed in Table 2 are reused. If one defines the current hitch angle of the vehicle-trailer system ($\mathsf{\Delta }\psi$) and the vehicle front wheel steering angle range (${\delta }_{f,min}$ and ${\delta }_{f,max}$) to take the values listed in Table 3, then applying Equation (15) will yield $\left[{\delta }_{T,lb1},{\delta }_{T,ub1}\right]=\left[-10.447,30.447\right]\left[deg\right]$. If one continues to define a reasonable set of trailer virtual steering angle limits (${\delta }_{T,lb2}$ and ${\delta }_{T,ub2}$) as shown in Table III, one can apply Equation (16) to narrow down the range of admissible virtual steering to $\left[{\delta }_{T,min},{\delta }_{T,max}\right]=\left[-10.447,28.6479\right]\left[deg\right]$. Given this result, the intermediate virtual steering can be calculated as ${\delta }_{T,int}=9.1004\left[deg\right]$ with Equation (17). Further application of Equation (9) and Equation (10) will result in the kinematic vehicle-trailer system inputs that can be used to generate the motion primitive branches.

4.4. Collision Check

Once the motion primitives are generated, collision checks will be applied to each candidate branch to determine if these kinematically feasible segments can avoid collisions with obstacles in the parking environment. To facilitate this function, a binary occupancy map should first be constructed to define the regions occupied by obstacles as well as unoccupied regions accessible to the vehicle-trailer system. It should be remarked that the motion primitive branches generated in the node expansion process represent the trajectories of the vehicle rear axle center and the trailer axle center. This means that collisions may still occur for the outer boundaries of the vehicle-trailer system even if the motion primitives themselves clear the obstacles. Given this consideration, the occupied regions in the binary occupancy map are inflated by the half width of the vehicle or the trailer, whichever is larger, and the inflated region is also assigned as occupied. Figure 8 provides an example of a binary occupancy map with inflated regions illustrated. Given this setup, as long as the vehicle-trailer system centerlines stay within the green unoccupied region, collision avoidance can be guaranteed for both the vehicle unit and the trailer unit.

Once the binary occupancy map has been established, the next step is to reconstruct the vehicle-trailer system centerlines. Since the motion primitives already contain vehicle states $\left(X_R,Y_R,{\psi }_1\right)$ and trailer states $\left(X_T,Y_T,{\psi }_2\right)$, the coordinates of additional points along the system centerlines can be calculated using the transport formula. Figure 9 provides an illustration of this process. The reconstructed centerlines can then be used to check for collisions in the binary occupancy map with inflated regions. It should be noted that the density of the reconstructed points should be relatively high for practical purposes, otherwise the inflated regions might clip the system centerlines without any reconstructed points being located inside them.

4.5. Priority Queue Design

As mentioned in a previous sub-section, the priority queue contains the details of all partial paths. For this proposed planning approach, the information included in each entry within the queue is listed in Table 4. Among these, action sequence, cost and terminal node states are necessary for the path-planning procedure to be carried out, with the action sequence providing the number of actions already applied to be used in cost function calculation, the cost informing on which node to expand first and terminal node states being used as initial conditions for node expansions. The remaining contents are included in the queue to allow for the easy visualization of the path-planning process. It should be remarked that even though the input history is available from the planning process and should theoretically allow the vehicle-trailer system to replicate the planned path, this is not a closed-loop system and hence is not robust against factors such as external disturbances. A feedback controller that will follow the desired path will be useful but is not treated here as it is outside the scope of the present paper.

5. Implementation Results

Simulation case studies are carried out to demonstrate the effectiveness of the proposed Hybrid A*-based path-planning approach. Table 5 lists the parameter value choices used in this case study. The parameter $T$ refers to the vehicle-trailer system simulation duration during the node expansion process where motion primitives are constructed, and its coupled usage with the value choice of constant trailer reverse speed $V_T$ guarantees that the motion primitive branches are comparable to each other using the cost function.

The binary occupancy map used in the case study is displayed in Figure 10. The goal of the path-planning procedure is to plan a kinematically feasible collision-free path from trailer start position to trailer goal position which is in an empty parking space with its neighboring parking spaces being occupied. Additionally, two inflated regions are added to facilitate the collision avoidance functionality of this proposed path-planning approach.

The path-planning results are displayed in Figure 11 and Figure 12. Figure 11 shows all the motion primitive branches generated for the trailer unit during the planning process. It can be observed that the motion primitives explored allow one sequence of branches to lead from trailer start position to a point very close to the trailer goal position, essentially forming a complete path that accomplishes the planning objective. This planned path is shown more clearly in Figure 12 with both vehicle unit and trailer unit trajectories displayed. It can be observed from Figure 12(b) that the system trajectories which describe the motion of the axle centers of both units clear the inflated binary occupancy map, which guarantees the exterior outlines of the units clear the uninflated occupancy map as demonstrated in Figure 12(a), proving the efficacy of the proposed collision avoidance design. It should be noted that the vehicle front axle steering input remains within the custom-defined limits, showcasing the effectiveness of the motion primitive design of the proposed path-planning algorithm. The method was further tested with many other starting positions which all performed effectively in achieving a collision free path ending close to the desired goal state.

6. Conclusions and Future Work

This paper proposed a Hybrid A*-based path-planning strategy to tackle the challenging problem of vehicle-trailer system reverse parking maneuver. A generic kinematic vehicle-trailer model with one trailer configuration was first derived. The inverse kinematics of the system was also derived to allow for the trailer to be regarded as a standalone unit where ‘virtual’ inputs at the trailer can later be propagated to the actual inputs at the tractor vehicle. A modified Hybrid A* algorithm was formulated to allow the planning of kinematically feasible and collision-free paths for the vehicle-trailer system in reverse motion. Simulation studies demonstrated the effectiveness of the proposed routine in its ability to guarantee kinematic feasibility and static obstacle avoidance. For future work, the path-planning approach proposed in this paper can be combined with path-tracking controllers such as nonlinear model predictive control (NMPC) systems to improve robustness of the parking operation. As an example of this integration, the path generated with the approach described in this paper can be used as a coarse initial trajectory in the NMPC routine that aims to mimic the initial path.