Collision Avoidance Algorithm
As it turns out, stopping Rover before he hits an obstacle is not too difficult with the SONAR sensor we are using. The hardest part was to determine what the stopping distance should be based on the angle at which the obstacle is detected. I think the method we have in place is adequate for the time being. It basically creates a triangle in front of Rover, the apex of which is at a point directly in from of Rover that is calculated based on how fast he is going. The base of the triangle is the SONAR turret. The width of the base is defined by the distance between the SONAR unit and the outside of the front tires plus a little extra.
The really hard part of the algorithm is determining where to go after Rover stops! This effort has made up the bulk of my “Rover” time for the past month. While Rover is traveling, the SONAR turret is swiveling back and forth over a range of +/- 80 degrees. Once an obstacle is detected that falls within the “Stop” triangle, Rover is told to stop. Here is the logic that I have finally (maybe
) settled on once a collision has been detected:
Stop the sweep, get the angles and distances in 10 degree increments for a span of +/-90 degrees
Algorithm Pseudo Code
Note: the following code is in a loop that will execute a maximum of 4 times:
If the obstacle is “too close”
Back up a little
Get the angles and distances in 10 degree increments for a span of +/-90 degrees
If the angle of the obstacle is too close to straight ahead
Set the turn angle to the angle with the longest path
Else
Set the turn angle to turn away from the obstacle by a fixed amount
EndIf
ElseIf the distance of the longest path is short
Turn away from the obstacle by 90 degrees
Else
If the angle to the object is not too close to the angle of the longest path,
Set the turn angle to the angle with the longest path
Else
Set the turn angle to the angle with the longest path + a fixed amount
Endif
If turn angle is opposite to previous turns
Backup a little
Get the angles and distances in 10 degree increments for a span of +/-90 degrees
If the angle of the obstacle is too close to straight ahead
Set the turn angle to the angle with the longest path
Else
Set the turn angle to turn away from the obstacle by a fixed amount
EndIf
EndIf
EndIf
Turn Rover by the calculated turn angle
Get the angles and distances in 10 degree increments for a span of +/-90 degrees
Exit the loop if the distance ahead is greater than the Collision distance
Something you should note about the algorithm is that it takes a new set of reading after EVERY time the position of Rover is changed. This is very important. Rover cannot make good decisions unless he has the latest data!
Another note of interest: Puck and I have been testing Rover in the “cube farm” within our office area. There are a number of cube walls that the SONAR we have does not detect. These walls seem to absorb the SONAR signal which essentially tells Rover that there is an open path where there is actually a wall! Obviously this is not a good situation. This particular phenomenon is what has caused me the most difficulty in developing the algorithm. I have another SONAR sensor on order that we will try just to see what happens.