 |
|
Position sensing of robots,
alternative solution with some maths. (And some strange ideas to play with)
By Hans A. Middelbeek
The Netherlands
In his article way back in
2001 from Richard T. Vannoy a very nice desription was given for position finding
of robots. Although the technique described in the underlying seems the same, I
halfway introduce a simpler method to find the robot’s position as the
substitution part in the first article proves to be rather complex.
In order to test the approach
I wrote a very simple Visual Basic program which will be presented here.
Suppose we have three
reflectors (or lights) which can be seen from the robot R:
Point A is at (0,0), point B at (XLength,0) and point C at (0,YLength). Angle A is 90°.
Figure 1
With different techniques we
could measure angles Angle1 (angle between A and B) and Angle2 (angle between A
and C) as seen from the robot R. At the end of this article a somewhat strange
idea is described.
Our Robot R will be at a
position on two circles (one containing A, B and R and the other containing A,
C and R), and thus will be at the point of intersection of both circles. (In
fact the two circles intersect at two points, R and A)
We first calculate the centre
of those circles:
‘Circle 1
DegreesToRadians =
3.14159/180
Radius1 = 0.5 * (XLength / Sin(Angle1 * DegreesToRadians)) 'Sinusrule! Basic works with
radians and not with degrees!
Angle1a = 180 - Angle1 ‘Found this by accident, don’t know why I need to do this
Sinus1 = (0.5 * XLength) / Sin(0.5 * Angle1a * DegreesToRadians)
Cosinus1 = Cos(0.5
* Angle1a * DegreesToRadians)
X1 = XLength / 2 'Centre must be halfway between A and B
Y1 = Radius1 - Sinus1 *
Cosinus1
‘Circle 2
Radius2 = 0.5 * (YLength / Sin(Angle2 * DegreesToRadians))
Angle2a = 180 - Angle2
Sinus2 = (0.5 * YLength) / Sin(0.5 * Angle2a * DegreesToRadians)
Cosinus2 = Cos(0.5
* Angle2a * DegreesToRadians)
X2 = Radius2 - Sinus2 *
Cosinus2
Y2 = YLength / 2 'Centre must be halfway between A and C
The two intersection
points A and R can be calculated now by substitution of the two equations of
the found circles. However, it is easier to follow another approach: first we
find the line connecting the centres of the two circles (the algebraic equation
of this line is: y = mo * x + n). For this we calculate the directional
coefficient (mo) and offset (n) of the line.
Figure 2
mo = (Y2 - Y1) / (X2 - X1)
n = Y1 + ((Y1 - Y2) * X1) /
(X2 - X1)
Points A and R
will be on a second line, perpendicular to the first one. To find this line we
simply have to find a line with a directional coefficient that is 90° on the
first one and starting at (0,0). The equation of this
second line is y = mn * x + z
alfa = Atn(mo) 'z, the offset, equals 0 because this line will start at
(0,0)
mn = Tan(alfa - 90 *
DegreesToRadians) ' this line is perpendicular to first line
Now we can do a
much simpler substitution to find the intersection of these lines:
XIntersect = -n / (mo - mn)
YIntersect = mn * XIntersect
Finally we have
to realise that the second intersection of the two circles is also at the
second line, but at twice the distance of the intersection of the two lines is
from (0,0).
XRobot = Int(2
* XIntersect + 0.5)
YRobot = Int(2
* YIntersect + 0.5)
One possible, not yet tested
approach for finding the angles between the three points I’ve been thinking of
could be as follows: Suppose we use a videocamera equipped with a conical
mirror on top (figure 3). Via the mirror we can see the 360° image of the room
like in figure 4.
Figure 3
Figure 4
All we now have to do is to
find the bright purple, green and blue spots on the image and calculate the
angles. I think it even must be possible to calculate yaw, pitch and roll of
the robot based on the distances between the centre of the image and the
separate spots. (Did not even have time to figure this out)
Hope this brings some new
ideas!