*1.1. Related Work*

Numerous studies have been devoted to vehicle trajectory planning [8–11]. The mainstream methods in these studies can be classified into four groups: curve interpolation methods, sampling methods, graph search methods, and numerical optimal control methods [12]. The curve interpolation methods generate a trajectory by interpolating the waypoints in a known set with curves such as the RS curve, clothoid, polynomial, Bezier, B-spline, and so on [13]. For example, in [14], Bae et al. adopted a quintic Bezier curve to generate candidate paths in the lane change maneuver while using lateral acceleration as the path judgment index. Sampling methods try to search for the connectivity of the configuration space by randomly sampling knots in it [15]. Rapid-exploring Random Tree (RRT) is the most typical sampling-based method. For example, in [16], Zheng et al. applied RRT to autonomous parking with vehicle nonholonomic constraints considered. The aforementioned two categories of methods are easy to implement, yet they are not applicable in

**Citation:** Xu, B.; Yuan, S.; Lin, X.; Hu, M.; Bian, Y.; Qin, Z. Space Discretization-Based Optimal Trajectory Planning for Automated Vehicles in Narrow Corridor Scenes. *Electronics* **2022**, *11*, 4239. https://doi.org/10.3390/ electronics11244239

Academic Editors: Bai Li, Youmin Zhang, Xiaohui Li and Tankut Acarman

Received: 16 November 2022 Accepted: 15 December 2022 Published: 19 December 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

complicated environments. In detail, paths generated by curve interpolation are deeply influenced by the preset waypoints, while paths generated by randomly sampling are not consistent. Moreover, paths generated by these two methods are suboptimal. Instead, graph search methods could construct the optimal trajectory by traversing the entire state space to find paths with the minimum cost [17]. One famous algorithm of graph searchbased methods is the A\* algorithm. For example, in [18], Min et al. proposed an improved A\* algorithm in an unstructured environment, in which profile collision avoidance was realized by simply setting a redundant security space. Graph search-based methods are inefficient while being used in high-dimensional spaces. To improve this method, some researchers combined the graph search methods with the former curve interpolation methods or sampling methods. In [19], Li et al. used the RS curve and the hybrid A\* algorithm to generate paths for automatic parking, yet the path is not curvature continuous. In [20], Dirik and Kocamaz proposed an RRT-Dijkstra algorithm to plan paths with discontinuous curvature. Although graph search methods could construct the optimal trajectories, they could not directly deal with complicated constraints such as obstacle avoidance and actuator limitation. Conversely, optimization-based methods could construct an optimal trajectory while well handling complicated constraints by creating accurate mathematical models [21]. In [22], Li et al. designed a moving trend function in a framework of nonlinear model predictive control, using a risk index to realize collision avoidance. In [23], Dixit et al. used an MPC controller to generate feasible and collision free trajectories by combing the artificial potential field method. In [24], Zhu et al. constructed an optimization problem of parameterized curvature control to realize trajectory generation in dynamic on-road driving environments. Though the trajectories generated by these optimization-based methods are feasible, smooth, and continuous, even in complicated environments, yet they are computational complex.

Narrow corridors are a special but common working scene for vehicles, especially for special purpose vehicles working in a fixed route. These vehicles include logistics vehicles in parks, forklifts in warehouses, underground scrapers in mines, and so on. Passable areas of those vehicles in narrow corridors are strictly limited by the corridor boundaries. Thus, vehicle motion in narrow corridors should be as precise as possible, otherwise, vehicles could collide with the corridor boundaries or could not pass the turning areas. Existing studies on trajectory planning for narrow corridors are concentrated on mobile robots such as unmanned aerial vehicles, and sampling-based methods are the most popular [25]. However, unlike those mobile robots with holonomic constraints who could perform in situ steering, vehicle turning maneuvers are restricted by the non-holonomic vehicle kinematics constraints. Thus, sampling-based methods, whose trajectory curvature is not continuous, are not suitable for vehicle trajectory planning in narrow corridors. There are a few studies on vehicle trajectory planning in narrow passable areas. In [26], Kim et al. used Dubins curves to generate paths for narrow parking lots in a predefined collision-free space, in which curvature of the generated path is discontinuous and quite a part of accessible areas is sacrificed. In [27], considering the parking environment with uncertainty, Li et al. proposed a parallel stitching strategy to replan the trajectory for avoiding the new appeared obstacles utilizing the accessible areas. In [28], Do et al. proposed a method based on the support vector machine (SVM) and fast marching method (FMM) to plan paths for narrow passage, in which obstacle avoidance and vehicle kinematics were considered yet other constraints such as vehicle actuator limitation and terminal postures were ignored. In [29], Tian et al. explored a method about how to turn around in narrow environments, in which RS curves and Bezier curves were applied. With RS curves, a lot of free space would be occupied in the place where the forward and back segments meet. Therefore, this method would not be applicable in a strictly restricted narrow corridor. In [30], Li et al. proposed a progressively constrained strategy that solves a sequence of easier planning problems with shrunk obstacles before handling their nominal sizes. Although the finally derived trajectories are curvature-continuous and optimal, the runtime is usually long and the algorithm performance relies highly on the initial guess. In [31], with the aim of reducing computational time, Li et al. proposed a lightweight iterative framework to

generate an optimal trajectory for autonomous parking. In [32], Lin et al. proposed a trajectory planning method for the mine scene but the experiments were not convincing due to the lack of field tests. Methods mentioned above do generate collision-free paths in narrow area scenes. However, the studies above either only consider the trajectory property of collision avoidance or aim at totally different subject models and vehicle operation space, which restricts their application in the narrow corridor scene.

## *1.2. Contributions*

In this paper, we extend our previous work [32] in terms of successfully applying the space discretization-based optimal trajectory planning method (hereinafter called the SOTP method) for automated vehicles in multi-corner narrow corridor scenes, wherein the trajectory curvature is guaranteed to be continuous, every inch of the precious drivable space is sufficiently utilized, and exterior/interior constraints are strictly satisfied. In the proposed SOTP method, we first design a mathematically-described driving corridor and discretize its centerline to generate reference waypoints. Based on these derived reference waypoints, we thereafter formulate a trajectory optimization model in the spatial domain with the consideration of travel time minimization, boundary collision avoidance, and constraint satisfaction in terms of vehicle kinematics, actuator range limitation, side force, etc. Finally, the constructed trajectory optimization model is verified with both simulations and field tests. The main contributions of this paper are as follows:

(1) A novel space discretization-based optimization method is proposed to solve the challenging trajectory planning problem in narrow corridor scenes with very limited passable areas. Compared with [32], more complicated constraints, e.g., boundary avoidance is considered and processed with the accurately established mathematical models of the vehicle and the narrow corridor being embedded into the trajectory generation process.

(2) A space discretization strategy is designed for the construction of the trajectory optimization model. In this strategy, we consider the target trajectory to be described by several discrete waypoints with velocity information. The simulation is designed to demonstrate the enhanced smoothness and computational efficiency of the trajectory planned by the proposed method compared with the baseline algorithm. A sensitivity analysis of the key parameter, e.g., the safety margin is conducted to show the performance of the proposed method.

(3) The field tests related to the trajectory generation ability and the quality of the generated trajectory are conducted to illustrate the advantages of the proposed method over a popular method in the application of the narrow corridor scenes.

### *1.3. Organization*

The rest of this paper is organized as follows. Section 2 formulates the problem. Section 3 introduces the methodology. Section 4 and Section 5 present the simulation and field test results respectively. Finally, Section 6 concludes the paper.

#### **2. Problem Statement**

Narrow corridors concerned in this paper refer to one-way roads with the ratio of vehicle width to road width exceeding 0.5. The workspace for vehicles operating in such corridors is usually closed and fixed, which brings a challenge for trajectory generation. In this case, how to generate such a reference trajectory is exactly what we explore below. We assume that the boundary and the centerline of the narrow corridor could be accessible by the perception technology or high definition map.

We consider a typical narrow corridor with specified left and right boundaries. Since the twisted narrow corridor makes it difficult for a vehicle to traverse, we simplify the boundaries via line segments and assume the corridor to be straight (Figure 1). By linear interpolation of the boundary position, we describe the corridor mathematically by using the formulas as follows:

$$A^l\_i \mathbf{x} + B^l\_i \mathbf{y} + \mathbf{C}^l\_i = \mathbf{0},\tag{1}$$

$$A^r\_i x + B^r\_i y + \mathbb{C}^r\_i = 0,\tag{2}$$

where *A<sup>l</sup> i* , *B<sup>l</sup> i* , *C<sup>l</sup> <sup>i</sup>* are expression parameters of the left boundary in segment *<sup>i</sup>* and *<sup>A</sup><sup>r</sup> <sup>i</sup>* , *<sup>B</sup><sup>r</sup> <sup>i</sup>* , *<sup>C</sup><sup>r</sup> i* are the right boundary. Suppose the corridor is comprised of *Ns* segments, so *i* ranges from 1 to *Ns*.

**Figure 1.** A typical narrow corridor with definite boundaries. The corridor is comprised of three segments. Hence, this corridor can be represented by the parameter groups *A<sup>l</sup> i* , *B<sup>l</sup> i* , *C<sup>l</sup> <sup>i</sup>* and *<sup>A</sup><sup>r</sup> <sup>i</sup>* , *<sup>B</sup><sup>r</sup> <sup>i</sup>* , *<sup>C</sup><sup>r</sup> i* , *i* ∈ {1, 2, 3}.

In the process of passing the narrow corridor, the vehicle is expected to run smoothly and efficiently without any collisions. Therefore, the reference trajectory which the automated vehicle tries to track should be smooth, efficient, and collision-free under the premise of feasibility. To generate such a trajectory, several factors are considered. In detail,

(1) For smoothness, large side-force related to high speed and large curvature is avoided, and the vehicle posture at the corridor terminal is restricted and the motion parameters are limited.

(2) For efficiency, the travel time is minimized.

(3) For collision avoidance, obstacle avoidance conditions based on the circle-fitting strategy are designed.

(4) For feasibility, trajectory generation is set up upon vehicle kinematics and the trajectory is designed as per the actuator range.

#### **3. Methodology**

To generate a time-optimal trajectory for a narrow corridor, we explore the trajectory optimization model as follows.

#### *3.1. Vehicle Kinematics Modeling*

Vehicles in a narrow corridor generally travel with a low or medium speed because of safety concerns, so the vehicle kinematic model [33], i.e., the bicycle model (Figure 2) that satisfies Ackerman's steering principles, is capable here.

**Figure 2.** The kinematic model with front-wheel steering.

Similar to [32], we use the coordinate of the rear axle center to represent the vehicle position. With the above-mentioned kinematic model, the vehicle kinematic equations generally expressed in the time domain are as follows:

$$
\dot{\mathfrak{x}} = \upsilon \cos(\mathfrak{q}),
\tag{3}
$$

$$
\dot{y} = v \sin(\varphi),
\tag{4}
$$

$$\phi = \frac{v \tan(\alpha)}{L},\tag{5}$$

where *x* and *y* are the vehicle position in the global coordinate system, *ϕ* is the vehicle yaw angle, *v* is the velocity, *α* is the steering angle and *L* is the wheelbase.

In this study, we discuss the trajectory planning problem in the space domain to make better use of the position information of the corridor boundaries. Therefore, we transform the kinematic relationship from the time domain to the space domain with the chain rule *d*(·) *dt* <sup>=</sup> *<sup>d</sup>*(·) *ds ds dt* <sup>=</sup> *vd*(·) *ds* . The transformed kinematic equations are as follows:

$$\mathbf{x}' = \frac{d\mathbf{x}}{ds} = \frac{\hat{\mathbf{x}}}{v} = \cos(\varphi),\tag{6}$$

$$y' = \frac{dy}{ds} = \frac{\dot{y}}{v} = \sin(\varphi),\tag{7}$$

$$q^{\prime} = \frac{d\varphi}{ds} = \frac{\dot{\varphi}}{v} = \frac{\tan(\alpha)}{L},\tag{8}$$

where *x* and *y* are the first derivatives of the vehicle position versus space, and *ϕ* is the first derivative of the vehicle yaw angle versus space.

#### *3.2. Space Discretization Strategy*

In narrow corridors, the total travel time is unpredictable due to the unknown velocity. On the contrary, the total travel distance can be approximated by the corridor centerline due to the limited drivable area. Particularly, the approximation error is tiny between the corridor centerline discrete interval and the target trajectory discrete interval. Therefore, we design a space discretization strategy here to construct the trajectory optimization model. In detail, we take the discrete waypoints on the corridor centerline as a reference and use its discrete interval to approximate the target trajectory discrete interval. In this way, the mathematical relationships in the target trajectory can be explicitly described and the target trajectory can be directly solved.

In the space discretization strategy, we discretize the vehicle trajectory in the space domain and describe the trajectory by the discrete waypoints with velocity information. To provide reference waypoints for this discrete trajectory, we discretize the corridor centerline with certain rules. For the reason that vehicle operation in corner areas is more difficult than that in other areas, we deal with these two situations differently by dividing the corridor into turning areas and straight areas. In this process, the turning area is decided by the unique tangent points of the corridor boundaries and corridor turning arcs with a fixed corridor turning center, see Figure 3. Then, we discretize the centerline in the turning areas and the straight areas separately, see Figure 4. The discrete points in the turning areas are expected to be denser than the discrete points in the straight areas because of the much harder driving environment in the turning areas. Thus, the waypoint interval in the turning areas should be smaller than that in the straight areas. With the rules mentioned above, we discretize the corridor centerline into *N* waypoints and then record their position information. Here *xo*(*k*), *yo*(*k*), *k* = 1, 2, ··· , *N* is used to represent the waypoint position in the global system and Δ*s*(*k*), *k* = 1, 2, ··· , *N* − 1 is used to represent the waypoint interval.

**Figure 3.** Corridor division. The turning area at the corridor is determined by the fixed road turning center and corridor turning angle.

**Figure 4.** Centerline discretization. The upper formula represents the left boundary while the lower formula represents the right boundary.

Now that the reference centerline has been discretized into *N* waypoints, the target trajectory can be approximated by these *N* points in the form of *ξ*(*s*(*k*)) = *ξ*(*s*(1)) + ∑*k*−<sup>1</sup> *<sup>j</sup>*=<sup>1</sup> Δ*s*(*j*), *k* = 2, 3, ··· , *N*, in which target trajectory waypoints interval are replaced by the centerline waypoint interval Δ*s*(*k*). We choose the vehicle position, vehicle yaw angle, and vehicle velocity as the trajectory states. Thus, the final target trajectory *ξ*(*s*(*k*)) can be represented by a sequence *X*(*k*), *k* = 1, 2, ··· , *N* as follows:

$$X(k) = [x(k), y(k), q(k), v(k)].\tag{9}$$

What should be noted here is that the final trajectory would be approximate because the actual intervals of the target trajectory waypoints are approximated by the centerline waypoint intervals to express mathematical relationships explicitly and to directly plan the target trajectory. Moreover, the approximation strategy used here is reasonable and workable for the reason that the error caused by this strategy is very small in the case of a narrow corridor.

### *3.3. Vehicle Trajectory Optimization*

This subsection presents the construction of the trajectory optimization model based on the space discretization strategy, considering the constraints related to safety, feasibility, smoothness, and travel time.

#### 3.3.1. Objective Function

Concerns about vehicle trajectory planning are mainly concentrated on safety, feasibility, comfort, and efficiency. The first three ones are hard constraints, while the efficiency requirement is a soft one. Thus, we choose efficiency as the objective here, and an objective function based on the corridor travel time is designed. By taking the discrete waypoints as a reference, the total travel time can be approximately expressed as:

$$J = \sum\_{k=1}^{N-1} \frac{\Delta s(k)}{v(k)}.\tag{10}$$
