## 2 Method
### Autonomous Vehicle Pipeline and Vehicle Setup
#### 2.1.2 Path Planning  
The trajectory planning algorithm generates the feasible poses that the vehicle has to follow acting on the acceleration/deceleration and steering commands which are computed by the designed MPC controller. Thus, the objective of the designed path planning algorithm is to find a collision-free motion between the start and goal positions of the vehicle in a structured environment. The environment where the vehicle moves is defined by the perception stage. The vehicle explores the environment using a real-time local trajectory planner, based on a modified RRT algorithm for non-holonomic car-like mobile robots, using Dubins curves. The algorithm takes into account kinematic and dynamic constraints of the vehicle, as defined in [36], in addition to the pure geometric problem of obstacle avoidance, and allows to search non-convex high-dimensional spaces by randomly building a space-filling tree [37]. The RRT algorithm is based on the incremental construction of a search tree that attempts to rapidly and uniformly explore obstacle-free segment in the configuration space. Once a search tree is successfully created, a simple search operation among the branches of the tree can result in collision-free path between any two points in vehicle environment. Due to differential constraints of non-holonomic car-like mobile robots, the set of potential configurations that a vehicle can reach from a certain state is reduced and Dubins curves are selected for building the branches in the search tree instead of the straight lines, as it is discussed in [23]. The initial position of the vehicle is always retained in the origin of the frame at the considered sampling time. The detected cones on the two-dimensional map are used to discretize the space within which the vehicle is moving through the Delaunay Triangulation algorithm [38], i.e., an iterative search technique based on the assumption that one and only one circumference passes through three non-aligned points. Considering the obstacles detected by the perception pipeline, a circumference passing by them is computed for each random triplet of points. As a result of the iterative process that is repeated until all possible combinations of points are investigated during Delaunay Triangulation, the final goal is selected among the potential goal points, considering the farthest point with respect to the vehicle initial position. Afterwards, the search tree is built and updated each time new vehicle state is available. The minimum turning radius is set on the basis of the vehicle steering specification, as the maximum front steering angle that is equal to \(\delta_{max}=\frac{\pi}{6}\) rad during turns, and it is equal to \(\frac{\pi}{18}\) rad when the vehicle is accelerating. The implemented RRT algorithm exploits Dubins curves to connect two consecutive vertices, as the shortest path is expressed as a combination of no more than three primitive curves. In this research work, three different types of primitive curves are considered. The \(S\) primitive drives the car straight ahead, while the \(L\) and \(R\) primitives turn as sharply as possible to the left and right, respectively. As discussed in [39], ten combinations of primitive curves are possible, but only six primitives can be optimal, and they are properly named Dubins curves. The possible combinations are listed below:  
\[\left\{L_{R}R_{\beta}L_{\gamma},R_{\alpha}L_{\beta}R_{\gamma},L_{\alpha}S_{d}L _{\gamma},L_{\kappa}S_{d}R_{\gamma},R_{\alpha}S_{d}L_{\gamma},R_{\alpha}S_{d} R_{\gamma}\right\} \tag{1}\]  
where \(\alpha,\ \beta,\) and \(\gamma\) denote the total amount of accumulated rotation and \(\alpha,\gamma\in\ [0,2\pi),\)\(\beta\in\ (\pi,2\pi),\)\(d\) is the distance travelled along the straight segment, \(0\leq d\leq 10\) m. A robust theoretical background about Dubins segments can be found in [39]. Finally, the cumulative length \(\Lambda\) of the path is given by the sum of the length of its constituent segments.