TASK 2: RESCUE PEOPLE

 

 INTRODUCTION

The objective of this practice is to implement the logic that allows a quad-rotor to recognize the faces of lost people and save their locations in order to perform a subsequent rescue maneuver. Besides, the quad-rotor batteries are limited and each 10 minutes, the quad-rotor should come back to charge again.

Along this explanation, I am going to cover the following topics:

• Coordinates conversion: from GPS to UTM
• State Machine -> Phase 1: Take off
• State Machine -> Phase 2: Go to survivors
• State Machine -> Phase 3: Finding people
• State Machine -> Phase 4: Go to boat
• State Machine -> Phase 5: Recharging
• State Machine -> Phase 6: Landing
• Video 
• Conclusion
   

COORDINATES CONVERSION: FROM GPS TO UTM

In this exercise, it is given GPS coordinates from boat position and the estimated survivors positions. Using the online conversor we can get the following UTM coordinates: 

• Boat: 40º16’48.2” N, 3º49’03.5” W   -> 4459161.1 N  430492.2 E
• Survivors : 40º16’47.23” N, 3º49’01.78” W ->  4459131.8 N  430532.5 E 

From UTM we need to convert it into relative coordinates:

x = 4300532.5 - 430492.2 = 40.29

y = 4459131.8 -4459162.1 = -30.299

 Now we can operate with this coordinates the whole exercises 

STATE MACHINE

To make implementation more efficient, I decided to implement a state machine that is going to be detailed explained down below. 

It shows like this:

• PHASE 1: TAKE OFF
• PHASE 2: GO TO SURVIVORS
• PHASE 3: FINDING PEOPLE
• PHASE 4: GO TO BOAT
• PHASE 5: RECHARGING
• PHASE 6: LANDING
  

 

 PHASE 1: TAKE OFF

To start moving the quad-rot, it is necessary to use the HAL.takeoff() function  and setting the height as 3 meters. 

PHASE 2: GO TO SURVIVORS

To go to that point we need the coordinates converted in the first step and using  the cmd_pos function, reach to the position.

All the time I am calculating the differences between the goal vs the actual point and I am setting an error that if it happens it means that it has reached to the goal point.

PHASE 3: FINDING PEOPLE

After reaching to the estimated point, I have been going through some possible solutions but I finally decided to use a rectangle path. This path is full up of waypoints calculated previously that it  uses again the cmd_pos and an error to go to each point. 

In this phase it is important to outline that to find people we need face detection. I have used the Haar Cascade classifier with its haarcascade_frontalface_default.xml. 

However, this classifier just recognize faces it they are straight so, we need to rotate the image.  I decided to rotate the image each 20 grades. This is how internally works: 

From all detections you get from each frame I just store 1 of them. Moreover, I just detect faces it it is close to the goal position. It helps me to avoid a possible detection when it goes to the recharging point.

PHASE 4: GO TO BOAT

If it is the time that the quad-rotor needs to recharge or finish its journey, need to go back to this point. I also use the cmd_pos command. 

PHASE 5: RECHARGING

Since the quad-rotor start operating, it is checking all the time if the time goes up to 10 minutes (600 seconds), needs to go back to recharge to the boat. However, I tried to land the quad-rotor over the boat but after that it couldn't take off again. So, what I maganed to do was just after going to the boat, stay there and print that it is recharging and after that, (it there are waypoints left) needs to come back and keep finding people from the same point it left in phase 3.

PHASE 6: LANDING

After phase 4 and no time to come back after recharging we need to land the robot using HAL.land() function.

After landing it prints the relative coordinates of each survivor and in UTM form. 

How I know there are different people?

I decided to use the euclidean distance between each coordinate and just I store the ones that are 4.5 meters or more between them.

 

Survivors' coordinates (relative , UTM):

Survivor 1: [40.3252449, -32.01535797, 3.01024175] , [430532.52 , 4459129.08]

Survivor 2: [40.33359909, -38.09850693, 3.0440681] , [430532.53, 4459123.0]

Survivor 3: [36.26777649, -35.04283905, 2.99961853] , [430528.47, 4459126.06]

Survivor 4: [32.2806282, -39.20793533, 2.95890236] , [430524.48, 4459121.89]

Survivor 5:  [27.12451363, -39.29663467, 3.04749322] , [430519.32, 4459121,80] 

Survivor 6: [24.26309967, -33.15195465, 2.84937] , [430516.46, 4459127.95]

You can check these coordinates in the video down below. 

VIDEO

Here you can see my final result. I hope you like it!

This video is 8x speed; original video is 20:47 mins 

 
CONCLUSION

I think this task has been quite interesting and I enjoyed programming it.