| SRS Home | Front Page | Monthly Issue | Index |
|
|
In preparation for a navigation contest hosted by the San Diego Robotics Society (http://www.sdrobotics.org/), the question came up about the possibility of accurate robot navigation in a robot playing field without the use of IR, sonar or laser range finders. This paper describes the theory behind a bearings-only laser navigation system, and is divided into three sections:
Section 1: The Solution and the Mathematical Algorithm (This paper) This paper is primarily the mathematical algorithm used. However it is written so that a reader without a background in geometry and trigonometry should be able to follow the process. The algorithm requires some geometry; the use of the Inscribed Angle Rule in a circle and some trigonometry such as the basic trigonometry functions and the Law of Cosines.
Section 2: Pseudocode for a Computer Program Derived from the Mathematical Algorithm (Future paper) This section will allow readers with some software competence to produce code in any language that will comply with the algorithm.
Section 3: Actual Code using the BX-24 chip and the Basic Express Development System compiler from Netmedia Inc. (Future paper) Section 3 is specific computer programming code that the author used on an actual robot to prove the implementation of the algorithm. The system described assumes the use of floating point or decimal arithmetic. Built-in trigonometry functions (sine, cosine, tangent), although not mandatory, would be a distinct advantage.
The mechanical requirements for the system are:
It is the author’s style to work with specifics wherever possible. The general solution is explained below for a single specific example. In the drawings to follow, Points A, B, C and D represent the northwest, northeast, southeast and southwest corners respectively, of a 13 X 21 foot, robot playing field. To keep all X,Y coordinates as positive numbers, Point D was arbitrarily chosen to be at position X=0, Y=0. Figure 1 shows the playing field coordinates.
Although the scenario will work with the playing field "pointed" in any direction, for clarity, Points A and B define the east-west line at the North boundary of the playing field.
An original playing field sized at 24 X 32 feet for another contest was to be used, however, the laser receptors degraded perceptibly past 25 feet, so the playing field was reduced so that the longest possible distance, the diagonal, was about 25 feet. Point R represents the exact position of the robot, which is unknown in the beginning. Also, Point D, the fourth corner of the rectangle is shown for viewer clarity, but it is never used in the computations or the geometry of the problem. The reflectors are intentionally placed at Points A, B and C, and NOT placed at Point D for several reasons. Picture yourself in the playing field, and rotating 360°. Since A and B are the closest together in degrees of arc, your timing should later confirm that your laser spent the least time in one sector, the northern one. Likewise, the placement at Points A and C, in your perspective, are placed farthest apart, so the laser will spend the most time scanning in the southwest sector. And of course, the eastern sector will be somewhere in between. This generally works except for a few special cases. There is of course one point where the angles between all three reflectors will all be 120 degrees. In this case, the robot would have to move, hopefully in a southwesterly direction, to resolve the problem. This algorithm also breaks down if the robot strays outside playing field, particularly if is wanders through the North or East boundary lines. For this paper, the robot is encouraged to avoid these ambiguous zones. Solving these ambiguities may well be a good topic for another paper.
From most points in the playing field, no matter which way you are originally pointing, since the laser always turns counterclockwise, you can only come up with three possible scenarios that make sense. You will sense:
Long time, medium time, short time, or
Medium time, short time, long time, or
Short time, long time, medium time
(Ignoring the single position in the playing field where all times are the same.) By recording the times between sensors, you will later be able to tell where you started out. Additionally, as we will see below, we only require three reflectors to determine an accurate position, so Point D has no reflector and is ignored.
This range-only navigation algorithm uses some geometry and trigonometry, so let’s review the math. Suppose you place two reflectors 13 feet apart, at Points A and B as shown in Figure 2.
If the robot is at Point R, angle R of the ARB triangle can be accurately measured. At this point, one might think you can do trigonometry here on the triangle, but it won’t work. Trigonometry requires three values to solve any triangle that doesn’t have a right angle of 90°. (You need to know three things; either Side/Angle/Side, Angle/Side/Angle, or Side/Side/Side for a solution.) So the trigonometry functions and the Laws of Sines and Cosines won’t help here.
However, there are several geometry rules that can get us to where we want. Notice the circle in Figure 2. Geometry states that given any three points not in a straight line, there is one and only one circle that can be drawn to touch all three points. Later, I’ll explain exactly how to find the center of that circle and draw it. Let’s say that you measure the angle R and see that it is 44°. Another geometry rule is that no matter where you put the robot on the circle, except points A and B, the angle between points A and B will always be the same, so angles P, Q, R and S are all the same. (In our example, 44°.) So the rule now allows us to say "If the angle from A to B is 44°, the robot MUST BE somewhere on this particular circle, but we have no idea exactly where on this circle." But we can be sure that the robot has to be on the perimeter of this particular circle. Now, if we add a third navigation reflector, (at point C) we can use one of the existing reflectors (Point B) and the new one to give us a second circle. Using these two reflectors will produce a second circle. The robot will be located somewhere on the circumference of the second circle also, hence the exact position of the robot is one of the two places where the two circles intersect (B or R in these drawings).
In Figure 3, the circle for Points A, B, and R is the same as in Figure 2.
A second circle is drawn that passes through points B, C, and R. Since the robot must be somewhere on the circumference of Circle 1 and also somewhere on the circumference of Circle 2, the robot can only be in one of the two places where the circles intersect, Point B or Point R. Since we know that B is the location of our reflector, we can throw out that position and accurately place our robot at Point R.
It remains but to show how to construct the circles, and then from these circles, determine point R as one of the intersection points of the circles.
To get the best accuracy possible, this algorithm presumes the robot stops, then takes readings. Stopping will give the best solution. The laser could rotate while the robot is moving, however the distance traveled per each laser rotation will introduce a corresponding error in the calculations.
First, let's make the timing and angle calculations. For this task, our example laser rotates counterclockwise once every four seconds. (The exact time per rotation is unimportant as long as the speed throughout the 360 ° circle is constant.) The author’s laser has a microswitch that "clicks" when the laser is pointed in a particular direction. For ease of computations later, it is best that the microswitch be adjusted so that it clicks just as the laser is pointing straight ahead (zero degrees relative) on the robot. Any method that lets the processor know when the laser is pointing a specific direction will work. Let’s rotate the laser, and start a timer when we get the "click". Now, in a 360° rotation, there are three reflectors, so your laser should get three readings before it gets back to the "click" point. The computer might record readings like those in Table 1.
EVENT AT TIME TIME BETWEEN
Click 0.0000 Sec
1st Reflection 1.0556 Sec 1.0556 Sec
2nd Reflection 2.3628 Sec 1.3072 Sec
3rd Reflection 2.8508 Sec 0.4880 Sec
Click 4.0000 Sec 1.1492 Sec
Table 1: Time Measurements
Notice several things here. The time from the first to second click was four seconds. This verifies that our laser is tracking at the correct speed. A reading not considered close should generate an error or warn the operator that something is wrong. Also notice that there were three reflections in the sequence. If there were less than or more than three reflections, we know that there has been a problem. Perhaps the laser missed a reflector or it got too many readings indicating one or more false reflections. In either case, this kind of result would show as bad to the computer which would throw out this set of readings and start over.
Also notice that the laser will usually "click" while the laser is pointing between two reflectors. To know the size of the arc involved on either side of the "click" point, we added a "TIME BETWEEN" column to Table 1. This allows us to add the time from the first click to the first reflection together with the time from the second click, backwards to the last (third) reflection, to determine the total time spent in this arc. In this example, we would add 1.0556 to (4.000 – 2.8508) = 1.0556 + 1.1492 = 2.2048 seconds. So the times we spent in each of the three areas, in counterclockwise order, are:
Figure 4a shows where we are at this point.
Remember, that at this point, although we can draw a nice neat Figure 4a, the robot has no idea about distance. Only the times and the ability to know which times represent which sector.
Next, we can compute the interior angles. Since the time spent in each sector is in
direct proportion the angle of the arc scanned, and 4.000 seconds equates to 360
Sector Angle = Time in Sector divided by Total Time times 360 But, since we want to allow the computer to work with values that are close to, but not
always exactly 4 seconds, we chose to read and store the total time for each revolution. Applying the formula to each arc time gives: 2.2048 sec / 4 sec X 360 1.3072 sec / 4 sec X 360 0.4880 sec / 4 sec X 360 -----------------
Total should
be: 360.0000° The situation at this point is illustrated in Figure 4b. You may notice that the line for the robot’s heading has been removed. That is
because we won’t need this for a while. Later, when we know the robot’s exact
position, we’ll come back to this to compute the direction the robot is heading. We don’t need the 198.4320° angle for our following computations, but it is handy
if we want to double check that all three angles add up to 360 From Figures 4a and 4b, we know that Points A, B and C are somewhere along the lines
drawn, but we can’t tell where exactly they should be drawn, so we will come from the
other direction in Figure 5. Initially, in Figure 5, we know is the distance between Points A and B is 13 feet and
that the ARB angle is 43.9200 Since we bisected line AB with the perpendicular line DE, we also know we have a right
triangle and that the lengths of the side BE is half of line AB or 13 feet / 2 = 6.5 feet.
Also, since we have a right triangle, the angle EBF is known to be 90 Solving the BFE triangle in Figure 5 will tell us two important things. First, Point F
is the center of the circle we are looking for. This requires us to find the length of
line EF so that we know exactly how far down from Point E we will find Point F. Second, we
need to know the length of the hypotenuse of triangle BFE (line BF) because this line
becomes the radius of the circle we will draw. Or - - - A circle, drawn from point F with
a radius FB, will pass through Points A, B and R. So we solve the triangle for those values. For the adjacent line DE: Tangent EFB = Opposite / Adjacent Transposing: Adjacent = Opposite / Tan 43.92°
= 6.750 feet Note: Without trig functions, you can solve the triangle by using the Pythagorean
Theorem where the hypotenuse squared equals the sum of the squares of the opposite and
adjacent sides. Or…..
c2 = a2 + b2 For the hypotenuse line BF (The radius of the circle): Sin BDE =
Opposite / Hypotenuse Transposing: Hypotenuse = Opposite / Sin 43.92° =
6.5 feet / 0.694 =
9.371 feet So, the center of the first circle is 6.5 feet to the right of Point A (X = 6.5 feet)
and 6.750 feet below line AB. (Y = 21 – 6.750 = 14.250 feet) All we have to do now,
is draw a circle from point F (x = 6.5, y = 14.25) with a radius of 9.371 feet and the
circle will pass through the three points A, B, and R. The completed portion for our first
circle is shown in Figure 6 along with the new dimensions we have computed. At this point, we know the robot is somewhere on this circle. Next, we are going to do
the same type of computations between Points B and C to develop the second circle. After
that, we will solve the two circles for their points in common. The next triangle to solve is triangle BRC that was first shown in Figure
3. There will be one difference you should note about our second circle. In Figure 3,
because the interior angle of the ARB inscribed angle was less than 90 Notice that to get twice the 117.648 Similar to what we did before, we are looking for the length of line GH to tell us how
far to the right of line BC the circle center is and for the radius of the circle which is
the hypotenuse line BH. Tangent BHG = Opposite / Adjacent Transposing… Adjacent = Opposite / Tangent BHG =
10.5 feet / Tangent 62.352° =
10.5 feet / 1.909 =
5.500 feet Sine BHG = Opposite / Hypotenuse Transposing… Hypotenuse = Opposite / sin BHG
=
10.5 feet / 0.886 =
11.8535 feet So, our second circle is to be drawn 5.500 feet to the right of line BC, making
x = 13 + 5.5
= 18.5 feet and half-way between B and C or
y = 21 feet / 2
= 10.5 feet. And a circle drawn from point x = 18.5 feet, y = 10.5 feet, with a radius of 11.8535
feet will pass through points B, R and C. Putting the Circles Together Figure 8 now has both circles we have constructed. Notice that Circle 1 and Circle 2 intersect at two points. The first is at x = 13, y =
21, or Point B, and the second intersection is at Point R. Shortly, we will solve the two
circles to get the exact location of Point R. Let’s review and consolidate at this point and take everything we know and apply
this information to Figure 8. We will also include the X-Y coordinates of the playing
field since we ultimately want to know the X-Y coordinates of our robot in relation to our
playing field. Point D was originally selected to be X=0 and Y=0 so that X and Y coordinates on the
playing field will be positive numbers. Points A, B, C and D are the corners of the playing field. Point F, the center of
Circle 1, is half the distance between A and B which is known to be 13 feet long, so X =
6.5. Y is the height of the playing field, 21 feet, minus the length of the side of the
small triangle, 6.75 feet, or Y = 14.25. Point G, the center of Circle 2, is half the
distance between B and C which is known to be 21 feet long, so Y = 10.5 feet. X is the
width of the rectangle, 13 feet, plus the length of the side of the small triangle, 5.5
feet, or X = 18.5 feet. Here is all of the data we have developed so far: Point
X Y
A 0
21
B 13 21
C 13
0
D 0
0
F 6.5 14.25
(The center of Circle 1)
G 18.5 10.5 (The center
of Circle 2)
R ?
? (The location of the robot. Still unknown!) …and we also know… Radius of Circle 1 = 9.371 feet Radius of Circle 2 = 11.8535 feet Finally! On to the Circles. Once we know the location of the two circles, we can use the general equation for a
circle, which is (X – H)2 + (Y – K)2 = R2 You may recognize the shorter form X2 + Y2 = R2 as the Pythagorean Theorem which works
when the circle is centered at 0,0. We’ll use the H and K as the offsets to the
circle centers and plug in our known values to look for X and Y. If we fill in the
information for Circle 1 in the equation, and the information for Circle 2 in an identical
equation, we will wind up with two equations with two unknowns which we attack with the
algebra rules for solving simultaneous equations. Circle 1: (X – H) X Simplify: X Circle 2: (X
– H) (X
– 18.5) X Simplify: X Now subtract Equation #1 from Equation #2: (Equation #2) X (Equation #1) -X (Equation #3) -24X + 7.5Y + 154.4976 = 0 And solve for X: -24X
= -7.5Y - 154.4976 Divide by –24: X
= 0.3125Y + 6.4374 Substitute this value for X into Equation #1: X (0.3125Y
+ 6.4374) Multiply: 0.0977Y2+4.0234Y
+41.4401 –4.0625Y –83.6862 + Y2 –28.5Y +157.4969 = 0 Combine like terms: 1.0977Y Divide by 1.0977: Y Solve by completing the square: (Move the number to the right:) Y Make the left side a square by taking half of the coefficient of Y and square it (
-25.9990 / 2 ) =
-12.99952 =
168.9870 and add that to both sides: Y Or (Y
– 13) Take square root of both sides: Y
– 13 = Y
= 13 + 8 =
21 and also… Y
= 13 - 8 =
5 So, the 2 values for Y are 21 and 5. (The computer doesn’t know it yet, but we can
see that 21 is the Y value for Points A and B, so 5 must be the Y value for the
robot’s location!) Substitute each of these values in Equation #3: For Y = 21: -24X
+ 7.5Y + 154.4976 = 0 –24X
+ 7.5(21) + 154.4976 = 0 -24X
+ 157.5 + 154.4976 = 0 -24X
+ 311.9976 = 0 -24X
= -311.9976 X
= 12.999 Feet = 13 Feet Since X = 13, Y=21 is the location of Point B, we can ignore this and get the location
of the robot next. For Y = 5: -24X
+ 7.5Y + 154.4976 = 0 -24X
+ 7.5(5) + 154.4976 = 0 -24X
+ 37.5 + 154.4976 = 0 -24X
+ 191.9976 = 0 -24X
= -191.9976 X
= 7.999 Feet X =
8 Feet So, FINALLY….. we find the robot at X
= 8, Y = 5.
Finding out where we are is a great achievement, but we need to go a few steps further.
Is the robot in its final resting place? Fine if it is, but most of the time it won’t
be, so we need to know three important things.
For illustration, let’s say that there is a power receptacle in the middle of the
west wall of the playing field, and we want the robot to return "home" and
recharge batteries. Refer to Figure 9 and let’s set up what we know. We know where the robot is located: X = 8, Y = 5 We know the plug is in the middle of the west wall that is 21 feet long so it is at: X = 0 feet Y = 21 / 2 = 10.5 feet There are several easy ways to find the length of the line JR. One is the Distance
Formula (Just another form of the Pythagorean Theorem) which is: d = Square Root of[(x2 – x1) d = Square Root of[(0 – 8) d = Square Root of[-8 d = Square Root of[64 + 30.25] d = Square Root of[94.25] d = 9.5 feet So 9.5 feet is the distance between Points R and J. To find out the direction to Point J, we can use trig to solve for angle R. Since we
know both the opposite side length and the adjacent side length, we can use: Tan R = Opposite / Adjacent Tan R = 5.5 feet / 8 feet Tan R = 0.6875 Angle R = 34.5085° Since robots typically can’t steer to this accuracy, 35 Last, but certainly not least is to compute how far and in what direction to turn to be
headed in the right direction. Remember way…. Way…. Back… just after Figure
1, I told you we were going to use the time between the first click and the first
reflection (1.0556 seconds) to determine which way the robot was pointing? Well, here is
where we do that. Go back and take a look at Figure 7 for a moment. We didn’t know it
then, but we now know the coordinates of Points R and C are: X
Y
R 8
5
C 13 0 To get from Point R to point C we would have to travel 5 feet to the right and 5 feet
down, and that means that angle BCR in Figure 7 is 45 1.0556 seconds is to w seconds as 4 seconds is to 360 w Cross-multiplying gives: 4w = 360(1.0556) 4w = 380.016 w = 95.004 So we know the robot started out pointed 95 You may have noticed a flaw above. We used the coincidence that the line RC was exactly
at 45 c Line RC RC RC RC
= Square Root of[50] RC
= 7.071 feet The Law of Sines is:
a b
c -------- = -------- = -------- sin A sin
B sin C Filling in knowns: 21 c
7.071 ----------------- = -------- = ----------- sin 117.648 Since we didn’t learn anything about C, we can throw the center term out and work
the proportion: 21
7.071 ----------------- = ---------- sin 117.648 Cross multiply: 21(sin B) = 7.071(sin 117.648 21(sin B) = 7.071(0.8858) 21(sin B) = 6.2636 sin
B = 6.2636 / 21 sin
B = 0.2983 Angle B = 17.3555 Since the interior angles of a triangle always add up to 180 Angle C = 180 Angle C = 44.9965 Or an angle of 45 So, it is possible to navigate a small autonomous robot using passive reflectors. There
is yet work to be done. Is it possible to use the algorithm to navigate outside of the
playing field? In his work cited below, Jim Ubersetzig successfully used three reflectors
along a wall for accurate positioning. But if the reflectors were free standing (no wall),
then would the robot wander past the plane of the reflectors and become "lost"
when on the wrong side of the reflectors? Is there a possible placement of three or more
sensors that will allow resolution of the ambiguities near corners and boundary lines? The
author hopes to pursue these and other navigation problems as this project matures. The following sources gave me the ideas and inspiration for this project. If you have
any additional resources relative to this algorithm, please let me know at http://www.trainprogrammers.com/ or richardv2@home.com. Borenstein, J., Everett, H.R., Feng, L. Where Am I?: Sensors and Methods for Mobile
Robot Positioning, University of Michigan, 1996 (CD-ROM with many robotics papers) Borenstein, J., Everett, H.R., Feng, L., Wehe, D. Mobile Robot Positioning –
Sensors and Techniques, Journal of Robotic Systems, September 1996 Everett, H.R.. Sensors for Mobile Robots: Theory and Application, A. K. Peters Ltd.,
1995 Jones, Joseph L. Seiger, Bruce A. and Flynn, Anita M. Mobile Robots: Inspiration to
Implementation, Second Edition, A.K. Peters, Natick, MA, 1999 McComb, Gordon. Robot Builder’s Bonanza: 99 Inexpensive Robotics Projects, Tab
Books, 1987, Chapter 29, Navigating Through Space Miller, Merl K., Winkless, Nelson B., Bosworth, Joseph H., Phillips, Kent. The Personal
Robot Navigator, A. K/ Peters, Ltd., 1998 Ubersetzig, Jim. A Circular Navigation System. The Robot Builder Newsletter for the
Robotics Society of Southern California, Sep, Oct, Nov 1999, http://www.dreamdroid.com/newsletter.htm
http://www.smartrobots.com/, Schwartz
Electro-Optics in Orlando Florida, Intelligent Solutions, Inc. in Marblehead MA, Denning
Branch Mobile Robotics in Pittsburgh PA and Guidance Control Systems in Leicester
(England?) all advertise laser reflector navigation kits for prices from $445.00 to
$12,000.00. There are probably others. Although none of these companies reveal the
algorithm used, it appears they use an algorithm similar to or exactly the same as that
described in this paper. I lay no claim to originality, original invention, patent or
other rights. I do, however, cheerfully reveal that I built the system described for under
$30.00! Home page for THE SAN DIEGO ROBOTICS SOCIETY http://www.sdrobotics.org/.
Note: If the rotation time was constant and never varied, total time would always be 4
seconds, and since it never changes, we could simplify the equation to:
Sector Angle = Sector time X 120 
= 6.5 feet / 0.963 
The Second Circle
= 10.5 feet / sin 62.352° 
(X – 6.5)
Which Way is the Robot Heading?
One More Thing!
Summary
Inspiration and Resources