Continuous-Curvature Target Tree Algorithm for Path Planning in Complex Parking Environments

The proposed continuous-curvature target tree addresses the curvature-discontinuity of the original target tree [19] by using a clothoid path. A clothoid path was used to find a continuous-curvature path by addressing the curvature-discontinuity between straight lines and circular arcs in [27, 28]. Motivated by these works, the continuous-curvature target tree in the present work is defined as a set of backward parking paths with continuous curvature by including a clothoid path between the straight and circular paths. In contrast to the original target tree, the continuous-curvature target tree considers not only the vehicle’s minimum turning radius but also its steering velocity.

The vehicle model for the continuous-curvature target tree is defined as a kinematic bicycle model [29], and additionally considers the curvature derivative of the path. The vehicle model is defined as

where $(x,y)$ is the position of the center of the rear axle. $\theta$ means the orientation of the vehicle. $L$ is the wheelbase of the vehicle, and $\delta$ is the vehicle’s steering angle. $d$ is the forward or backward direction of the vehicle. $(\bullet^{\prime})$ means the derivative with respect to the path length. $\kappa$ ($=\theta^{\prime}$) is the curvature of the path. $\sigma$ means the sharpness, the curvature derivative. The sharpness is related to $\delta^{\prime}$, the vehicle’s steering velocity of the front wheels ($\kappa^{\prime}=\dfrac{\delta^{\prime}}{Lcos^{2}(\delta)}$). The curvature of the path $\kappa$ and its derivative $\sigma$ are constrained by $|\kappa|\leq\kappa_{\text{max}}$ and $|\sigma|\leq\sigma_{\text{max}}$.

The continuous-curvature target tree is divided into perpendicular and parallel parking cases (see Fig. 2). For perpendicular parking, the continuous-curvature target tree consists of straight, clothoid, and circular paths (left part of Fig. 2(a)). For parallel parking, the continuous-curvature target tree is designed by adding a set of backward and forward circular paths, and a clothoid path (right part of Fig. 2(a)). The vehicle can be parked by aligning it to the parking spot by moving forward and backward via these additional paths.

The clothoid path is a path of which curvature is changed linearly by the path length. The curvature, $\kappa$, is described as

where $s$ is the path length. In Fig. 2(b), the curvature of the clothoid path (cyan line) is initially zero. It is changed linearly from zero to the maximum curvature, $\kappa_{\text{max}}$, by the sharpness, $\sigma$, as the slope in the path length and curvature plot. After the clothoid path, the vehicle’s configuration, $q_{cl}$, can be derived with respect to the $xy$-axis in Fig. 2(a), given as

where $(x_{cl},y_{cl},\theta_{cl})$ is the vehicle’s pose after the curvature changes from zero to $\kappa_{\text{max}}$ through the clothoid path. $C_{f}(s)$ and $S_{f}(s)$ are Fresnel Integrals [30] defined as $C_{f}(s)=\int_{0}^{s}cos(\frac{\pi}{2}u^{2})du$ and $S_{f}(s)=\int_{0}^{s}sin(\frac{\pi}{2}u^{2})du$.

The sharpness, $\sigma$, is changed within $\{-\sigma_{\text{max}},...,\sigma_{\text{max}}\}$ to build the branches of the continuous-curvature target tree. For example, the branch of the target tree in the left part of Fig. 2(a) (the green, cyan and red lines) is constructed by setting the sharpness as $\sigma_{\text{max}}$. As the sharpness, $\sigma$, decreases from $\sigma_{\text{max}}$ to $-\sigma_{\text{max}}$, the other branches are formed. At each branch, when the clothoid path’s curvature becomes the maximum curvature, $\kappa_{\text{max}}$, the circular path whose radius is $\kappa_{\text{max}}^{-1}$ (red line) is added to the end of the clothoid path (see Fig. 2(b)). If the curvature does not reach the maximum curvature due to collisions, the branch will consist of straight and clothoid paths.

For parallel parking, a set of circular paths, and a clothoid path are added to the straight and turning paths. This set of circular paths was proposed in [31] as a method by which a vehicle drives out of a parking spot in parallel parking. The continuous-curvature target tree for parallel parking is described in Fig. 2(c). The backward circular path is built by the vehicle driving backward from the parking spot, $q_{goal}$, to approach the rear obstacle without collisions. The vehicle’s direction is switched to forward, and the forward circular path is added. If the vehicle can drive out of the parking spot with these circular paths, the clothoid path whose curvature is changed from $\kappa_{\text{max}}$ to zero is added to the end of this forward circular path. Otherwise, another set of backward and forward circular paths is added, and the above steps is repeated. For example, in the right part of Fig. 2(a), two sets of backward and forward circular paths, and the clothoid path are built. The number of sets of circular paths is increased as the dimensions of the parking spot are decreased [31].

The cost function is proposed to determine the target tree that can reduce the planning time. The target tree that covers a larger area of the road for a parking situation is identified, by calculating the cost of the target tree. This is because the area to be searched by RRT can be reduced if the road could be covered widely by the target tree. In this regard, the planning time can be reduced further if the target tree covers a larger area of the road. The larger the area that the target tree covers, the lower the cost of the target tree.

Examples for calculating the cost of the target tree are shown in Fig. 3. The cost function uses the area covered by the turning path of the target tree. The area is divided into two portions with respect to the straight path of the target tree. It is represented as the red rectangles. Each portion is calculated by the length and width of each rectangle. The length (width) is defined as the maximum distance of the turning path along the $x$-axis ($y$-axis). The cost function is defined as

where $x_{b}$ and $y_{b}$ are the coordinates at the end of each branch $b$ in the target tree. $B_{k}$ means the set of the branches at {Left, Right} in the target tree in Fig. 3. $\max_{\forall b\in B_{k}}\lvert x_{b}\rvert$ and $\max_{\forall b\in B_{k}}\lvert y_{b}\rvert$ are the length and width, respectively. Thus, $A_{k}$ means the area of each red rectangle, and $\Sigma_{k}A_{k}$ means the area covered by the turning paths of the target tree (see Fig. 3). $l_{\text{max}}$ and $w_{\text{max}}$ are the length and width, respectively, when there are no obstacles blocking the turning paths of the target tree. In the case of Fig. 3 (top-left), $l_{\text{max}}$ becomes the length of the turning path. $w_{\text{max}}$ is calculated by the distance of the turning path along the $y$-axis, in the leftmost branch. Hence, $2l_{\text{max}}w_{\text{max}}$ in (4) denotes the area covered by the target tree when there are no obstacles, and it is depicted as the green rectangles. $(\Sigma_{k}A_{k})/(2l_{\text{max}}w_{\text{max}})$ in (4) becomes the ratio of the area covered by the target tree in the parking situation to the area covered by the target tree when there are no obstacles around the turning paths.

The proposed target tree algorithm initializes the target tree using (4), as detailed in Algorithm 2.

For determining the target tree that considers obstacles, continuous-curvature target trees are built by changing the length of the straight path, $l$, from zero to the length of the parking spot, $l_{parking}$, by intervals of $\alpha$ (lines 2-6 in Algorithm 2). The $cost$ of each target tree is calculated by the proposed cost function in (4) (line 4 in Algorithm 2). For example, in parking situation #1 in Fig. 3, the $cost$ of the target tree is calculated with different lengths of the straight path, $l$. The continuous-curvature target tree, $T_{target}$, with minimum $cost$ is selected among those target trees (line 7 in Algorithm 2).

The proposed target tree algorithm finds a shorter parking path by integrating with RRT* and executing the minimum-length path selection step. RRT* rewires a tree (denoted as $T$ in Algorithm 1) to reduce the length of the RRT* path that connects $q_{init}$ to a reached candidate goal, $q_{new}$. The proposed path selection step searches for a shorter parking path among the randomly reached candidate goals of the target tree within the sampling time ($t_{max}$ in Algorithm 1).

The minimum-length path selection step replaces lines 10 and 11 in Algorithm 1. First, several candidate goals of the target tree reached by the RRT* tree, $T$, are stored within the sampling time. This is because, even after the first parking path is obtained, RRT* paths that reach other candidate goals may be found during the additional time for a tree-rewiring step. Lines 10 and 11 in Algorithm 1 are replaced by,

where $q_{new}$ is the candidate goal in the target tree reached by the RRT* tree, $T$. $Q_{soln}$ is a set of these goals. Next, the minimum-length path selection step is added as follows,

In the GetMinimumLenPath function, the shortest parking path is returned among a set of the reached candidate goals, $Q_{soln}$. This function is executed when one of the candidate goals is reached or when the RRT* tree, $T$, is rewired. This minimum-length path selection step allows the target tree algorithm to find a shorter parking path as the sampling time increases. It is similar to the RRT* path planning algorithm, which finds a shorter path by adding a tree-rewiring step to RRT. The path selection step can also be used when the cost function for tree rewiring considers not only the path length but also other costs such as the number of forward/backward direction switches, and obstacle clearance.

The proposed algorithm was tested with an autonomous vehicle in real parking environments. The hardware configuration of the autonomous vehicle is shown in Fig. 4. The proposed target tree algorithm was implemented with Open Motion Planning Library (OMPL) [32]. The vehicle’s dimensions and path planning parameters are described in Table I.

The hybrid curvature (HC) tree-extension function [28] was used when the proposed target tree algorithm was integrated with RRT* for planning the continuous-curvature parking path. A kanayama controller [33] was used for tracking the parking path. The controller received the vehicle’s position and orientation relative to the path and curvature of the path as inputs, and the vehicle’s input steering angle was calculated. The vehicle’s maximum velocity was set to 4 km/h and 2 km/h when tracking the RRT path and the backward parking path of the target tree, respectively. The vehicle’s velocity was decreased in an inversely proportional manner to the path’s curvature. At the forward/backward direction switch, the vehicle stops for 3 s to change the steering angle. The LeGO-LOAM algorithm [34] was used to obtain the vehicle’s position and orientation relative to the path.

This section presents an analysis of whether the probabilistic completeness [36] of RRT or RRT* is maintained after the integration of the proposed target tree algorithm. Additional experiments show that the proposed algorithm can find a path close to the optimal path planned by RRT*, within a shorter sampling time than can the original target tree algorithm and other sampling-based planning algorithms for parking.

In the path planning problem, $\mathcal{P}(q_{init},q_{goal},Q_{free})$, the probabilistic completeness of the sampling-based planning algorithm means that if a collision-free path exists, the probability of finding the path will converge to one when the number of random samples approaches to infinity. The proposed target tree algorithm does not change the completeness of RRT*. The target tree is a set of poses constituting collision-free paths, and any pose can reach the original goal ($q_{goal}$); i.e., the target tree algorithm is complete with respect to these poses. In this regard, the target tree, $T_{target}$, can be an extended goal region ($Q_{target}$) that includes not only the original goal ($q_{goal}\in Q_{target}$) but also candidate goals that can reach this original goal. Accordingly, the proposed algorithm can be deemed to solve the path planning problem to reach this extended goal region with the RRT* path planning algorithm, $\mathcal{P}(q_{init},Q_{target},Q_{free})$. Integration of the target tree algorithm and RRT*, which are both complete, maintains the probabilistic completeness.

Additional experiments were executed to discuss the optimality of the proposed algorithm, as shown in Fig. 7. The proposed target tree algorithm was compared with the original target tree algorithm [19] with RRT* and minimum-length path selection, and other sampling-based planning algorithms for parking [35, 26]. Each algorithm was run 100 times for 60 s. The dashed horizontal line in Fig. 7 is regarded as the minimum length (i.e., optimal cost) in the original RRT* after 7200 s. The results show that the proposed target tree algorithm can obtain a parking path close to the optimal-length path (i.e., near-optimal). There are two reasons for these results. First, integrating RRT* and the proposed minimum-length path selection step (Section IV-C) allows to find a shorter parking path (near-optimal path) as the sampling time increases (see Fig. 7). Second, the proposed minimum-$cost$ target tree (Section IV-B) contains backward parking paths similar to the backward path of the optimal path. For example, as shown in situation #1-a of Figs. 6 and 7(a), a shorter parking path was found when planning a path with the proposed target tree considering obstacles.