Autoparking

 

 Autoparking

Goal:

 The objective of this exercise is to implement the logic of a navigation algorithm for an automated vehicle. The vehicle must find a parking space and park properly.

Implementations:

First, the system reads the laser data from the sensors. Then, it executes the following finite state machine (FSM):

 

- APPROACH:

 

    P Controller:

 

    In this state the car follows a P controller that compares in the right_laser every angle with its opposite angle. If one of those angles are inf they are ignored but if they have values we calculate the error and we sum all the errors multiplied by their factor which is higher the nearer the laser is to the middle of the car so it doesn't oscilate. 

 

Then we calculate the mean of the errors and using the laser at the lateral, exactly at 90º we calculate the current distance, and by knowing that we want to be at 1.5 meters of distance we calculate the distance error and our angular velocity (w) is:

    w = -kp_angle * orientation_error - kp_distance * distance_error. This way it keeps the orientation and distance desired.

 

    Hole detection:

    We establish a simulated hole with the size we want it to be.  Taking account the laser is in the middle of the car: 

half_len = HOLE_LENGTH / 2.0

x_min, x_max = -half_len, half_len

y_min, y_max = obstacle_dist, obstacle_dist + HOLE_WIDTH 

 

Then we look at every angle of the right laser and using trigonometry with the distance and the angle we calculate the position where the laser ends. If that position is inside the hole simulated then there is not hole. If all laser points have been analyzed and the hole is free then we go to the next state.

 

- PARK_1:

    Now we have detected the hole we look to the front laser. If there are no cars in the front we park with just the car behind the hole and go to the state DIRECT_PARK.

 

Nevertheless, if a car has been detected in front the car is going to go parallel to that car by looking to the right side of the back laser (170 to 180º) and the front laser (0 to 20º). If the right section of the front laser has detected an obstacle, which is when the car passed the hole, and the right section of the back laser detects something in less than 3 meters then goes to the state PARK_2.

 

- PARK_2:

    It starts going backwards and to the right until

        - The right laser detects nothing in the range (0 to 40º) this is effective because it means our car has turn enough using the car it was parallel to as its reference.

             

         - The car behind is at less than 6 meters. 

 

- PARK_3:

     It starts going backwards and left until 2 options:

        -The front car is 2 meters far away just looking at the front middle section (80 to 100º)

        -Our car is at less than 5 meters from the car behind. Just in case not to crash.

 

- STOP:

     The car starts going to the front and right until the front car is at less than 2 meters.

 

 

- DIRECT_PARK: 

    This is state is used because when we don't have a car reference to start our parking it was more realistic if the car just turned right until it was at a safe distance to start maneuvering backwards and the car is at less than 2 meters. 

 

Problems and Solutions: 

 Just by using all the lasers values i had problems because they cover a very large area so the distances used weren't as i though. The solution to this was to use the lasers by sections. Choosing the angles i wanted to use and this way I could use angles to park in a more realistic way.

 

Other problem was that my algorithm was made by using the front car as a reference. Because of this i had to implement a new way of parking my car when there is no car in front. 

videos: 

 No cars behind

No cars in front

Normal parking