Page 172 - Designing Autonomous Mobile Robots : Inside the Mindo f an Intellegent Machine

Hard Navigation vs. Fuzzy Navigation

First, we calculate the vector between P1 and P2, and find its magnitude, which we will
call “d”. We assume that the points P1 and P2 lie on the surface of the column and
then we can calculate where these points imply that the center of the column is. We
find this by adding the vector “a” to the midpoint of the line from P1 to P2. It is rel-
atively straightforward to find the magnitude of “a” since we know all three sides of
an equilateral triangle. By treating half of this triangle as a right triangle, and using
the formula that the square of the hypotenuse is equal to the sum of the squares of
the sides, we can quickly show that:

2
2
a = sqrt ( r – (d/2) ) (Equation 11.1)
We also know that the azimuth of vector “a” is 90 degrees counterclockwise from the
vector azimuth for P1-P2. By adding the vector “c” to the midpoint of P1-P2, we find
the center of the column “C12” as implied by points P1 and P2. Then we repeat the
process for points P2 and P3. Since P3 does not lie on the column, the implied cen-
ter C23 is not at the same place as C12, but our robot does not yet know which point is
wrong. We average the two implied centers and get the center position “C.” The
error for the P1-P2 implied center is thus the distance e12, and that for P2-P3 is e23.

Had there been additional points for the feature, we would have had enough data to
tell that a point like P3 was not a good reading because the implied center of segments
using P3 was further from the average center than the implied centers of other point
pairs. We could have eliminated P3 from the collection. After removing the bad
points, we can recalculate the implied centers for the neighbors on either side of
them, as well as the average center.
Once we have filtered down to the points we wish to keep, the detected center of the
7
column is taken as the average of the remaining implied centers . However, what is
the quality of this feature data? This is where the enlightened instinct kicks in. One
frame of reference is the radius of the columns. If the implied centers vary by a sig-
nificant fraction of the radius of the column, then the image really doesn’t look much
like the column we are looking for.

7 I prefer simple averages to more complex statistical calculations because they are very fast to
calculate and the difference in results is usually insignificant if the points have been filtered for
outliers.

155