XU et al.: CEILING-BASED VISUAL POSITIONING FOR AN INDOOR MOBILE ROBOT WITH MONOCULAR VISION 1619
the direction vectors of the X
w
-, Y
w
-, and Z
w
-axis, and
c
p
w
=
[
c
p
wx
c
p
wy
c
p
wz
]
T
is the position vector of the origin O
w
,
expressed in the camera frame, respectively.
The camera’s parameters, including intrinsic and initial ex-
trinsic parameters, can be self-calibrated with the parallels
formed by the slots on the ceiling and a reference block [22],
[23]. The camera’s self-calibration is very helpful to improve
the flexibility and adaptability for the mobile robot to work
autonomously. For more general cases when the ceiling has
no parallels, the intrinsic parameters of the camera can be
calibrated in advance with traditional methods.
III. P
OSITIONING
A. Initial Pose Estimation
A block on the ceiling is selected as reference, whose size
and four vertexes are taken as known knowledge in the world
frame. Each vertex satisfies (3). Thus, the pose of the camera
relative to the reference block can be uniquely determined
with the four vertexes using PnP-based method [23]. In other
words, the initial pose of the camera, i.e., the transformation
matrix
c
T
w
of the world frame relative to the camera frame, is
determined
⎧
⎪
⎨
⎪
⎩
x
wi
c
n
wx
+ y
wi
c
o
wx
− x
1ci
x
wi
c
n
wz
− x
1ci
y
wi
c
o
wz
+
c
p
wx
− x
1ci
c
p
wz
=0
x
wi
c
n
wy
+ y
wi
c
o
wy
− y
1ci
x
wi
c
n
wz
− y
1ci
y
wi
c
o
wz
+
c
p
wy
− y
1ci
c
p
wz
=0
(3)
where (x
wi
,y
wi
,z
wi
) are the coordinates of a vertex in the
world frame; (x
1ci
,y
1ci
) are the normalized imaging coordi-
nates of a vertex, as given in
x
1ci
= x
ci
/z
ci
=(u
i
− u
0
)/k
x
y
1ci
= y
ci
/z
ci
=(v
i
− v
0
)/k
y
.
(4)
B. Orientation
For the indoor mobile robot working in planar floor, there are
only the motions moving along X
w
- and Y
w
-axis and rotating
around Z
w
-axis. Obviously, it is more convenient to investigate
the motions of the indoor mobile robot in the world frame.
Therefore, the motion of the camera at ith sampling moment
is denoted as
w
T
ir
=
⎡
⎢
⎣
cos φ
i
− sin φ
i
0 p
xi
sin φ
i
cos φ
i
0 p
yi
0010
0001
⎤
⎥
⎦
(5)
where (p
xi
,p
yi
) are the translations of the camera along X
w
-
and Y
w
-axis, and φ
i
is the yawing angle of the camera around
Z
w
-axis in the world frame.
The camera’s pose at ith sampling moment in the world
frame can be expressed as
w
T
ci
=
w
T
ir
w
T
c
. (6)
Then, the transformation from the camera frame to the world
frame can be obtained from (2), (5), and (6). This is expressed
in (7), shown at the bottom of the page.
1) Ceiling With Parallels: Parallels are helpful to determine
the orientation of a mobile robot [24]. For any two points in a
line parallel to X
w
-axis, their normalized imaging coordinates
satisfy (8). On the other hand, for any two points in a line paral-
lel to Y
w
-axis, their normalized imaging coordinates satisfy (9)
[22]. For simple, the upper index ci is denoted with i hereafter
i
n
wx
(y
1cx1
− y
1cx2
)+
i
n
wy
(x
1cx2
− x
1cx1
)
+
i
n
wz
(x
1cx1
y
1cx2
− x
1cx2
y
1cx1
)=0 (8)
i
o
wx
(y
1cy1
− y
1cy2
)+
i
o
wy
(x
1cy2
− x
1cy1
)
+
i
o
wz
(x
1cy1
y
1cy2
− x
1cy2
y
1cy1
)=0 (9)
where (x
1cx1
,y
1cx1
), and (x
1cx2
,y
1cx2
) are the normalized
imaging coordinates of two points in a line parallel to X
w
-axis;
(x
1cy1
,y
1cy1
) and (x
1cy2
,y
1cy2
) are the normalized imaging
coordinates of two points in a line parallel to Y
w
-axis; they are
computed from their image coordinates with (4).
Applying the expression of the parameters in (7) with vari-
able φ
i
to (8) and (9), we have
[
c
n
wx
(y
1cx1
− y
1cx2
)+
c
n
wy
(x
1cx2
− x
1cx1
)
+
c
n
wz
(x
1cx1
y
1cx2
− x
1cx2
y
1cx1
)] cos φ
i
=[
c
o
wx
(y
1cx1
− y
1cx2
)+
c
o
wy
(x
1cx2
− x
1cx1
)
+
c
o
wz
(x
1cx1
y
1cx2
− x
1cx2
y
1cx1
)] sin φ
i
(10)
[
c
o
wx
(y
1cy1
− y
1cy2
)+
c
o
wy
(x
1cy2
− x
1cy1
)
ci
T
w
=
c
T
w
w
T
−1
ir
=
⎡
⎢
⎣
c
n
wx
cos φ
i
−
c
o
wx
sin φ
i
c
n
wx
sin φ
i
+
c
o
wx
cos φ
i
c
a
wx
c
n
wy
cos φ
i
−
c
o
wy
sin φ
i
c
n
wy
sin φ
i
+
c
o
wy
cos φ
i
c
a
wy
c
n
wz
cos φ
i
−
c
o
wz
sin φ
i
c
n
wz
sin φ
i
+
c
o
wz
cos φ
i
c
a
wz
000
c
n
wx
(p
xi
cos φ
i
+ p
yi
sin φ
i
)+
c
o
wx
(−p
xi
sin φ
i
+ p
yi
cos φ
i
)+
c
p
wx
c
n
wy
(p
xi
cos φ
i
+ p
yi
sin φ
i
)+
c
o
wy
(−p
xi
sin φ
i
+ p
yi
cos φ
i
)+
c
p
wy
c
n
wz
(p
xi
cos φ
i
+ p
yi
sin φ
i
)+
c
o
wz
(−p
xi
sin φ
i
+ p
yi
cos φ
i
)+
c
p
wz
1
⎤
⎥
⎦
=
⎡
⎢
⎣
ci
n
wx
ci
o
wx
ci
a
wx
ci
p
wx
ci
n
wy
ci
o
wy
ci
a
wy
ci
p
wy
ci
n
wz
ci
o
wz
ci
a
wz
ci
p
wz
0001
⎤
⎥
⎦
(7)