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

Abstract

Reverse parking maneuvering of a vehicle with a trailer system is a difficult task to complete for human drivers due to the multi-body nature of the system and the unintuitive controls required to orientate the trailer properly. The problem is complicated with 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. This paper hence proposes a modified Hybrid A*-based algorithm that can accommodate the vehicle–trailer system as well as collision avoidance considerations with the other vehicles and obstacles in the parking environment. One of the novelties of this proposed approach is its adaptability to the vehicle with trailer system, where limits of usable steering input that prevent the occurrence of jackknife incidents vary with respect to system configuration. The other contribution is the addition of the collision avoidance functionality which the standard Hybrid A* algorithm lacks. The method is developed and presented first, followed by simulation case studies to demonstrate the efficacy of the proposed approach.

1. Introduction

There have been many advancements in the field of autonomous driving vehicles [1,2,3] and connected vehicles [4] in recent years. A basic functionality of an autonomous vehicle is its capability to plan and track its own path [5,6]. 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 [7]. 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 become more complicated due to the presence of other vehicles or obstacles that the trailer and its connected vehicle unit must avoid during the reverse parking maneuver. While path-planning methods for vehicles with trailers exist, there is a lack of rapidly deployable approaches that tackle reverse motion planning while simultaneously offering 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 [8,9,10,11]. 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 [7,9,10,12,13].

There have been numerous research results in the literature on the general topic of road vehicle/mobile robot path-planning, which can be roughly divided into several domains. Some of the earlier approaches are of graph-based or search-based nature. One of the most notable examples in this family of approaches is the A* algorithm formalized in reference [14] for the use of static environment, which served as the basis for various other approaches such as the D* algorithm detailed in [15] and D* Lite explained in [16]. More extensive modifications of graph-based approaches lead into the hybrid lattice-based family of methods, with the principle example being the Hybrid A* approach detailed in [17], which takes vehicle/mobile robot kinematics into account. Another popular family of path planners are of sampling-based design and include approaches such as the probabilistic roadmap (PRM) method explained in [18], the rapidly exploring random tree (RRT) method detailed in [19] as well as the improved PRM* and RRT* algorithms introduced in [20]. Other families of approaches include optimization-based planners such as the ones illustrated in [21,22], as well as the more recent learning-based path planners such as the motion planning networks (MPNet) introduced in [23].

An important criterion for successful path-planning operation is the planner’s capability of obstacle collision avoidance. To achieve this function, numerous methods have been explored in the literature. One of the commonly used collision-checking approaches is to approximate vehicle footprint with several inflated circles. This approach is frequently used in planning methods such as the Hybrid A* algorithm. One example of this approach is described in reference [24]. A more geometrically intuitive approach is described in [25], where collision avoidance relies on vertices check of vehicles and obstacles represented as oriented bounding boxes (OBB). Another family of methods, such as [26,27], aims to establish safe rectangular corridors to formulate convex collision constraints. In addition, Control Barrier Function (CBF) and its many variants, such as high-order CBF detailed in [28], also remain a popular method for collision avoidance.

In addition to the generic path-planning methodologies described above, implementations unique to vehicles with single-trailer systems have also been explored in the literature. Some approaches opt to plan the path that starts from the terminal/goal states and ends at the starting states [29]. Similarly, Ref. [30] uses a tree-based path planner while [31] presenting a Pontryagin’s Minimum Principle (PMP)-based optimal path-planning routine with obstacle avoidance through the artificial potential field for forward motion. Reference [32] presents a lattice-based path planner that uses kinematically feasible motion primitives. Reference [33] uses differential flatness and also uses an RRT-based motion planner. Reference [34] explores the minimum parking space for a vehicle with a one-axle trailer. Reference [35] presents feasible paths for vehicles with a single-trailer system by assembling simple path constructs such as rotations, translations, stretches and bends. Reference [36] applies semi-supervised learning where a deep neural network is used to generate paths that minimize off-track portions of the area swept by a vehicle with a single-trailer system. Reference [37] proposes a cooperative trajectory planning algorithm for vehicles with single-trailer wheeled robots.

Some of the above-mentioned path-planning approaches for vehicles with single-trailer systems are designed mainly with forward motions in mind. These approaches also do not always contain collision avoidance considerations, and some obstacle avoidance routines are too complicated for fast deployment. Thus, this paper 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 vehicles with single-trailer system reverse parking maneuvers. The main advantages of the proposed approach are as follows: (1) the derivation of inverse kinematics allows for the implicit constraint of hitch angle, which prevents jackknifing behavior of the vehicle with single-trailer combination in reverse motion; (2) the collision avoidance mechanism is simplified as only vehicle–trailer system centerlines need to be reconstructed during collision check, while vehicle–trailer width information is encoded into the inflation of static obstacles that remain stationary.

The outline of the rest of this paper is as follows. Section 2 presents the kinematic vehicle with a single-trailer model. Section 3 illustrates the inverse kinematics of the vehicle with a single-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

A kinematic vehicle–trailer model is used in this paper as it offers simplicity compared to a dynamic model while also retaining sufficient accuracy under low-speed maneuvers, which is common in vehicle–trailer system reverse parking motions. This section aims to present such a kinematic vehicle–trailer model. The schematic of a generic vehicle–trailer system with one-trailer configuration is displayed in Figure 1, and its parameters are listed in

Table 1. Parameters of kinematic vehicle–trailer model.

Model Parameter	Explanation
$L$	Wheelbase of the tractor vehicle (passenger car, SUV or pickup truck)
$L_F$	Distance between vehicle center of gravity G and front axle center
$L_R$	Distance between vehicle center of gravity G and rear axle center
$L_H$	Distance between vehicle rear axle center and trailer hitch joint
$L_T$	Distance between trailer axle center and trailer hitch joint
${\delta }_f$	Vehicle front wheel steer angle
${\psi }_1$	Vehicle yaw angle
${\psi }_2$	Trailer yaw angle
$\overrightarrow{V_F}$	Vehicle front axle center velocity
$\overrightarrow{V_R}$	Vehicle rear axle center velocity
$\overrightarrow{V_H}$	Trailer hitch velocity
$\overrightarrow{V_T}$	Trailer axle center velocity

. It should be remarked that the type of tractor vehicle (semi-tractor or passenger vehicle) is determined by $L_H$, which is the distance between the vehicle rear axle center and the hitch point. Within the scope of this paper, a positive value of $L_H$ is chosen to represent the tractor unit as a typical passenger vehicle, SUV or pickup truck with the trailer hitch located at the rear bumper of the vehicle. A traditional front wheel steering configuration is also selected for the tractor vehicle as shown in Figure 1.

The kinematic model can be derived as follows:

$$\dot{X_R}=V_R·\mathrm{c}\mathrm{o}\mathrm{s}\left({\psi }_1\right)$$

(1)

$$\dot{Y_R}=V_R·\mathrm{s}\mathrm{i}\mathrm{n}\left({\psi }_1\right)$$

(2)

$$\dot{{\psi }_1}={\displaystyle \frac{V_R}{L}}\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_f\right)$$

(3)

$$\dot{X_T}=V_R\mathrm{c}\mathrm{o}\mathrm{s}({\psi }_2)[\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)+{\displaystyle \frac{L_H}{L}}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_f\right)]$$

(4)

$$\dot{Y_T}=V_R\mathrm{s}\mathrm{i}\mathrm{n}({\psi }_2)[\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)+{\displaystyle \frac{L_H}{L}}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_f\right)]$$

(5)

$$\dot{{\psi }_2}={\displaystyle \frac{V_R}{L_T}}[\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)-{\displaystyle \frac{L_H}{L}}\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_f\right)]$$

(6)

The process of derivation can be found in reference [7]. In this model, there are two inputs: 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 primary challenges of reverse parking maneuvers for vehicle–trailer combinations comes from the fact that the system tends to demonstrate unintuitive yaw behaviors for inexperienced drivers. As a result, a natural course of action is to decouple this multi-body system into individual components, where the trailer unit is treated as a standalone vehicle. One can thus calculate a ‘virtual’ steering angle at the trailer that would orientate it properly and then map this ‘virtual’ angle to the actual steering angle at the vehicle steerable axle through kinematic derivations.

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, and it is demonstrated in Figure 2 that only shows the trailer portion of the system to reduce visual clutters. With this ‘virtual’ steerable axle, the trailer unit can now be regarded as a standalone vehicle, and its ‘virtual’ steer angle is denoted as ${\delta }_T$.

The inverse kinematic model can be derived as follows:

$$\dot{{\psi }_2}={\displaystyle \frac{V_T}{L_T}}\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_T\right)$$

(7)

$$\dot{{\psi }_1}={\displaystyle \frac{V_T}{L_H}}[\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)-\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_T\right)]$$

(8)

$$V_T={\displaystyle \frac{V_R}{\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)+\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\delta }_T\right)}}$$

(9)

$${\delta }_f=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}\left({\displaystyle \frac{L}{L_H}}·{\displaystyle \frac{\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)-\cos \left(\mathrm{\Delta }\psi \right)\tan \left({\delta }_T\right)}{\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)+\sin \left(\mathrm{\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 mostly identical to the kinematic model presented in Section 2 and can be found in reference [7] as well.

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\mathrm{s}\mathrm{i}\mathrm{n}\left({\psi }_2\right)+L_T\dot{{\psi }_2}\mathrm{c}\mathrm{o}\mathrm{s}\left({\psi }_2\right)}{V_T\mathrm{c}\mathrm{o}\mathrm{s}\left({\psi }_2\right)-L_T\dot{{\psi }_2}\mathrm{s}\mathrm{i}\mathrm{n}\left({\psi }_2\right)}}\right)-{\psi }_2$$

(11)

The parameter value choices used in this simulation study are detailed in

Table 2. Parameter value choices for inverse kinematics simulation study.

Model Parameter	Value Choice
$L$	2.896 [m]
$L_H$	1.159 [m] (passenger vehicle)
$L_T$	2.693 [m]
$V_R$	−1 [m/s] (backward motion)

. 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 static 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 the trailer system in reverse motion; (2) the generated path must be collision-free. This is a reverse motion path-planning problem for a coupled multi-body system 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 aims to provide 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 the 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 becomes 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 {\mathbb{R}}^{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 used 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 vehicles with trailer systems, 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 identical simulation duration for all branches, ensures the terminal nodes of these branches are 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 a 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 and avoid undesirable behaviors such as jackknifing. 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. It should be noted that these upper and lower bounds of the virtual steering angle can be very different across different motion primitive expansion iterations depending on the hitch angle of the vehicle–trailer system.

$${\delta }_T=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}\left({\displaystyle \frac{\mathrm{L}·\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{\Delta }\psi \right)-L_H·\cos \left(\mathrm{\Delta }\psi \right)\tan \left({\delta }_f\right)}{\mathrm{L}·\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{\Delta }\psi \right)+L_H·\sin \left(\mathrm{\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 guarantees 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]\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. It should be remarked that the value of this intermediate virtual steering input can also be different across different node expansion iterations as the virtual steering input constraints (${\delta }_{T,min}$ and ${\delta }_{T,max}$) vary across iterations depending on vehicle–trailer hitch angle.

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

(17)

Finally, since the motion primitive branches are generated by simulating the kinematic vehicle–trailer system that takes actual inputs at the vehicle, and 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 Equations (9) and (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 ($\mathrm{\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. Parameters value choices for input constraint design numeric example.

Model Parameter	Value Choice
$\mathrm{\Delta }\psi$	0.1745 [rad] = 10 [deg]
$\left[{\delta }_{f,min},{\delta }_{f,max}\right]$	[−0.75, 0.75] [rad] = [−42.9718, 42.9718] [deg]
$\left[{\delta }_{T,lb2},{\delta }_{T,ub2}\right]$	[−0.5, 0.5] [rad] = [−28.6479, 28.6479] [deg]

, then applying Equation (15) will yield $\left[{\delta }_{T,lb1},{\delta }_{T,ub1}\right]=\left[-10.447,30.447\right]\left[\mathrm{d}\mathrm{e}\mathrm{g}\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 3, 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[\mathrm{d}\mathrm{e}\mathrm{g}\right]$. Given this result, the intermediate virtual steering can be calculated as ${\delta }_{T,int}=9.1004 \left[\mathrm{d}\mathrm{e}\mathrm{g}\right]$ with Equation (17). Further application of Equations (9) and (10) will result in kinematic vehicle–trailer system inputs that can be used to generate motion primitive branches.

The main advantage of synthesizing the steering input limits according to the above-mentioned routine is that it takes the vehicle–trailer system hitch angle into account and hence avoids the generation of motion primitive branches containing undesirable behaviors such as jackknifing. This benefit subsequently results in a more efficient algorithm as node expansion operations will only produce paths that are usable in future iterations.

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. A common way to do this is to first construct several circles centered on sample points along the vehicle (and trailer in this case) system centerline at each time step within the motion primitive branch. The vehicle and trailer unit footprints can hence be approximated by these circles with appropriately selected radius values. Collision can then be checked by evaluating if there exists any overlap between the obstacle and the circles. This sub-section aims to introduce a more simplistic version of this approach to aid in the goal of fast deployment.

Similarly to the circular footprint approach mentioned above, 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, plus some additional safe distance, 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 easily calculated using the transport formula. Figure 9 provides an illustration of this process. Collision tests can then be carried out by checking if these reconstructed centerline points are located in the occupied region of the inflated occupancy map. A certain advantage of this collision check setup is that much of the system footprint information, namely the vehicle and trailer width, is encoded into the static obstacles that remain stationary. As a result, only the system centerlines need to be reconstructed for each time step within the motion primitive branch, effectively reducing computational load. It should be noted, however, 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. Another consideration is that the reconstructed centerlines must extend the full length of the vehicle and the trailer units in order to guarantee collision avoidance of the whole system footprint, as displayed in Figure 9.

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. Priority queue contents.

Priority Queue Contents	Explanation
Action Sequence	Sequence of actions already applied in the current partial path
Cost	Cost value of the terminal node in the current partial path
Terminal Node States	Vehicle–trailer system states at the terminal node of current partial path
Expanded Branch Trajectory	Vehicle–trailer system trajectory of the most recent motion primitive in the current partial path
Overall Path Trajectory	Overall vehicle–trailer system trajectory of the current partial path
Input History	Vehicle–trailer system input history for the current partial path

. 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. Algorithm Implementation

5.1. Implementation Settings

Simulation case studies are carried out to demonstrate the effectiveness of the proposed Hybrid A*-based path-planning approach. This sub-section aims to present and explain the settings for the case studies, which are listed in

Table 5. Simulation case study value settings.

Model Parameter	Value Choice
Vehicle–Trailer System Parameters
$L$	2.896 [m]
$L_H$	1.159 [m] (passenger vehicle)
$L_T$	2.693 [m]
Vehicle Length	5.046 [m]
Trailer Length	3.84 [m]
Vehicle Width	1.935 [m]
Trailer Width	1.63 [m]
Cost Weightings
$Q$	$\left[\begin{array}{ccc}2& 0& 0\\ 0& 2& 0\\ 0& 0& 3\end{array}\right]$
$K_A$	0.1
Motion Primitive Settings
No. of Motion Primitive Branches	3
Motion Primitive Durations	1 [sec]
$V_T$	−1 [m/sec]
$[{\delta }_{f,min},{\delta }_{f,max}]$	[−0.75, 0.75] [rad] = [−42.9718, 42.9718] [deg]
$\left[{\delta }_{T,lb2},{\delta }_{T,ub2}\right]$	[−0.5, 0.5] [rad] = [−28.6479, 28.6479] [deg]
Collision Check Settings
Binary Occupancy Map Grid Resolution	10 [cells/m]
Radius of Obstacle Inflation	1 [m]
No. of Vehicle Centerline Reconstructed Points	10
No. of Trailer Centerline Reconstructed Points	10

.

The cost weightings used in the case studies are chosen via the trial-and-error method to accommodate the tested scenarios. Some rules of thumb, however, are being followed during the tuning process. As the Q matrix determines how trailer state deviations from desired final states affect the heuristic cost, its diagonal elements represent the importance of trailer X-position error, trailer Y-position error and trailer heading error, respectively. As a result, a higher value choice in a diagonal element in the Q matrix corresponds to the higher priority of the trailer converging to the desired final states along this direction. The selection of scaling factor $K_A$ in the accumulated action cost, in comparison, is much simpler, as the purpose of the accumulated action cost is to differentiate two different partial paths that share the same end node trailer states. In this sense, as long as $K_A$ is selected as a nonzero positive value, the function of accumulated action cost can be effectively facilitated. One consideration, however, is that the value of $K_A$ should not be too large compared to the value choices in the Q matrix, as the accumulated action cost may otherwise disrupt the planner’s ability to converge to the desired final trailer states.

For the collision check procedure, the obstacle inflation radius used in the binary occupancy map is designed to be the larger of the vehicle half-width and trailer half-width plus some additional safety margins, as described in Table 5. It should be noted that if parking space is more restrictive, the safety margins can be reduced in order to allow the vehicle–trailer combination to move more freely. The other collision avoidance parameters that can be easily tuned are the number of reconstructed points along the centerlines of the vehicle and the trailer, where more reconstructed points will slow the planner slightly but provide more robust collision avoidance guarantee.

As mentioned in Section 4.3, this paper adopts a three-branch motion primitive design. It should be remarked that more branches can be added to the node expansion process to further increase numbers of potentially feasible solution paths with the cost of slower planning. Section 4.3 also provides the design details for the maximum, minimum and intermediate steering inputs used to generate the motion primitive branches, which incorporates both tractor vehicle steering limits $[{\delta }_{f,min},{\delta }_{f,max}]$ and trailer unit-reasonable virtual steering limits $\left[{\delta }_{T,lb2},{\delta }_{T,ub2}\right]$ to avoid jackknifing behaviors. The value choices of these two sets of steering limits are also included in Table 5. An additional remark on the motion primitive generation setup used in the case studies is that all branches are produced with equal reverse speed and equal time durations to make sure the branches are comparable to one another.

5.2. Implementation Results

This sub-section aims to present the detailed implementation results of an example perpendicular docking scenario. The binary occupancy map used in this 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 for a set of feasible initial conditions using the proposed modified Hybrid A* planner are displayed in Figure 11, Figure 12 and Figure 13. Figure 11 shows all the motion primitive branches generated for the trailer unit during the planning process. It can be observed that branches that eventually result in collision with the inflated occupied region are not explored further, highlighting the effectiveness of the collision avoidance approach. It can also 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 12b that the system trajectories which describe the motion of the axle centers of both units clear the inflated occupied regions in the binary occupancy map, hence allowing the exterior outlines of the units to clear the uninflated occupied regions in the occupancy map. This conclusion can be validated in Figure 12a, where the footprint of the vehicle–trailer system can be observed to have stayed clear of the actual static obstacles, once again proving the efficacy of the proposed collision avoidance design.

Apart from the collision avoidance capability demonstrated in Figure 11 and Figure 12, Figure 13 presents additional results that illustrate the kinematic feasibility guarantee of the proposed planner along with its robustness against undesirable jackknifing behaviors. It can be observed from Figure 13a that the vehicle–trailer combination retains reasonable hitch angle values for the duration of the planned path execution, and the hitch angle never displayed the tendency to exceed 90 degrees that corresponds to the jackknifing event. Meanwhile, Figure 13b presents the tractor vehicle steering input history for the planned path, and it can be observed that the tractor vehicle steering limits $[{\delta }_{f,min},{\delta }_{f,max}]$ specified in Table 5 have not been breached, demonstrating the effectiveness of the proposed motion primitive design that incorporates inverse kinematics.

To further strengthen the validity of the proposed planning approach, additional results are displayed in Figure 14 and Figure 15, where an alternative set of initial conditions that are infeasible for the parking maneuver are used. Figure 14 displays all the motion primitive branches attempted by the proposed modified Hybrid A* planner, where it can be observed that no kinematically feasible and collision-free path can be established to connect the initial position and the goal position. This is in contrast to the results obtained with the standard Hybrid A* planner that does not feature inverse kinematics-based input constraints, as illustrated in Figure 15. It can be observed that despite Figure 15a showing a planned path that roughly connects the start position and the goal position, Figure 15b displays the hitch angle exceeding and maintaining beyond 90 degrees along the path, confirming that jackknifing incident has occurred, hence invalidating the planning result. It can thus be concluded that the modified Hybrid A* planner proposed in this paper is more suitable for the specific task of vehicle–trailer system reverse parking operations.

6. Conclusions and Future Work

This paper proposed a Hybrid A*-based path-planning strategy to tackle the challenging problem of planning the 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. Jackknifing behaviors were avoided by introducing input limits during the motion primitive generation process, and collision avoidance formulation was simplified by encoding the vehicle–trailer system width information into static obstacle inflations. 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 [38,39] or parameter space control systems [40] to improve robustness of the parking operation by introducing a feedback loop into the control system, rendering it a closed-loop 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. Dynamic collision avoidance [41] and experiments with a real vehicle using their sensors [42,43,44,45] to determine drivable areas and the parking spot will also be the focus areas of future work.