Position sensing of robots, alternative solution with some maths. (And some strange ideas to play with)
By Hans A. Middelbeek
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°.
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:
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
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.
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
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.
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!