A guaranteed collision-free trajectory planning method for autonomous parking

Planning a feasible, and safe trajectory is a crucial procedure for autonomous parking which remains to be fully solved. Generally, a trajectory is depicted by a sequence of conﬁgu-rations which include positions, and orientations. However, existing approaches usually design parking trajectories without considering the evolution from one conﬁguration to the next. Hence, these trajectories are practically infeasible in some scenarios, and safety risks may exist. In this paper, a guaranteed collision-free trajectory planning method is proposed for autonomous parking. A dynamic optimisation problem is built, which includes vehicle kinematics, collision avoidance constraints, and physical restraints. Then the dynamic optimisation problem is discretised into a ﬁnite-dimensional nonlinear programming problem. To describe collision avoidance constraints, the concepts of the virtual protection frame, and the magniﬁcation parameter are introduced. It is highlighted that a novel collision detection criterion is raised from the view of geometry. Tests in three common scenarios, and a complex scenario illustrate the effectiveness of the proposed approach. Moreover, the stability of the algorithm is discussed in this paper. Via the proposed approach, an integrated design of planning, and control is possible.


INTRODUCTION
Nowadays, fully autonomous driving is still a long way off but many manufacturers have promoted cars equipped with parking systems, such as BMW, Ford, Toyota etc. Imagine that a driver gets off at the garage entrance and goes to his office directly, then the vehicle drives itself from the drop-off point to the parking spot. Autonomous parking can emancipate drivers from the last driving stage. Besides these commercial applications, the number of literature on autonomous parking has been increasing steadily. Reference [1] divides the autonomous parking strategies into two categories, the path planning approach and the skill-based approach. The latter tries to park by imitating an experienced human driver, for example, neural network control and fuzzy control [2][3][4]. Since the recent two decades, the path planning approach has received a lot of attention because skill-based algorithms are computationally expensive and difficult to This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited. © 2020 The Authors. IET Intelligent Transport Systems published by John Wiley & Sons Ltd on behalf of The Institution of Engineering and Technology implement [5]. The trajectory (or path) planning problem for autonomous parking can be shortly defined as: given the initial and final configurations, find a feasible curve that an autonomous vehicle can move along. In theory, there may exist a lot of feasible trajectories from the initial configuration to the final configuration. An optimal trajectory is usually defined with the shortest length or shortest time.
As stated in [6,7], trajectory planning approaches are classified into four types. In graph search-based methods such as Dijkstra's algorithm, A* algorithm, State Lattice algorithm, the states of a vehicle are stored in the grids, then a solution is searched in the whole grids [8][9][10]. Sampling-based planners like RRT [11] can quickly explore the space since they sample the configuration space or the state space randomly at the price of suboptimal solution. Interpolating curve planners are widely used for the ability to offer a smoothing path solution. To ensure the high order continuity, polynomial curves [12], Bezier curves [13], spline curves [14] are often adopted. Compared with the above three types of planning algorithms, numerical optimisation aims to obtain a trajectory by solving a constrained dynamic optimisation problem (CDOP) [15][16][17][18]. The CDOP defines the objective function according to the driving mission. Vehicle kinematics, obstacle avoidance and physical limitations are included as constraints. Then the infinitedimensional function space of the dynamic optimisation problem is projected into a finite-dimensional vector space to leverage existing nonlinear programming (NLP) methods, that is, the problem is discretised in the whole time horizon and then a sequence of configurations are obtained. However, discretisation can lead to improper simplification. Most existing CDOP approaches do not consider the possible constraints violation between two adjacent configurations. In other words, the evolution between two adjacent configurations is omitted when discretisation. In [17], at every configuration point, two auxiliary decision variables are induced to compute the minimum distance between the vehicle and the obstacle by dual-function. In [18], the collision-free is determined if at every configuration point the four vertices of a vehicle are not inside the obstacle. In consequence, the potential collision is still a problem in the CDOP approach for autonomous parking. To address this challenge, a guaranteed collisionfree trajectory planning method for autonomous parking is proposed.
The contributions of this paper can be summarised as follows: (i) The concepts of the virtual protection frame and the magnification parameter are introduced to depict the region the vehicle covers along the trajectory. (ii) A collision detection criterion is proposed from the view of geometry. Specifically, the trajectory is collision-free only if there is no intersection between the virtual protection frame and the obstacle. The intersection is determined by the cross product. (iii) An iterative planning algorithm is stated which guarantees the collision-free solution in the whole time horizon.
The remainder of this paper is structured as follows: In Section 2, the formulation of CDOP when parking a four-wheel vehicle is introduced. In Section 3, the CDOP is solved via the nonlinear programming. The method to discretise the CDOP is detailed. Then a guaranteed collision-free trajectory planning algorithm is proposed. Numerical simulation results are shown in Section 4, which is followed by some analysis and comparisons with the conventional method in Section 5. At last, conclusions and future work are stated in Section 6.

FORMULATION OF THE CONSTRAINED DYNAMIC OPTIMISATION PROBLEM WHEN PARKING A FOUR-WHEEL VEHICLE
In this section, assuming a reliable map is available, the trajectory planning of a vehicle from the initial configuration to the final configuration is formulated as a CDOP, in which the kinematics model of a vehicle, the constraints to avoid collisions, and the physical restrictions are all taken into account.

Kinematics description of a four-wheel vehicle
A four-wheel vehicle can be regarded as a rigid body when parking at low speed [19]. Hence the dynamics of vehicle suspensions and the side-slip of tires are ignored. The kinematic equations [18] of a front-steering vehicle is denoted by Equation (1) where (X, Y ) is the center position of the rear axle in the global coordinates as shown in Figure 1, and is the orientation of the vehicle. f is the steering angle of the front wheel. v and a stand for the velocity and acceleration at the center of the rear axle. L represents the wheelbase. Figure 1 also indicates the vehicle width w, four vertices in clockwise order starting from left-front E 1 , E 2 , E 3 , E 4 , and an obstacle with five vertices

Collision avoidance from the view of geometry
In this section, a geometrical condition of avoiding collision is discussed. Here, the vehicle and obstacles are modeled as polygons. For the vehicle in Figure 1, its contour is defined by four vertices: The contour of the vehicle is formed by connecting four vertices clockwise and thus is a set of four line segments.
Similarly, for the j th (assuming total J obstacles, j = 1, 2, 3 , … , J ) obstacle, the contour can be described by m j vertices: (3) From the point of geometry, there is no collision between the vehicle and the obstacle as long as the boundaries of vehicle and obstacles do not intersect at any moment, which is intuitive and can be presented by: where ∕ − means line segments E and O j get intersected.
Remark 1. The vehicle should be outside the obstacles at the beginning. Then there is no collision as long as Equation (4) holds.

Physical restraints
Limited by the mechanical principle, the variable bounds should be considered. The physical constraints used in this study include: The front-wheel steering angle and its derivative are bounded to meet the torque requirement in the vertical direction of the wheel. Constraints of speed, acceleration and jerk (the derivative of acceleration) ensure the smoothness and stability for parking. Moreover, the rigid body assumption only holds when the vehicle moves at low speed. In practice, one can define the physical constraints to meet some other requirements.

The whole formulation
Now the overall formulation of CDOP when parking a vehicle should be given. Time consumption t f is chosen as an objective function since people usually desire to park their cars as fast as possible. Consequently, for total J obstacles in the scenario, the optimisation problem can be formulated as Eq (6). Unfortunately, computers cannot deal with infinitedimensional problems such as the optimisation problem (6). It is necessary to discretise the problem first, which will be detailed in the next section.

DISCRETISATION OF THE CONSTRAINED DYNAMIC OPTIMISATION PROBLEM
In this section, the CDOP (6) in Section 2 is transformed into a finite-dimensional nonlinear programming (NLP) problem by discretisation. To discretise the collision avoidance constraints in Equation (4), a novel collision checking method is presented. Finally, a guaranteed collision-free trajectory planning algorithm is proposed with theoretical analysis.

Multiple-shooting to discretise the vehicle kinematics model
Provided that the whole time horizon is divided into N intervals equally, on each which the control input is kept constant (see Figure 2). Then the control function u(⋅) is replaced FIGURE 3 It is collision-free at these two configurations, but the right front corner of the car has collided with the obstacle between [k, k + 1] by u 1 , u 2 , … , u k , … , u N . Similarly, 1 , 2 , … , k , … , N are states during each time interval and during each time interval. Given a start state k and a fixed control input u k , the state for the next interval is calculated by performing a time integration of Equation (1).
Provided the length of each interval is T : The integration resorts to the fourth-order Runge-Kutta which has less truncation error: Note that there is a mismatch between integration outcomẽ k+1 and the next state variable k+1 . Multiple-shooting is adopted to add constraints that handle the mismatch.

Discretisation of collision avoidance constraints
Since t f is divided to N intervals, it is a direct way to discretise collision avoidance constraints by satisfying the constraints just on the N time points. However, a constraint violation may occur at any instant other than the N configurations where the constraints are satisfied, which is unacceptable for vehicle trajectory planning (see Figure 3). When constraint violations happen in some time intervals, new time points are added into these intervals to enhance the resolution, which is called mesh refinement algorithm [20]. For trajectory planning, it is hard to calculate the minimum distance between the vehicle and obstacles [16], which makes it difficult to evaluate the violation level of collision avoidance constraints, and thus a proper refinement strategy is hard to define. Even more time points cannot ensure the non-existent of constraint violation. In addition, as a nonconvex problem, the CDOP may convergence to a different local optimum after the mesh refinement, making the result hard to explain.
In this section, a novel method to discretise the collision avoidance constraints is presented, via which collision-free will be satisfied in the whole time horizon. To illustrate this method, the definitions of virtual protection frame (VPF) and magnification parameter are given first. Figure 4 To consider the evolution between two adjacent configurations, the VPF only extends along the width of the vehicle, instead of the length. In Figure 4(a), four vehicle vertices on the VPF are marked correspondingly. E f i|k denotes the position of point E f i at time point k and E i|k is the position of point E i (i = 1, 2, 3, 4).l r and l f are the distances from the rear axle to the front end and the rear end. In one time interval [k, k + 1], the vehicle trajectory from the configuration at k to the configuration at k + 1 can be described as Figure 4(b). Three important elements should be noticed: (i) The red part is the region where the vehicle passes through, that is, at any time between [k, k + 1], any point on the vehicle is in this region. It is a corollary of vehicle kinematics. (ii) Similar to the red region, the green part is the region where the VPF passes
Equation (11) are all continuous functions. Note that for a circular arc in time interval [k, k + 1] (see Figure 5), given the difference of vehicle's orientation between k and k + 1 is less than , the place with the maximum error is at the midpoint of arc when fitting the arc by a line segment. The errors are defined with Equation (12).
With the above definition, Error E f i|k is greater than zero if the line segment is inside the region which VPF passes through and Error E f i|k < 0 means line segment is outside the region.
Next, the cover property is certified in an indirect way. Define the difference between the radius of E f i|k and E i|k as R i|k (i = 1, 2, 3, 4) according to: (13d) R i|k should be always greater than zero because the VPF is larger than the border of vehicle. Then the distance between the line segment and the inner circular arc is defined as: The cover property is satisfied when the following relationship holds: Therefore, the orange polygon can totally cover the region where the vehicle passes through. □ Lemma 1 illustrates that the orange polygon must have no intersection with obstacles to ensure a collisionfree trajectory in every time interval. It is too complex to calculate the orange polygon analytically.
Remark 2. Cormen et al. [21] have proposed a method to determine whether a line segment intersects with another. If the endpoints of the two line segments satisfy the relationship in the Appendix, the two segments do not intersect.

The guaranteed collision-free trajectory planning algorithm
After projecting the infinite-dimensional function space to the finite-dimensional vector space, the CDOP is reformulated as an NLP problem: ¬ Problem (17) is nonconvex due to the introduction of the kinematics model, hence a good initial guess is crucial to guarantee an optimal solution. Inspired by reference [17], Hybrid A* is leveraged to generate an initial guess path. Here an assumption is made before exploring the novel trajectory planning algorithm further. (17), and the initial guess path generated by Hybrid A* is always near to the collisionfree path. Remark 3. With the above assumption, problem (17) is feasible.

Assumption 1. A collision-free path exists in problem
On the basis of Equation (17), the proposed iterative trajectory planning algorithm is presented in Algorithm 1. A larger can guarantee a collision-free but conservative solution. Suppose * is the minimum results in a trajectory that happens to not collide, that is, * = max{inf i|k distance i|k ≤ 0}, ∀i = 1, 2, 3, 4, ∀k = 0, 1, 2, … , N − 1, For any i and K , according to the knowledge of monotonicity of distance i|k , it is apparent that the larger is, the less distance i|k is. To ensure that the infimum exists, it needs to be determined that an ( ≥ 1) exists making distance i|k = 0.
Taking i = 1 as an example, the distance 1|k is a function of 1|k as Equation (19) and the derivative of f ( 1|k ) is given by Equation (20). When turning right, the derivative is always negative and f ( 1|k ) is monotonic decreasing. Since f ( 1|k ) > 0 when 1|k = 1, there must be a root near one for 1|k (see Figure 6). It is the same when turning left. As a result, the infimum of i|k exists for any i and k. Among all the possible infimum values, the maximum one is selected to ensure distance i|k ≤ 0 for all i and k. □

SIMULATION RESULTS
In this section, simulation results in three common parking scenarios are presented, including reverse parking, parallel parking and diagonal parking. Then the proposed method is verified in a more complex parking scenario. All simulations are performed on a Windows 10 OS with an Intel i5 processor clocked at 1.8 GHz and the programming language is MATLAB with version 2019b. An open-source Matlab toolbox CasADi [22,23] is chosen to solve the NLP problem.

Parking in common parking scenarios
As defined in Figure 1, the vehicle model is regarded as a rectangle of size 4.5 m×1.9m. The wheelbase is L = 2.8 m. Limitations and start/final configurations are listed in Table 1.
In the reverse parking scenario, the parking spot is 2.5 m wide and 5.3 m long. The parking spot in the parallel parking scenario is 2.5 m wide and 6m long and in the diagonal scenario, it is also 2.5 m wide and 6m long, facing at 45 degrees. In all three cases, a 6 m wide road is prepared for maneuvering. Results are shown in Figures 7-9. The gray regions are obstacles such as other parking spots and road boundaries. The red regions are the place which the vehicle passes through. In Figures 7-9, the planned trajectory of the center of the rear axle is shown in the In Figure 7, the vehicle goes forwards and turns left slowly, then the vehicle should switch to reverse gear, at the same time turn right to move into the spot. The control inputs also satisfy the constraints listed in Table 1 and the planned trajectory is reasonable and feasible which will be illustrated in the next section.
It should be noted that in parallel parking (see Figure 8), at the final stage, the vehicle goes forward and turns right to align with the spot, which is similar to the action of a human driver. This action is necessary especially when the parking spot is not long enough.
Diagonal parking is not very common in daily life. Its difficulty is between that of reverse parking and parallel parking. In general, diagonal parking should start at a specific point, that is, the vehicle should start at left if the parking spot faces 45 degrees as Figure 9. An improper starting point may result in a failing trajectory planning.

Parking in irregular scenarios
In this subsection, the vehicle needs to park into an irregular spot which is slightly narrow in someplace. The  Figure 10, the vehicle still has no collision during parking especially at some crucial points such as (5,7.5) and (5,5.3).

Performance comparison with the ordinary trajectory planning algorithm
As emphasised before, the most important feature of the algorithm proposed in this paper is to guarantee the trajectory is collision-free during the whole time horizon. It may be hard to explain the collision-free property from Figures 7 to 9 directly. In Figure 11, by zooming in point (5,5) when reverse parking and points (6,2), (12,2.5) when parallel parking, it illustrates that there is indeed no collision. To demonstrate the effect of constraint (17e), another simulation is carried on with considering collision avoidance constraints only at the N time points. This method is regarded as a conventional CDOP-based planning algorithm. Note that for most CDOP-based trajectory planning approaches, collision avoidance constraints are implemented in different ways but with the same effect, so this comparison is representative.
As shown in Figure 12, in the parallel parking scenario, the same initial conditions are given and especially = 1.0316. It is apparent that the red region overlaps the gray one, which is clearly an undesired outcome. The vehicle may get a small scratch or even fail to parking. In general terms, one could argue that inflating the obstacle region is also an effective approach. Then even the vehicle has collisions with the larger region, it will not collide actually. It is truly a good idea but cannot solve the problem systematically.

Computational efforts
In the reverse parking scenario, the computational time for the optimisation problem (17) Table 2. It is a fair time-consuming part of the proposed algorithm. However, the parallel computing method might be helpful here to speed up the computation, which is beyond the scope of this paper.

Application of the designed trajectory
Different from path planning approaches like Hybrid A*, trajectory planning methods not only offer a reference path, but also consider the control execution time. Specifically, {Tra j (k)} N k=0 and {u(k)} N k=1 obtained from Algorithm 1 are very suitable to design a trajectory tracking controller.
Model predictive control (MPC) has a wide application ranging from process industry to robot systems [17,18,[24][25][26][27][28]. As a nonlinear model, Equation (1) should be linearised first. Specifically, linearising the kinematics model along the reference trajectory obtained by the proposed trajectory planning method, that is, {Tra j (k)} N k=0 and {u(k)} N k=1 , a control law is accessible from MPC by solving a quadratic program problem [29]. In the application, CarSim is employed to simulate the real vehicle and the MPC controller is an S-Function in Matlab. The whole planning and control scheme can be described as Figure 13.
The target speed and steering angle commands are used as the inputs of CarSim. Then the real states of the vehicle will be sampled for MPC to calculate the target control commands at the next sample time. Figure 14 shows the target commands calculated by the MPC controller and the real states of the vehicle in CarSim. Due to the reliability of low-level controllers (e.g. PID controllers) in Car-Sim. The actual speed and steering angle are very closed to the  target commands, hence the actual trajectories (see  are almost the same with the planned ones. Table 3 shows the maximum tracking error in three scenarios when using the MPC controller. In addition, the control inputs are slightly different from the inputs/states planned by the proposed method since the kinematics model cannot always accurately represent the real vehicle, which also validates the stability of MPC [30].

CONCLUSION
In this paper, a guaranteed collision-free trajectory planning algorithm for autonomous parking is presented. The parking problem is described as a dynamic optimisation problem which takes the vehicle kinematics, collision avoidance and physical constraints into account. When discretising the dynamic optimisation problem into a nonlinear programming problem, the concepts of the virtual protection frame and the magnification parameter are introduced to present a novel detection criterion from the view of geometry. The stability of the algorithm is studied analytically. Compared with conventional methods, the proposed algorithm creatively considers the evolution between the adjacent configurations. In the simulation part, the results for three typical parking scenarios along with a complex scenario illustrate the effectiveness of the method. Computational efforts are compared and analysed to demonstrate the algorithm is efficient. An MPC controller is designed to track the generated trajectory of which the tracking error is fairly small. In the future work, the further integration of trajectory planning and control using the proposed algorithm will be concentrated on to achieve the consistency in planning and control. Implementation on a real car robot is also necessary to prove the practicality.

APPENDIX A
A convex combination of two distinct points p 1 = (x 1 , y 1 ) and p 2 = (x 2 , y 2 ) is any point p 3 = (x 3 , y 3 ) such that for some in the range 0 ≤ ≤ 1, we have p 3 = p 1 + (1 − )p 2 . In other words, p 3 is any point that is on or between p 1 and p 2 on the line. Given two distinct points p 1 and p 2 , the line segment p 1 p 2 is the set of convex combinations of p 1 and p 2 , which are called endpoints of segments p 1 p 2 .
Considering two vectors v 1 = (x 1 , y 1 ) and v 2 = (x 2 , y 2 ) starting at the origin, the determinant of a matrix is adopted as the