- Email: [email protected]
Incremental Map-Making in Indoor Environments
INCREMENTAL MAP-MAKING IN INDOOR ENVIRONMENTS A. EKMAN, P. KLOOR and D. STROMBERG Naiicllal De/ellet' Re~af'('h Establish",ellt, Dept.
0/ In/on/lOtion
T~chnolog.\:·
unkiiping,
Sl'.~den
Abstract. This paper presents an algcrith:n that transfonns la~r range data from indoor environments into a bird's cyc vicw of thc robot swr:>undings. The algorithm is intendeJ to be used for semiautonomous robots, wherc thc operator needs a global vicw of the cnvironment. Emphasis is put on the important Line Extraction step, where rwo approaches are mentioned, one of which is a new Polyline Segmentation approach. Input data comes from odomerry and a laser range ~nner. Key words. Baycsian statistks; Hough Transfonn; Polyline Segmentation; Robots; State estimation
1. INTRODUCTION
1.1. DefInitions
In critical applications, such as bomh disposal, a semi-autonomous robot, monitored by an expert operator, is superior to an autonomous robot. This is due to the diffkulty to quantify expert knowledge.
The state of the robot Le; defined as:
A prohlem that has been found during experiments with semi-autonomous robots is the problem for an operator to perceive the robot position in the environment, if only views from TV-cameras are available.
where s corresponds to range. y to bearing and heading, while the range data tuple Le; defmed ae;:
(1)
S - «r l • !pj),(r2. !P2) ' ...• (rll/ ' !Pili))
to
(2)
The transition vector Le; defmed as (see Fig. 2) :
The Map-maldng algorithm is intended to reduce this prohlem hy providing a bird's eye view of the robot surroundings for the operator. The complete algorithm can be divided into two main steps; position estimation and map database maintenance . In Fig. I, the three rightmost boxes describes the fIrst step, while the two boxes to the left describes the second step.
T
(3) and the odometer measurement of the transition is defined ae;:
(4)
Bird's eye view Laser d
x Fig. 2. Mapping of mca!>1ll'emenl poinls lu Ihe global coordinate system.
Fig . 1. Map-milking algorithm outline. TIle Laser Wtlll arc used both [ur posiliun cstimllliul1, using Ihe Odumcter data as an initial guess, and as environmental infonnation for the Map datahase .
187
Hou~h
2. LINE EXTRAcrION
Transform
The primitives to he used are the walls in the indoor environment. To be able to identify a wall in a new measurement as a wall in a previous one, it is necessal}' to perform a mapping based on lbe Slate transition (~~), which is indicated in Fig. 3.
The Hough Transform (HT), is a common method for line detection. It is tolerant to noise, but computational heavy. In the case of range data, though, the HT can he computed rather fast, provided moderate resolution is used in the calculation.
Eqs. (5) and (6) summari7.es the parameter dependencies that are found in Fig. 3.
Below follows a vcry brief description of the developed HT calculation. A more thorough description is presented in Ekman et al. (1992).
,
,
p - p + As ' cos (a + ~T
a
E
a' + ~T
- ~y)
(5)
(6)
x Fig. 5. 1IIu.c;tration, in the spatial domain, of the idea with the HT. Ea(.:h line (out of 11 discrete, predetermined set) is tcsted for how many measurement points it covers. Fig . 3. Relations hetween coordinates of a wall. as seen from rn'o different scan positions.
It is fruitful to transform the contents of Fig. 5 to the Hough domain:
The appearance of the range data is shown in Fig. 4.
p r
~
0
'"
./
J
'\
:\
f.
V
\
/
~
"-. cp-B
8
V
= 90°
Fig. 6. Spatial points and lines visuali:>:ed in the Hough domain. The cosine curve corresponds 10 Ihe mea· surement point that lies on th~· tip of the (p,B)-arrow in Fig . 5, while the Idtmost point in Fig. 6 wrrespontis to Ihe solid line in Fig. 5. The numher of points in vertical direction is depending on Ihe derivative of the cosine curie. Fig . 4. Typical inpl;t data from range sensor. The numher of IDeasureIDen; points (m) is 3200. Robot position is marked with a white dol.
To get an understanding for the HT calculation, it is useful to think of m cosine curves (onc for each measurement point) with distinct phase angles (cp). For each curve, an iteration is performed in angular direction (8), and for each angle. a certain number of points arc incremented (sce Fig. 6).
2.1 . Line extrdction methods Two different line extraction methods are suggested. The Hough Transform was the tirst approach. while the Polyline Segmentation Algoritlun was developed as a faster, and in many cases more precise, line extrdction mdhod .
The expression below states the same in a mathematical way (assuming ~8 = 2n/a) :
]88
i-I. 2.....
11/
(j - O. 1. .... a-I
(7)
rolyline
Se~mentation A1~orithm
(PSA)
Fig. 7 and Fig. 8 illustrates the principle of rSA. DIST corresponds to the distortion, based on an EigenvcctoT Fit Criterion defined below (Refer 10 Eq. (9) and Fig. 12 for an illuslration of the parameters involved) .
•
• i= 1
i-2
i=3
Fig . 7. The two lirsI data points arc coWlCCtcd .
• • • Fig. 8. The line is extended 10 include a third point in a way Ihat minimiles DIST (solid line) . Notice the Mnoating" end-points of the line. reducing the impact of starting in a heavily distorted point. The broken line inwcalL's Ihe position of thc linc if il is possible 10 add point 4 . If nOI. a new line is created. slarting from point 4.
Fig. 9. Line segments (white) resulting from application of PSA on the range data in Fig . 4 .
When using rSA, a post-processing step. where segments with close coordinates arc merged into new segments. is necessary to make the algorithm less sensitive 10 noi<>e. The result is illlL<;traled in Fig. 10.
Ddine line L, covering poinls n. n+ I ..... n+k-I as: (8)
L - (p. 8) 11 . 1:
Define the distortion for line L a<;: 11 • , . -
1
DISTIl. ,. - k - 1
I
I.
~7
(9)
, • IJ
where
~i
is defined as (sec Fig. 12): ( 10)
The polyline segmentation algorithm can then he stated as (assuming m data poinIS): O. n"'l I. k"'2
2. Create line between poinl nand n+ I
Fig . 10. Rang" data after post-pf(x:essing and thre~holdi.ng. Segmcnt endpoints lire mark,'d with ' + '-sigru;.
3. Calculate line (.0. 8 ) 11 , I: + I minimizing DISTil , L. J 4. IF DISTil. k + I < THRESHOLD AND n+k
3. WALL IDENTIFICATION
The only non-trivial step is 3 .• which is closer described in APPENDIX .
This part of the algorithm performs an interpretation from line segmenls 10 walls. These walls are compared to Ihe walls extracted in previolL<; scans, to generate a one-to-one mapping between the set of walls. Since walls may become occluded or new walls may hecome vi<;ihle as the robot moves, it is necessary to inlroduce a notation for the state of a wall. Each wall is either New. Old or Active. A wall that becomes visible in the current scan is labeled New. while a previously visihle wall that has become occ1ud~d in this scan is la!leled Old. All walls
189
that are visible in both current and previous scans are labelled Active. The walls with Active-labels can be divided into pairs, and are used to calculate the transition in the next step. (An algorithm for conducting the wall identification is given in Ekman et al. (1992».
6. PRESENTING BIRD'S EYE VIEW The updated, global Certainty Grid contains the bied's eye view that will be displayed for the operator. Different presentation approaches have been examined; either to output the Certainty Grid as it is, with the value of cach cell mapping to a unique magnitude on the dic;play, or to first perfonn a thresholding to foclL<; on the areas with high probabilities of being occupied.
4 . CALCULATION OF TRANSITION By inspecting Eq. (5) and (6), it is obvious that it is now possible to calculate the transition since (p', e') corresponds to the wall expressed in the new coordinates and (p, 6) corresponds to the wall expressed in the old coordinates. Each wall-pair offers an estimate of ~~ calculated through the equations below: ~T -
~y -
- alan
(
e- e
(11)
d;cos8j - djcos8; \
l
I
d I.sin8J - dsin8 .) J I
+ k·
1t
(12)
where dl.. - PI.. - PI..' The range (~s) is calculated by inserting the expressions for ~T and ~y in Eq. (5): ~s
-
(p-p') / cos(8+~y )
Fig. 11. Ccrtainty Grid after onc scan (left). Cerlainly Grid after two scans (right) . Each cell has a colour (thal unfortunately can't he seen in this paper) corresponding to its prohahility of heing occupied .The size of the Certainty Grids has been 200*200.
7. CONCLUSIONS
(13)
Since ~y has two solutions, so has also~. However, only onc solution gives a positive value of ~s . The ditlerent estimates can be combined in a number of ways, for instance a Kalmall filter can be used.
5. MAP UPDATING
-
When the estimate of ~~ has been calculated, the local Certainty Grid is created. The creation involves the usage of the updating formulation of 8ayes' theorem given in Elfes (1990). Thereafter, the global Certainty Grid can be updated by using the relations in Fig. 2 to transf:)rm between the coordinate systems.
5.1. Data repn;sl:ntation jn Map databasl: The data representation is similar to the onc described in Elfes (1990), with a few changes . The most significant is the use of polar coordinates in the local Certainty Grid . This conforms to the rddial properties of the laser sensor and enables fast updating of the local Grid, since the predicate "Cell i lies behind cell j" will be easily verified. When the local Certainty Grid has been created, it is mapped to the glohal Grid, which has Cartesian format. There is no guarantee to have a one-to-one mapping, but this is the prize to pay for higher speed.
The recursive fonn of PSA enahles a fast execution, independently of the number of points already covered by a line (i e there is no need to recalculate all sums for each new mea<;urement point added) . A distinguL<;hing property of the presented algorithm is the use of both odometry and range data for position estimation. Another key feature is the incremental map maintenance, based on Certainty Grids.
Another approach wltich involves incremental mapmaking is found in AndeTS\:n et al. (1992), though the position estimation i<; bac;ed solely on odometry.
An incremental map-making algorithm with a position estimation based on both odometry and range data is found in Chatila and Laumond (1985) . The map is represented as a set of polyline segments (to he compared with the Certainty Grid approach in this paper). Topics for future research is the augmentation of a 30model, defined for areas where ohjects are to he manipulated . Thus, since the robot moves mainly horizontally. a 20 model is sufficient fOT navigational tasks, wltile a 3D model is necessary for most tasks involving manipulation of ohjects. Investigation of the connection between a 20 and a 30 model have heen started and will be funher examined.
190
8. REFERENCES
DIST • - _1_(1 . p2 + cos 2e'A + sin 2e· B + n.~ k-I
Andersen C.S. et al. (1992). Navigation using range images on a mobile robot. Robotics and Autonomous systems, 10. 147-160. Chatila R. J-P Laumond (1985). Position referencing and consistent world modelling for mohile robots. IEEE 1. of Robotics &. Automation. pp. 138-145. Duda R.O .• P.E. Hart (1973). Pattern Cla.......ijication and Scene analvsis. WHey & Sons. New York. Ekm:m. A. et al. (19n). Incremental map-making based on range images. FOA Report C 30672-3.4 ISSN 0347-3708. Elfcs. A (1990). Occupancy Grids: A Sto;~as'ic Spatial Representation for Active Rohot Perception. Proc. afsixth CO/if. 01/ Uncenainty in Al. July 1990.
(16)
2 · sine· cose · C- 2p(cose· D + sine · E)
Ncxt. the cxpression will be reformulated by isolating p:
DISTn. t
k 1 - 1 ( (p
-
-
cose ' D+sine ' E 2 1 ) +
D2 E2 · (A--) +sin 2e · (B--) + k t DE 2 . sine'cose . (C_ » k
I
_«(..'OS2 e
k
(17)
The squared parenthesis is always ~ O. implying that to minimi7.c the distortion, the optimal p (p) will be chosen as (when knowing the optimal 6
(a »:
9. APPENDIX
cosa . D + sina . E
p-
9.1. Derivation of the line calculation in PSA Below follows the derivation of the equation.o; needed to pcrfonn the calculation of the coordinates of a line (p. e). The criterion LL<;ed is Eigenvector fit, ao; described in Duda and Hart (1973) (see Fig. 12).
(18)
k
Equation (18) can be reformulated ao;: n+k-I
1 ~
P '"' k J..
. r j ' cos (e -~)
(19)
i· n
which shows that p in fact is calculated as the arithmetic mean value of the orthogonal projections of measurement points along a line through the origin. and whose angle component e is equal to B. I1 is also easy to sce that p cannot be negative. Fig. 12. Th.' dislam:1: lIsed in lhc Eigcnvl:clor Iil crilerion (~). for a point (r.
In the sequel. it is assumed that p is selected as in Eq. (18). turning Eq. (17) into the following expression:
A; can be rewritten as:
I
DIST - k _ 1 ( (cos t:.i
= ri '
(COS~i '
cose + sin~i' sine) - p
2
sin e · (B-
I
= --
k- I
(
,
l k· p- + cos
2
6·
I·
1
r
('i s/
COS
6·.r
I· "
+ 2 . sin 6 . cos6'
DIST
r
,;cjsi - (15)
,,+k-1
6·.r
E~
+
;·11 'iC'i + sin
T ) +2sin6cos6 ' (C- T
1- B-k
I
n+t-)
i - n "+A: - j
21' (
I
n
,,+t-)
sin 6 ·
(r.e .) 2
~
.J..
DE
»
DE J - C- k
(21)
leading to the following equation describing the distortion:
,,+t-j ". t
E2
Nexl introduce H, I and J, deflDe.d as:
By inserting Eq. \14) into Eq . (9). it is possible to express the distortion in the following manner:
DIST
e . (A - T) ) + (20)
(14)
In this equation. COS~i and sin~i can be calculated in advance and rut in a tahle of size 3200*2. Let Cj denote cos(q;,) and Si de.)o~e sin(~,), where ~j-i*21t/3200.
D2
2
',si
JJ
, • "
Name the sums A. B. L. D and E n ..-spcctively:
191
c
.-2-,,- I
2
2
(cos e· H + sin e· 1+ 2sinecos6 ' J) (22)
The minimum can he found hy calculating the derivative with respect 10 6 in Eq. (22). which will be done on the following analogous expression:
I 1 + cos (28) 1 - cos (28) DIST---( ' H+ ,1+ k- I 2 2
(23)
sin(28) ' J)
which, before calculating the derivative, can be simplified to: DlST - -
1
J.: - J
H-/ H+/ (cos (28) . - - + - - + sin (28) ' J)(24)
2
2
hy calculating p with one of tbe 8's, a positive p indicates that the correct 8 has been chosen, whereas a negative p leads to a negation of p and an addition of 1800 to e. The coordinates of the line will be updated with (p.8) just calculated (Broken line in Fig. 8). If, on the other hand, DIST exceeds THRESHOLD, a new line will be created, starting from this last point that could not be added to the current line (The two solid lines in Fig. 8).
The derivativ~ becomes: 9.2. The Recursjye Updatini Scheme
(25)
Below follows a summary of the recursive calculation of the five partial sum., (A,B, ... ,E).
where K is defmed as: H-l
K=
(26)
2
Set 2e = q and the derivative" 0, yielding: cos (q) . J
~
sin (q) . K
=
J alan ( -) + I' . K
It
e
I
J
2
K
EZ
I'
which in turn gives four solutions for
k - 0
A". t - A".t-I + (r,, ' c,,)2
k - I. 2 ....
B".t - 0
k - 0
(27)
On the interval [0, 27t1. two possihle solutions will occur [or q: q
A".t - 0
k - I. 2. ...
(28)
a:
= - ' aI3n ( -)+I"71 / 2
,. E Z
C".,. -
0
C". t
=
C". t _ I
D".t
e
0
k - 0
,
+
r~ . e" . s"
/.: =
I. 2 . ...
/.: =
0
/.: =
I. 2 ...,
(29)
Define the function W through:
w_(O l 71
K> 0 K< 0
(30)
D". ,. aD". t
_I +
r" . C"
(The case K -0 needs special treatment) Now, the 7eros of the derivative can be expressed uniquely in q in the following way:
£".,
=
0
£". t - £". tq
~
J K
which is unique on the interval This am be
trdnsfonn~
1
e - -2
(31)
alall ( -) + W
to, 2nl.
to an expression in
J (alan ( -) +
K
r,, ' Sn
E
k
E
0 I. 2. ...
From the equations ahove, J and K can he calculated as:
a:
J
_ C ".t
w ± 71)
I +
/.:
". t
_ D ". t E " . t k
(33)
(32)
Each of these two values, suhstituted for a in Eq. (22), will give the same value for DIST. Thus, any of the values will do in the calculation of DIST. After DIST has heen calculated with any of the solutions for a, it is compared to a predetermined value - THRESHOLD. If it is less than THRESHOLD, p is calculated hy suhstituting a for El in Eq. (18)
D K
n. t
C
2J (..4 ".t - T". t
£
2 - B
n. t
2
+...!!:..!:)
J:
(34)
It is possihle to express the calculation of J and K through recursive equations as well. but the gain in uniformity does not exceed the degradation of execution speed.
In the calculation of p. the selection of a does maller. Inserting wrong e in Eq. (18) yield<; a negative p. Thus,
192
0/ In/on/lOtion
T~chnolog.\:·
unkiiping,
Sl'.~den
Abstract. This paper presents an algcrith:n that transfonns la~r range data from indoor environments into a bird's cyc vicw of thc robot swr:>undings. The algorithm is intendeJ to be used for semiautonomous robots, wherc thc operator needs a global vicw of the cnvironment. Emphasis is put on the important Line Extraction step, where rwo approaches are mentioned, one of which is a new Polyline Segmentation approach. Input data comes from odomerry and a laser range ~nner. Key words. Baycsian statistks; Hough Transfonn; Polyline Segmentation; Robots; State estimation
1. INTRODUCTION
1.1. DefInitions
In critical applications, such as bomh disposal, a semi-autonomous robot, monitored by an expert operator, is superior to an autonomous robot. This is due to the diffkulty to quantify expert knowledge.
The state of the robot Le; defined as:
A prohlem that has been found during experiments with semi-autonomous robots is the problem for an operator to perceive the robot position in the environment, if only views from TV-cameras are available.
where s corresponds to range. y to bearing and heading, while the range data tuple Le; defmed ae;:
(1)
S - «r l • !pj),(r2. !P2) ' ...• (rll/ ' !Pili))
to
(2)
The transition vector Le; defmed as (see Fig. 2) :
The Map-maldng algorithm is intended to reduce this prohlem hy providing a bird's eye view of the robot surroundings for the operator. The complete algorithm can be divided into two main steps; position estimation and map database maintenance . In Fig. I, the three rightmost boxes describes the fIrst step, while the two boxes to the left describes the second step.
T
(3) and the odometer measurement of the transition is defined ae;:
(4)
Bird's eye view Laser d
x Fig. 2. Mapping of mca!>1ll'emenl poinls lu Ihe global coordinate system.
Fig . 1. Map-milking algorithm outline. TIle Laser Wtlll arc used both [ur posiliun cstimllliul1, using Ihe Odumcter data as an initial guess, and as environmental infonnation for the Map datahase .
187
Hou~h
2. LINE EXTRAcrION
Transform
The primitives to he used are the walls in the indoor environment. To be able to identify a wall in a new measurement as a wall in a previous one, it is necessal}' to perform a mapping based on lbe Slate transition (~~), which is indicated in Fig. 3.
The Hough Transform (HT), is a common method for line detection. It is tolerant to noise, but computational heavy. In the case of range data, though, the HT can he computed rather fast, provided moderate resolution is used in the calculation.
Eqs. (5) and (6) summari7.es the parameter dependencies that are found in Fig. 3.
Below follows a vcry brief description of the developed HT calculation. A more thorough description is presented in Ekman et al. (1992).
,
,
p - p + As ' cos (a + ~T
a
E
a' + ~T
- ~y)
(5)
(6)
x Fig. 5. 1IIu.c;tration, in the spatial domain, of the idea with the HT. Ea(.:h line (out of 11 discrete, predetermined set) is tcsted for how many measurement points it covers. Fig . 3. Relations hetween coordinates of a wall. as seen from rn'o different scan positions.
It is fruitful to transform the contents of Fig. 5 to the Hough domain:
The appearance of the range data is shown in Fig. 4.
p r
~
0
'"
./
J
'\
:\
f.
V
\
/
~
"-. cp-B
8
V
= 90°
Fig. 6. Spatial points and lines visuali:>:ed in the Hough domain. The cosine curve corresponds 10 Ihe mea· surement point that lies on th~· tip of the (p,B)-arrow in Fig . 5, while the Idtmost point in Fig. 6 wrrespontis to Ihe solid line in Fig. 5. The numher of points in vertical direction is depending on Ihe derivative of the cosine curie. Fig . 4. Typical inpl;t data from range sensor. The numher of IDeasureIDen; points (m) is 3200. Robot position is marked with a white dol.
To get an understanding for the HT calculation, it is useful to think of m cosine curves (onc for each measurement point) with distinct phase angles (cp). For each curve, an iteration is performed in angular direction (8), and for each angle. a certain number of points arc incremented (sce Fig. 6).
2.1 . Line extrdction methods Two different line extraction methods are suggested. The Hough Transform was the tirst approach. while the Polyline Segmentation Algoritlun was developed as a faster, and in many cases more precise, line extrdction mdhod .
The expression below states the same in a mathematical way (assuming ~8 = 2n/a) :
]88
i-I. 2.....
11/
(j - O. 1. .... a-I
(7)
rolyline
Se~mentation A1~orithm
(PSA)
Fig. 7 and Fig. 8 illustrates the principle of rSA. DIST corresponds to the distortion, based on an EigenvcctoT Fit Criterion defined below (Refer 10 Eq. (9) and Fig. 12 for an illuslration of the parameters involved) .
•
• i= 1
i-2
i=3
Fig . 7. The two lirsI data points arc coWlCCtcd .
• • • Fig. 8. The line is extended 10 include a third point in a way Ihat minimiles DIST (solid line) . Notice the Mnoating" end-points of the line. reducing the impact of starting in a heavily distorted point. The broken line inwcalL's Ihe position of thc linc if il is possible 10 add point 4 . If nOI. a new line is created. slarting from point 4.
Fig. 9. Line segments (white) resulting from application of PSA on the range data in Fig . 4 .
When using rSA, a post-processing step. where segments with close coordinates arc merged into new segments. is necessary to make the algorithm less sensitive 10 noi<>e. The result is illlL<;traled in Fig. 10.
Ddine line L, covering poinls n. n+ I ..... n+k-I as: (8)
L - (p. 8) 11 . 1:
Define the distortion for line L a<;: 11 • , . -
1
DISTIl. ,. - k - 1
I
I.
~7
(9)
, • IJ
where
~i
is defined as (sec Fig. 12): ( 10)
The polyline segmentation algorithm can then he stated as (assuming m data poinIS): O. n"'l I. k"'2
2. Create line between poinl nand n+ I
Fig . 10. Rang" data after post-pf(x:essing and thre~holdi.ng. Segmcnt endpoints lire mark,'d with ' + '-sigru;.
3. Calculate line (.0. 8 ) 11 , I: + I minimizing DISTil , L. J 4. IF DISTil. k + I < THRESHOLD AND n+k
3. WALL IDENTIFICATION
The only non-trivial step is 3 .• which is closer described in APPENDIX .
This part of the algorithm performs an interpretation from line segmenls 10 walls. These walls are compared to Ihe walls extracted in previolL<; scans, to generate a one-to-one mapping between the set of walls. Since walls may become occluded or new walls may hecome vi<;ihle as the robot moves, it is necessary to inlroduce a notation for the state of a wall. Each wall is either New. Old or Active. A wall that becomes visible in the current scan is labeled New. while a previously visihle wall that has become occ1ud~d in this scan is la!leled Old. All walls
189
that are visible in both current and previous scans are labelled Active. The walls with Active-labels can be divided into pairs, and are used to calculate the transition in the next step. (An algorithm for conducting the wall identification is given in Ekman et al. (1992».
6. PRESENTING BIRD'S EYE VIEW The updated, global Certainty Grid contains the bied's eye view that will be displayed for the operator. Different presentation approaches have been examined; either to output the Certainty Grid as it is, with the value of cach cell mapping to a unique magnitude on the dic;play, or to first perfonn a thresholding to foclL<; on the areas with high probabilities of being occupied.
4 . CALCULATION OF TRANSITION By inspecting Eq. (5) and (6), it is obvious that it is now possible to calculate the transition since (p', e') corresponds to the wall expressed in the new coordinates and (p, 6) corresponds to the wall expressed in the old coordinates. Each wall-pair offers an estimate of ~~ calculated through the equations below: ~T -
~y -
- alan
(
e- e
(11)
d;cos8j - djcos8; \
l
I
d I.sin8J - dsin8 .) J I
+ k·
1t
(12)
where dl.. - PI.. - PI..' The range (~s) is calculated by inserting the expressions for ~T and ~y in Eq. (5): ~s
-
(p-p') / cos(8+~y )
Fig. 11. Ccrtainty Grid after onc scan (left). Cerlainly Grid after two scans (right) . Each cell has a colour (thal unfortunately can't he seen in this paper) corresponding to its prohahility of heing occupied .The size of the Certainty Grids has been 200*200.
7. CONCLUSIONS
(13)
Since ~y has two solutions, so has also~. However, only onc solution gives a positive value of ~s . The ditlerent estimates can be combined in a number of ways, for instance a Kalmall filter can be used.
5. MAP UPDATING
-
When the estimate of ~~ has been calculated, the local Certainty Grid is created. The creation involves the usage of the updating formulation of 8ayes' theorem given in Elfes (1990). Thereafter, the global Certainty Grid can be updated by using the relations in Fig. 2 to transf:)rm between the coordinate systems.
5.1. Data repn;sl:ntation jn Map databasl: The data representation is similar to the onc described in Elfes (1990), with a few changes . The most significant is the use of polar coordinates in the local Certainty Grid . This conforms to the rddial properties of the laser sensor and enables fast updating of the local Grid, since the predicate "Cell i lies behind cell j" will be easily verified. When the local Certainty Grid has been created, it is mapped to the glohal Grid, which has Cartesian format. There is no guarantee to have a one-to-one mapping, but this is the prize to pay for higher speed.
The recursive fonn of PSA enahles a fast execution, independently of the number of points already covered by a line (i e there is no need to recalculate all sums for each new mea<;urement point added) . A distinguL<;hing property of the presented algorithm is the use of both odometry and range data for position estimation. Another key feature is the incremental map maintenance, based on Certainty Grids.
Another approach wltich involves incremental mapmaking is found in AndeTS\:n et al. (1992), though the position estimation i<; bac;ed solely on odometry.
An incremental map-making algorithm with a position estimation based on both odometry and range data is found in Chatila and Laumond (1985) . The map is represented as a set of polyline segments (to he compared with the Certainty Grid approach in this paper). Topics for future research is the augmentation of a 30model, defined for areas where ohjects are to he manipulated . Thus, since the robot moves mainly horizontally. a 20 model is sufficient fOT navigational tasks, wltile a 3D model is necessary for most tasks involving manipulation of ohjects. Investigation of the connection between a 20 and a 30 model have heen started and will be funher examined.
190
8. REFERENCES
DIST • - _1_(1 . p2 + cos 2e'A + sin 2e· B + n.~ k-I
Andersen C.S. et al. (1992). Navigation using range images on a mobile robot. Robotics and Autonomous systems, 10. 147-160. Chatila R. J-P Laumond (1985). Position referencing and consistent world modelling for mohile robots. IEEE 1. of Robotics &. Automation. pp. 138-145. Duda R.O .• P.E. Hart (1973). Pattern Cla.......ijication and Scene analvsis. WHey & Sons. New York. Ekm:m. A. et al. (19n). Incremental map-making based on range images. FOA Report C 30672-3.4 ISSN 0347-3708. Elfcs. A (1990). Occupancy Grids: A Sto;~as'ic Spatial Representation for Active Rohot Perception. Proc. afsixth CO/if. 01/ Uncenainty in Al. July 1990.
(16)
2 · sine· cose · C- 2p(cose· D + sine · E)
Ncxt. the cxpression will be reformulated by isolating p:
DISTn. t
k 1 - 1 ( (p
-
-
cose ' D+sine ' E 2 1 ) +
D2 E2 · (A--) +sin 2e · (B--) + k t DE 2 . sine'cose . (C_ » k
I
_«(..'OS2 e
k
(17)
The squared parenthesis is always ~ O. implying that to minimi7.c the distortion, the optimal p (p) will be chosen as (when knowing the optimal 6
(a »:
9. APPENDIX
cosa . D + sina . E
p-
9.1. Derivation of the line calculation in PSA Below follows the derivation of the equation.o; needed to pcrfonn the calculation of the coordinates of a line (p. e). The criterion LL<;ed is Eigenvector fit, ao; described in Duda and Hart (1973) (see Fig. 12).
(18)
k
Equation (18) can be reformulated ao;: n+k-I
1 ~
P '"' k J..
. r j ' cos (e -~)
(19)
i· n
which shows that p in fact is calculated as the arithmetic mean value of the orthogonal projections of measurement points along a line through the origin. and whose angle component e is equal to B. I1 is also easy to sce that p cannot be negative. Fig. 12. Th.' dislam:1: lIsed in lhc Eigcnvl:clor Iil crilerion (~). for a point (r.
In the sequel. it is assumed that p is selected as in Eq. (18). turning Eq. (17) into the following expression:
A; can be rewritten as:
I
DIST - k _ 1 ( (cos t:.i
= ri '
(COS~i '
cose + sin~i' sine) - p
2
sin e · (B-
I
= --
k- I
(
,
l k· p- + cos
2
6·
I·
1
r
('i s/
COS
6·.r
I· "
+ 2 . sin 6 . cos6'
DIST
r
,;cjsi - (15)
,,+k-1
6·.r
E~
+
;·11 'iC'i + sin
T ) +2sin6cos6 ' (C- T
1- B-k
I
n+t-)
i - n "+A: - j
21' (
I
n
,,+t-)
sin 6 ·
(r.e .) 2
~
.J..
DE
»
DE J - C- k
(21)
leading to the following equation describing the distortion:
,,+t-j ". t
E2
Nexl introduce H, I and J, deflDe.d as:
By inserting Eq. \14) into Eq . (9). it is possible to express the distortion in the following manner:
DIST
e . (A - T) ) + (20)
(14)
In this equation. COS~i and sin~i can be calculated in advance and rut in a tahle of size 3200*2. Let Cj denote cos(q;,) and Si de.)o~e sin(~,), where ~j-i*21t/3200.
D2
2
',si
JJ
, • "
Name the sums A. B. L. D and E n ..-spcctively:
191
c
.-2-,,- I
2
2
(cos e· H + sin e· 1+ 2sinecos6 ' J) (22)
The minimum can he found hy calculating the derivative with respect 10 6 in Eq. (22). which will be done on the following analogous expression:
I 1 + cos (28) 1 - cos (28) DIST---( ' H+ ,1+ k- I 2 2
(23)
sin(28) ' J)
which, before calculating the derivative, can be simplified to: DlST - -
1
J.: - J
H-/ H+/ (cos (28) . - - + - - + sin (28) ' J)(24)
2
2
hy calculating p with one of tbe 8's, a positive p indicates that the correct 8 has been chosen, whereas a negative p leads to a negation of p and an addition of 1800 to e. The coordinates of the line will be updated with (p.8) just calculated (Broken line in Fig. 8). If, on the other hand, DIST exceeds THRESHOLD, a new line will be created, starting from this last point that could not be added to the current line (The two solid lines in Fig. 8).
The derivativ~ becomes: 9.2. The Recursjye Updatini Scheme
(25)
Below follows a summary of the recursive calculation of the five partial sum., (A,B, ... ,E).
where K is defmed as: H-l
K=
(26)
2
Set 2e = q and the derivative" 0, yielding: cos (q) . J
~
sin (q) . K
=
J alan ( -) + I' . K
It
e
I
J
2
K
EZ
I'
which in turn gives four solutions for
k - 0
A". t - A".t-I + (r,, ' c,,)2
k - I. 2 ....
B".t - 0
k - 0
(27)
On the interval [0, 27t1. two possihle solutions will occur [or q: q
A".t - 0
k - I. 2. ...
(28)
a:
= - ' aI3n ( -)+I"71 / 2
,. E Z
C".,. -
0
C". t
=
C". t _ I
D".t
e
0
k - 0
,
+
r~ . e" . s"
/.: =
I. 2 . ...
/.: =
0
/.: =
I. 2 ...,
(29)
Define the function W through:
w_(O l 71
K> 0 K< 0
(30)
D". ,. aD". t
_I +
r" . C"
(The case K -0 needs special treatment) Now, the 7eros of the derivative can be expressed uniquely in q in the following way:
£".,
=
0
£". t - £". tq
~
J K
which is unique on the interval This am be
trdnsfonn~
1
e - -2
(31)
alall ( -) + W
to, 2nl.
to an expression in
J (alan ( -) +
K
r,, ' Sn
E
k
E
0 I. 2. ...
From the equations ahove, J and K can he calculated as:
a:
J
_ C ".t
w ± 71)
I +
/.:
". t
_ D ". t E " . t k
(33)
(32)
Each of these two values, suhstituted for a in Eq. (22), will give the same value for DIST. Thus, any of the values will do in the calculation of DIST. After DIST has heen calculated with any of the solutions for a, it is compared to a predetermined value - THRESHOLD. If it is less than THRESHOLD, p is calculated hy suhstituting a for El in Eq. (18)
D K
n. t
C
2J (..4 ".t - T". t
£
2 - B
n. t
2
+...!!:..!:)
J:
(34)
It is possihle to express the calculation of J and K through recursive equations as well. but the gain in uniformity does not exceed the degradation of execution speed.
In the calculation of p. the selection of a does maller. Inserting wrong e in Eq. (18) yield<; a negative p. Thus,
192