The objective of this project – Autonomous Parallel Parking – is to develop an optimal approach for vehicle’s trajectory generation, as well as obstacle avoidance for performing parallel parking motion.
The dynamics of the vehicle is given by:
The trajectory generation and obstacle avoidance can be formulated as a constrained nonlinear optimal control problem, where we need to minimize the cost of the system. Our goal is to minimize the control effort, so the cost functional
The cost functional is minimized while subjecting to the inequality constraints
We have a nonlinear optimization problem, which requires numerical methods to find a solution. We approach the problem using the \textit{Direct Collocation} method, as it is easier to handle the path constraints, while simultaneously remaining the efficiency, stability, and simplicity under the large-scale complex problem.
As for the implementation, the running cost
Since we value the final heading angle the most, so in the
The vehicle and the obstacles are all rectangular objects, it would be inaccurate if we only calculate the distance between them using their center coordinates (i.e. as we did for disk obstacles). As a result, we have developed a function to calculate the minimum distance between two rectangular objects. The calculation involves the following steps:
-
Given the center coordinates of the rectangles
$\rightarrow$ calculate 4 corner coordinates of each rectangle. -
For each corner of our vehicle, calculate the shortest distance between the corner and the sides of the obstacle (parked vehicle).
- If the point lies within the line segment
$\rightarrow$ return the vertical distance - If the point lies outside of the line segment
$\rightarrow$ return the distance between the point and the closest endpoint
- If the point lies within the line segment
-
Repeat the process for the vehicle and all objects, then return the minimum value among those.
Finally, after obtaining the minimum distance between the obstacles and the vehicle, we can turn it into inequality constraints.
For safety reason, we want to add a buffer distance between the vehicle and the obstacles, so we add an additional 0.5 meter to each inequality constraint to ensure collisions will never occur.
The standard procedure to perform a parallel parking usually involves a two-stage parking technique, where the vehicle drives forward to first reach an intermediate goal right in front of the parking slot. Then, the vehicle needs to start reversing and adjust its steering angle to park into the slot. Therefore, we adopt the same technique to perform the autonomous parallel parking. We setup an intermediate goal for each parking slots, where we offset the x inward by 3 meters, and the y upward by 4 meters.
For simulation, we created a parking lot containing 8 slots, and the user is able to select which slot they want to park in. Once the parking slot is specified, all the remaining slots will become obstacles, and the boundaries of the parking lot will also become walls, where we want to avoid collision.