**5. Field Experiment**

Regarding the field test, we plan the trajectories by the SOTP method and the hybrid A\* method separately and establish controller to track them in a real environment on a modified vehicle with an autopilot system.

#### *5.1. Experiment Setup*

In the field test, we test the trajectory planning and tracking in Narrow Corridor NC10, which is the most rigorous among the five narrow corridors introduced above. We ignore the generated velocity profiles and replace them with a constant speed of 10.8 km/h here for the safety concerns in real narrow corridor scenes. The vehicle parameters and the motion limitations are shown in Table A2 and Table A3 respectively.

The field test is conducted on a modified Lincoln MKZ platform as shown in Figure 12. The platform is equipped with an integrated Global Navigation Satellite System (GNSS) and an Inertial Measurement Unit (IMU). The vehicle also supports the by-wire control of the throttle, brake, steering and gear shifting system. The algorithms of motion planning and trajectory tracking control are implemented in C++ under Robot Operating System (ROS). Additionally, the parameters of the SOTP method and the hybrid A\* method are the same as the previous setting. After generating the target trajectory, the vehicle is controlled by a nonlinear model predictive control algorithm introduced in [39] to track the planned trajectory at frequencies in excess of 100 Hz.

**Figure 12.** The modified Lincoln MKZ with an autopilot system used in the field experiment.

#### *5.2. Experiment Result*

The trajectories tracking results are shown in Figure 13, including the path tracking performance and the path tracking error. The lower tracking error shows that the trajectory could be tracked more easily and the corresponding method can be used in the real world more widely.

**Figure 13.** The trajectories tracking results in the field experiment. (**a**) the path tracking performance (**b**) the path tracking error.

The vehicle planned paths and the corresponding tracking paths in the field test are shown in Figure 13a. Intuitively, the dotted red path almost completely overlaps with the solid red path, while the dotted blue path has an obvious gap with the solid blue line. An objective illustration of the path tracking result is shown in the Figure 13b. The centimeterlevel tracking error here is related to both the advanced tracking control algorithm and the low testing speed for safety concerns. Anyway, the peak value in red is only 0.01 m while the peak value in blue is almost 0.045 m, which means the tracking error in red is much less than that in blue. Moreover, the fluctuation frequency of the tracking error in red is much gentler than that in blue. Hence, it can be concluded that the SOTP method is superior indeed.

#### **6. Conclusions**

This paper presents a space discretization-based optimal trajectory planning method for automated vehicles in narrow corridor-related scenes, which we name the SOTP method. With the space discretization strategy, we take the pre-discretized centerline waypoints as a reference and construct the optimal trajectory generation model totally in the space domain. An objective function in the trajectory optimization model is designed considering the travel time, with the goal of high efficiency. For constraints, vehicle kinematics, boundary collision avoidance, side force, actuator range limitation, terminal states, and boundary collision-free constraints are considered to make sure that the generated trajectory is safe and feasible. The proposed SOTP method is verified with both simulations and field experiments. The results show that the SOTP method is capable of generating feasible, smooth, and collisionfree trajectories in narrow corridor scenarios. Furthermore, compared to the popular hybrid state A\* algorithm, the SOTP method owns higher efficiency to generate a trajectory in the narrow corridor scene and the generated trajectories are smoother and more efficient. Moreover, the tracking performance of the trajectories planned by the SOTP method is much better, which would lead to more stable conditions for vehicle rides. Consequently, the proposed method has ability to plan a feasible trajectory in a narrow corridor scenario regarded as a corner case in the autonomous driving, potentially overcoming the limitation of other search-based methods.

We consider a static scenario with the predefined passable corridor which may depend on the high definition map. Therefore, in future works, we are going to consider the narrow corridor scenario with dynamic obstacles and uncertainty such as the sudden appearance of a pedestrian shielded by other obstacles to explore a more universal method.

**Author Contributions:** Conceptualization, B.X. and M.H.; methodology, B.X.; writing—original draft preparation, X.L. and S.Y.; formal analysis, M.H., Y.B. and Z.Q. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by National Key R&D Program of China, grant number 2021YFB2501800, National Natural Science Foundation of China, grant number 52102394, 52172384, and 52222216, and Hunan Provincial Natural Science Foundation of China, grant number 2021JJ40095 and 2021JJ40065.

**Data Availability Statement:** Not applicable.

**Acknowledgments:** The authors sincerely thanks to associate professor Bai Li of Hunan University for his critical discussion and reading during manuscript preparation.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **Appendix A**

**Table A1.** Narrow corridor parameters.


**Table A2.** Vehicle size parameters.


**Table A3.** Vehicle motion parameters.


#### **References**

