Page 183 - Autonomous Mobile Robots

Landmarks and Triangulation in Navigation 167

L
L
Assume that the position of the lens center is (x , y ). According to
c c
Pythagoras theory, we have:
L L 2 L 2 2
(p − x ) + (y ) = l
1 c c 1
(4.24)
L
L 2
L 2
(p − x ) + (y ) = l 2
2 c c 2
L
Considering that y is always a positive value, we have:
c
 2 2 L 2 L 2
l − l + p − p
 L 1 2 2 1

x =
c L L
2(x − x )
2 1 (4.25)

 L 2 L L 2

y = l − (x − p )
c
c
1
1
In Figure 4.9, the direction may be easily obtained as:
L L I
p − x x r
−1 1 c −1 1
θ = ∠1 + ∠2 = tan + tan (4.26)
y c L f
By combining Equation (4.25) and Equation (4.26), we have
 2 2 L 2 L 2 
l − l + p − p
1 2 2 1
 L L 
 L   2(p − p ) 
x c  2 1 

 2 
L
L 2
 L 
y  =  l − (x − p )  (4.27)
c
c
1
1


θ L  
L
c  p − x L I 
1
tan + tan
 −1 1 c −1 x r 
y L f
c
L
We substitute P into Equation (4.21), and then the localization is com-
pleted. Equation (4.27) is the result of localization. The coordinates given by
the program are in {L}. The errors of this method are caused by the imprecise
extraction of each character. Differentiating Equation (4.27), we have:
1
 
(l 1 dl 1 − l 2 dl 2 )
L
L
(p − p )
 
 L   2 1 L L 
dx c  l 1 x − p 1 
c


dl 1 −
dx L 
L  2 2 (4.28)
 c 
c 2 L L 2 L L
dy  =  l − (x − p ) l − (x − p ) 
c
c
1
1
1
1
dθ L  
c  I L L L 
dx 2y (p − x ) dy
 L 
c
c
L
 1 2 − 2 1 dx + c 
c
2
L
L 2
I
1 + (x r/f ) y L c + (p − x ) y L c
c
1 1
© 2006 by Taylor & Francis Group, LLC
FRANKL: “dk6033_c004” — 2006/3/31 — 16:42 — page 167 — #19