Navigation of LEGO Vehicle in a Parking Lot

A Lego vehicle enters a parking lot. As soon as it enters a camera's system range the system "takes over" the vehicle and parks it safely in a free parking spot.


A Lego vehicle enters a parking lot. As soon as it enters a camera’s system range the system “takes over” the vehicle and parks it safely in a free parking spot.
The camera system includes camera, iPAQ, Fly Jacket and transmitter, which transmits commands to a controller on the vehicle.


The problem:
The project’s problem consists of two main parts:
1) Image processing: Identifying the vehicle’s location from a bird’s view image. In order to recognize the vehicle’s direction we identified the front and rear gravity center. This part can be implemented on any navigation system. For example: directing a vehicle to different locations on the camera’s range according to the user’s needs (like GPS).
2) Direction commands to the parking spot. This part was only developed in theory, and can be implemented on future projects.
The solution
1) Image processing:
In order to identify the rear and front center of mass points we followed these steps :
1. Subtraction of the picture of the vehicle in the parking lot from an empty Parking lot picture: this step’s purpose was to “remove” the parking spot from the picture and prevent difficulties concerning noise and dazzling from the florescent ceiling lights.
2. Thresholding: in order to separate the background from the vehicle parts. We decided to work with HSV color representation method because the saturation of the red and yellow parts of the vehicle is relatively high.
3. Finding the front and rear areas: we found the largest area (rear) and the second largest area (front) in the image after thresholding. The detailed algorithm for that is described in the final report.
4. We found the rear and front gravity center and marked it with squares around it.

The original picture of the vehicle                    Step 1:
1                                2

Step 2: THRESHOLDING                                 Step 3 : Finding REAR & FRONT areas
3                               4
Step 4: Finding the center of mass of REAR & FRONT


The iPAQ can only work with C++, and the main obstacle in our work has been the implementation of Matlab functions in C++.
The c++ algorithm and implementation is described very thoroughly in our report.

2) Direction commands to the parking spot:
1. We know the location of the parking spot because the camera is in a permanent location. We also know the direction which the vehicle should enter the spot.
2. We assume that logically the vehicle enters the parking spot from outside the camera’s range, and therefore the initial condition of the vehicle when the camera spots it is near the image margin.

  • We will always want to get the rear point of the vehicle to a state which it could move forward without getting on the parking boundaries (“restricted areas”). X on the picture below is an example of a point in that state
  • We split the parking lot into 9 rectangular areas, where the middle area includes the parking spot



Navigation algorithm:
If the vehicle is in area 1 – go to area 2.
If the vehicle is in area 2 – go to area 3.
If the vehicle is in area 3 – go to area 6.
If the vehicle is in area 4 – go to area 7.
If the vehicle is in area 7 – go to area 8.
If the vehicle is in area 8 – go to area 9.
If the vehicle is in area 9 – go to area 6.
If the vehicle is in area 6 – go to X, turn around to face the parking spot and go forward until you get to the desired place in the parking spot in area 5.
For example:


  • Whenever the vehicle is on two different areas we decide its behavior by the location of its rear
  • Angles:

we found the angle of the vehicle according to the formula:

We define beta as the angle between the desired direction of the vehicle and the existing direction.beta can be easily found by alpha according to the area (in the example beta is about (-70) degrees).
If beta is NOT between 0 to 10 degrees then we will turn the vehicle accordingly. We decided on 10 degrees so that the vehicle wouldn’t ‘cut’ the main area in any case.
Note: the vehicle could be turned on the spot due to the way we built it.

  • In order to avoid ‘cutting’ the main area when the vehicle is getting to areas 1,3,9 from a neighboring area the vehicle will be ordered to continue untill it reaches the areas half, and only then the turn will occur
  • X will be in a small range of y coordinate value of points. Then the vehicle will turn and go forward to the spot




The project components are:

The vehicle:                                          Battery:
10 11
Vehicle’s engine:                                    2.0 Microcomputer RCX controller:
12             13
Compaq iPAQ H3900 :                         Fly Jacket i3800 and A/D wire
14                          15
Extension wire:                                      Serial IR-Tower transmitter:
16 17
RS232 serial cable:                                WATEC 207-CD camera
18                 19

The programs we used are:

  • LEGO Mindstorm
  • Premiere
  • CamCapture
  • Matlab
  • Microsoft eMbedded Visual C++
  • ActiveSync


Results and conclusions
After running the system we managed to draw on the iPAQ screen four pictures: The empty parking lot, the vehicle in the parking lot, the subtraction of them and the vehicle with its front and rear marked.


It is very recommended to see our tips and ideas for future projects in the report.



We are grateful to our project supervisor Johanan Erez for his great support and guidance throughout this work.
We would also like to share our gratitude to Ronel Veksler and Gershon Koltun for their efforts in software guidance, and to the lab staff Ina Krinsky and Aharon Yacobi for their technical support and warm treatment.
We are also grateful to the Ollendorf Minerva Center Fund for supporting this project.