3.3.7. Acceleration and Angular Velocity Constraints

Motion change has a great influence on the smoothness and comfort of vehicle operation. The vehicle motion is realized by speed control in the longitudinal direction and front steering control in the lateral direction. Thus, we consider constraining the change of the speed and steer angle here. That is to say, we constrain the acceleration and steer

angular velocity. Since we take the discrete waypoints as a reference, the constraints can be approximately expressed as:

$$\left|\frac{v^2(k+1) - v^2(k)}{2\Delta s(k)}\right| \le a\_{\text{max}, \prime} \tag{20}$$

$$\left|\frac{v(k)(a(k+1)-a(k))}{\Delta s(k)}\right| \le \omega\_{\text{max}}\tag{21}$$

where *a*max represents the limit value of accleration and *ω*max represents the limit value of the steering angle velocity.

#### 3.3.8. Collision Avoidance Constraints

Safety is the most important consideration in vehicle trajectory planning. For narrow corridor scenarios, safety means no border collision happens. For border-collision detection, we adopt a strategy of approximating a vehicle body shape with circles here to describe the collision condition intuitively and leave some safety margin at the same time [34], as shown in Figure 5.

In the circle fitting strategy, the circle parameters are decided by the circle number and vehicle body parameters. Suppose the vehicle is fitted by *Nc* circles, and then the circle's parameters are calculated as follows:

$$\mathbf{x}\_{j}^{c}(k) = \mathbf{x}(k) + (l\_{f} + L + \frac{l}{N\_c}(0.5 - j)\cos(\varphi(k)),\tag{22}$$

$$y\_j^\varepsilon(k) = y(k) + (l\_f + L + \frac{l}{N\_c}(0.5 - j)\sin(\varphi(k)),\tag{23}$$

$$r = 0.5\sqrt{(\frac{l}{N\_c})^2 + w^2} \,\text{.}\tag{24}$$

where *r* is the circle radius, *x<sup>c</sup> <sup>j</sup>*(*k*), *<sup>y</sup><sup>c</sup> <sup>j</sup>*(*k*) are the circle center position of the *j*th fitted circle, *L* is the wheel base, *l* and *w* are the length and width of the vehicle respectively.

Now that the circle centers and radius are quantitatively derived. Boundary-collision detection can be easily realized by comparing the value of the circle radius with all the distance from the circle centers to the corresponding boundary segments, as shown in Figure 6.

**Figure 6.** Corridor boundary-collision checking in trajectory planning.

If any distance is less than the circle radius, a collision between the vehicle and corridor boundary happens. Therefore, all the distances should be greater than the circle radius. In particular, the distance between a circle center and the boundary segments is as follows:

$$d\_j^l(k) = \frac{\left| A\_i^l \mathbf{x}\_j^c(k) + B\_i^l y\_j^c(k) + \mathbf{C}\_i^l \right|}{\sqrt{A\_i^{l2} + B\_i^{l2}}},\tag{25}$$

$$d\_{\dot{f}}^r(k) = \frac{\left| A\_i^r \mathbf{x}\_{\dot{f}}^c(k) + B\_i^r y\_{\dot{f}}^c(k) + \mathbf{C}\_i^r \right|}{\sqrt{A\_i^{r2} + B\_i^{r2}}}. \tag{26}$$

By taking the discrete waypoints as a reference, the collision avoidance constraints can be expressed as:

$$\left(A\_i^l \mathbf{x}\_j^\varepsilon(k) + B\_i^l \mathbf{y}\_j^\varepsilon(k) + \mathbf{C}\_i^l\right)^2 - r^2 \left(A\_i^{l2} + B\_i^{l2}\right) > 0,\tag{27}$$

$$\left(A\_i^r \mathbf{x}\_j^\varepsilon(k) + B\_i^r \mathbf{y}\_j^\varepsilon(k) + \mathbf{C}\_i^r\right)^2 - r^2 \left(A\_i^{r2} + B\_i^{r2}\right) > 0,\tag{28}$$

in which *k* range from 1 to *N*. On the basis of [32], the principle of the parameter and its influence on the results will be discussed in Section 4.3.

#### *3.4. SOTP Method Design*

With the objective function and the constraints mentioned above, the trajectory optimization model is finally constructed as follows:

$$\begin{array}{c} \min \, (10), \\ \text{s.t. (11) to (21), (27) to (28).} \end{array} \tag{29}$$

The trajectory optimization model would lead to an optimal sequence of vehicle velocities and steering angles. By the integration of this sequence, a target trajectory with the characteristics of safety, feasibility, comfort, and efficiency would finally be generated.

What should be noticed here is that the target trajectory is used for reference in a fixed scene. Thus, the optimization model is solved in a single time rather than the rolling horizon way. Moreover, if the solving process fails to find an optimal solution because of being trapped in a local infeasibility point, a trajectory recorded by an experienced driver could be used to replace the target trajectory.

#### **4. Numerical Simulation**

Numerical simulation is conducted to verify the proposed SOTP method. We first test the practicability in the single corner scenario and subsequently conduct statistical analysis in the narrow corridor scenario with multi-corners compared with the baseline methods.

#### *4.1. Single Corner Scenario*

Narrow corridors are hard for vehicles to pass because the corridor boundaries strictly limit the collision-free space, especially in the turning area. Therefore, in this subsection, we apply the proposed method SOTP to the single corner scenario for investigating two problems: (1) could the proposed method be feasible for the narrow corridor scene with single corner? (2) what is the minimum corridor turning angle for passing?

#### 4.1.1. Simulation Setup

To explore the solving ultimate limitation of the proposed method SOTP, we here construct a narrow corridor cluster with a decreasing amplitude of 5◦, which ranges from 180◦ to the minimum angle within their solving capability. Parameters of the narrow corridor cluster are shown in Table 1. As for the vehicle model and vehicle motion limitation, we adopt the same parameters as in Table A2 and Table A3 respectively. As we construct

terminal posture constraints, we need to set the threshold value Δ*x*, Δ*y*, Δ*θ* to ensure the vehicle's final posture to be near the expected posture. The key parameters of the SOTP method are all listed in Table 2.

**Table 1.** Narrow corridor cluster parameters.


**Table 2.** The proposed method parameters.


We use the platform of MATLAB to design the mathematically-described narrow corridors and establish the proposed trajectory generation model. Since the trajectory planning process discussed in this paper is converted to a nonlinear program (NLP), we here use the NLP solver, IPOPT [35], to find the solution. After realizing trajectory generation by the SOTP method, we subsequently set the trajectory tracking simulation on a co-simulation platform of Simulink and CarSim to prove the generated trajectory could be tracked in the practical application. For the established tracking controller, we take the famous Stanley algorithm [36] for lateral tracking and the classical PID algorithm for longitudinal tracking. All processes are executed on an Intel Core i5-11300H CPU with 16 GB RAM that runs at 3.1 Ghz.

#### 4.1.2. Simulation Result

The minimum corner angle required to find a feasible solution is 120◦ for the proposed SOTP method. As for the study purpose of this subsection, we here choose five groups of data from the corridor clusters, within which the solvability of our method is guaranteed, to illustrate the proposed method's performance. These five groups of data belong to Narrow Corridor NC1, Narrow Corridor NC4, Narrow Corridor NC7, Narrow Corridor NC10 and Narrow Corridor NC13 of which the corner angles are 180◦, 165◦, 150◦, 135◦ and 120◦.

As shown in Figure 7, the planned trajectory is smooth with the guarantee of minimum travel time and collision free. With the decrease in the turning angle, the average velocity of the trajectory slow down as well while the controller could track the generated trajectory which indicates the proposed method has huge potential to solve the narrow corridor scenario.

**Figure 7.** The simulation results in the single corner scenario. (**a**) the trajectory planning result and (**b**) the trajectory tracking result. The minimum turning angle of the proposed method is 120◦ and the trajectory could be tracked in different corner angles.

#### *4.2. Multi Corners Scenario*

In this subsection, we apply the proposed method and the baseline methods to a more general scenario that has two corners and evaluate the quality of the generated trajectories according to the evaluation metrics to present the excellent performance of the proposed method. The narrow corridor scenario with limited collision-free space means the scope of the feasible solution is smaller compared with those scenes with a larger passable area. Hence, we only consider the narrow corridor scenario.

### 4.2.1. Baseline Methods

The baseline algorithm we choose for comparison is the hybrid state A\* algorithm and dynamic window approach (DWA) method [37]. The hybrid state A\* method is widely used in complicated scenes because of its advantage to find the optimal path satisfying the non-holonomic constraints while the DWA method is popular in mobile robot navigation by generating the control variables directly to avoid obstacles.

In the hybrid state A\* algorithm, the path is formed by extending the nodes on the grid map with the lowest cost in the 3D kinematic state space. Since the extending nodes are influenced by the grip resolution, the target posture may never be reached. In this case, a termination condition is necessary so that the search process can be stopped once the extending node reaches a preset domain. In this study, we set the domain as: Δ*x* : ±0.25 m, Δ*y* : ±0.25 m, Δ*ϕ* : ±15◦ After finishing the search process, a rough path can be formed. Since the curvature of the original path is not continuous, a smoothing process is essential. We use the conjugate gradient descent algorithm introduced in [38] to smooth the original path here. Besides path planning, vehicle velocity also needs to be planned. Based on the path generated by the hybrid A\* algorithm, we here adopt nonlinear programming to plan the vehicle velocity, considering the side force and acceleration constraint.

#### 4.2.2. Simulation Setup

In this case study, the narrow corridor scenarios with more than two corners could be split into a scene with two corners. Therefore, we only consider two special narrow corridors. The first narrow corridor turns continuously in the same direction, while the second turns continuously in the opposite direction. The former situation can be continuous left to left turns or right to right turns. We name them the L2L mode and the R2R mode. The latter situation can be continuous right to left turns or left to right turns. We name them the R2L mode and the L2R mode. Because of the symmetry, we only need to simulate one mode in each situation. As shown in Figure 8, narrow corridors with the L2L and R2L modes are finally chosen to be the simulated scenes. More scenario details could be available in Table A1 and the parameter of the SOTP method is adopted in Table 2.

**Figure 8.** Typical continuous turning situations in narrow corridors with (**a**) presents the L2L mode and (**b**) presents the R2L mode.

#### 4.2.3. Evaluation Metrics

The assessment of the trajectory planning methods is concentrated on the ability to find the optimal solution and the quality of the generated trajectory. We consider computational performance, curvature, acceleration, and trajectory tracking error in the simulation assessment to evaluate the performance of the proposed method.

Computational Performance : we compute the trajectory generation time of each method on the same simulation platform to compare the computational performance. The shorter time presents a higher computational efficiency.

Curvature: although we have added a limitation for the curvature, the curvature variation stands for the smoothness of the trajectory and the smaller curvature variation shows a better trajectory performance.

Acceleration: acceleration changes sharply may cause the jerk to change violently which can lead to a terrible ride.

Travel Time: this point is also a kind of evaluation metric to present the travel efficiency. The shorter travel time shows a higher quality of trajectory.

#### 4.2.4. Trajectory Generation Result

We draw the generated trajectories and the evaluation results of all methods in Figure 9 for the L2L mode while in Figure 10 for the R2L mode. We know that vehicle operation in a narrow corridor is very hard, especially at the corridor corner, because of the requirement of more free area for turning. As shown in Figures 9a and 10a, the trajectories planned by the SOTP method in both the L2L mode and R2L mode approach the outer boundaries before turning while to the inner boundaries in the other areas. This phenomenon conforms to the fact that the vehicle with the proposed method can find more free space before turning compared with other baseline methods and it needs to shorten the trajectory to save travel time in the other areas. Additionally, as the velocity heat distribution shown in Figures 9a and 10a, the planned velocity would slow down before and rise up after turning, which is in accordance with the characteristics of actual vehicle rides. Therefore, the trajectories generated by the SOTP method in both corridors with the L2L and R2L modes are qualitatively reasonable.

**Figure 9.** Trajectory generated by all methods in the narrow corridor scene with heat distribution indicating the velocity change. (**a**) the trajectory planned in the L2L mode. (**b**) the curvature and acceleration profile of the trajectories in the L2L mode.

**Figure 10.** Trajectory generated by all methods in the narrow corridor scene with heat distribution indicating the velocity change. (**a**) the trajectory planned in the R2L mode. (**b**) the curvature and acceleration profile of the trajectories in the R2L mode.

The evaluation results including the curvature and the acceleration are shown in Figures 9b and 10b. More details concerning the maximum curvature, maximum acceleration, and average velocity are recorded in Table 3. From Figures 9b and 10b, the curvature of the DWA method changes sharply at each corner while the SOTP method and hybrid A\* can availably restrain the curvature below 0.2 m−1. As for the acceleration, both the SOTP method and the hybrid A\* method can keep the acceleration in the allowable scope while the travel time of the SOTP method is shorter than the hybrid A\* method. Furthermore, the computational time of the SOTP method is far less than that of the hybrid A\* method which presents the proposed method has more powerful computational performance.


**Table 3.** The evaluation results.

It is concluded that the proposed method has the highest computational efficiency and shortest travel time with the highest average velocity compared with other baseline methods while its acceleration and curvature could also satisfy the motion limitation.

#### *4.3. Sensitivity Analysis*

As designed in Equation (24), we use the fitted circles to approximate the vehicle shape for collision avoidance constraints. In this case study, we utilize different fitted circle numbers to generate trajectory while the other parameters are the same as the previous setting. The safety margin *r* is 1.24 m, 1.05 m, and 0.99 m with the case of *Nc* = 3, *Nc* = 5, and *Nc* = 7 respectively. The planned trajectories are shown in the Figure 11 while the evaluation results are shown in the Table 4. With the fitted circle number increasing, the planned trajectory is smoother with a higher velocity in that the more fitted circles could reduce the redundant area, as a consequence of enlarging the feasible solution space. The trajectories with *Nc* = 5 and *Nc* = 7 have little difference indicating that the performance could not be improved by continuously increasing the fitted circles. The computational time of the L2L mode is totally greater than the R2L mode as a result of the limited space used to correct the vehicle posture for passing the next corner.

**Figure 11.** Trajectory generated by the SOTP method with different fitted circle numbers in the narrow corridor scene with the velocity curve. (**a**) the trajectory planned in the L2L mode. (**b**) the trajectory planned in the R2L mode.


**Table 4.** The result of sensitivity analysis.
