INCLUSION OF ELLIPSOIDS
Romain Pepy and Eric Pierre
Institut d’Électronique Fondamentale
Université Paris XI
Orsay, France
Keywords:
Path planning, uncertainty, ellipsoid.
Abstract:
We present, in this paper, a ready-to-use inclusion detection for ellipsoids. Ellipsoids are used to represent
configuration uncertainty of a mobile robile. This kind of test is used in path planning to find the optimal path
according to a safety criteria.
1 INTRODUCTION
Nowadays, path planning for mobile robots has taken
a new dimension. Due to many failures when exper-
imenting the following of geometrical paths, deter-
mined by the first generation of planners (Latombe,
1991), with real robots, searchers concluded that
those too simple paths were no longer enough. Plan-
ning method must now guarantee that the paths pro-
posed are safe, i.e. that the robot will be able to fol-
low a path without any risk of failure, or at least in
warranting a high success rate. To achieve this goal,
some parameters must be considered : uncertainties
of the model used (not-so-perfect mapping, inaccu-
racy of the sensors, slipping of the robot on the floor,
etc.).
Collision detection is very important in mobile
robotic and furthermore when trying to find a safe
path in an uncertain-configuration space. Thus,
searchers tend to integrate evolved collision detection
in their planners. Thus, after having used circular disk
to approximate the shape taken by the mobile robot,
more and more searchers use elliptic disks as they of-
fer a better accuracy.
Used in the context of safe path planning (Pierre
and Lambert, 2006; Lozano-Pérez and Wesley, 1979;
Gonzalez and Stentz, 2005; Pepy and Lambert, 2006),
ellipsoids allow to approximate the shape of the set of
positions where the mobile could be (ellipsoids thus
take the mobile robot’s geometry and the uncertain-
ties on its position into account). The the Safe A*
with Towers of Uncertainty (SATU*) planner (Pierre
and Lambert, 2006) is one of those safe path planner
that use ellipsoid to approximate the shape of the set
of positions where the mobile could be. Ellipsoids
are used in the SATU* to perform collision detection
between the mobile robot and its environment. How-
ever, the authors of the SATU* have also proposed a
new mean of organising the performing of the planner
so that the ellipsoids can be used to detect very early
beginning of useless paths. In order to achieve this
goal, inclusion detection must be performed between
two ellipsoids (that correspond to two different ways
to come to the same position). In this paper, we are
going to present an algebraic method using the resul-
tant of Sylvester (Lang, 1984) to solve this problem.
The SATU* algorithm (Alg. 1) has already been pre-
sented in (Pierre and Lambert, 2006) and three tests
of inclusion of uncertainties (lines 7, 26 and 36) are
used. However the authors did not explain how they
implemented those tests nor give the algorithms used.
As the model of uncertainties used in the SATU* cor-
responds to an ellipsoid in 3 dimensions, this test of
inclusion of uncertainties can be seen as a test of in-
clusion of ellipsoids.
In the present paper, we are going to propose
an algorithm of test of inclusion of ellipsoids. In a
first part, the uncertain configuration space will be
described. Then, Sylvester’s resultant will be used to
defined a ready for use inclusion detection test.
98
Pepy R. and Pierre E. (2007).
INCLUSION OF ELLIPSOIDS.
In Proceedings of the Fourth International Conference on Informatics in Control, Automation and Robotics, pages 98-102
DOI: 10.5220/0001645200980102
Copyright
c
 SciTePress

Algorithm 1 (SATU*).
1: CLOSE ←
/
0
2: OPEN ← NodStart
3: while OPEN6=
/
0 do
4: Nod ← Shortest_f
∗
_Path(OPEN)
5: CLOSE ← CLOSE + Nod
6: OPEN ← OPEN - Nod
7: if Base(Nod) = Base(NodGoal) and uncertainty(Nod)
⊆ uncertainty(NodGoal) then
8: RETURN Success
9: end if
10: NEWNODES ← Successors(Nod)
11: for all NewNod of NEWNODES do
12: if NewNod /∈ OPEN,CLOSE then
13: g(NewNod) ← g(Nod)+cost(Nod,NewNod)
14: f
∗
(NewNod) ←
g(NewNod)+h(NewNod,NodGoal)
15: build(NEWTOWER,base(NewNod))
16: AddLevel(NEWTOWER,NewNod)
17: OPEN ← OPEN+NewNod
18: parent(NewNod) ← Nod
19: else
20: TOWER ← ExtractTower(base(NewNod))
21: level ← -1
22: repeat
23: AddLevel ← false
24: level ← level+1
25: LevelNod ← ExtractNode(TOWER,level)
26: if (g(NewNod) ≥ g(LevelNod) and uncer-
tainty(LevelNod) * uncertainty(NewNod)) or
g(NewNod) < g(LevelNod) then
27: AddLevel ← true
28: end if
29: until level 6= TopLevel(TOWER) and Ad-
dLevel=true
30: if AddLevel=true then
31: level ← insert(NewNod,TOWER)
32: OPEN ← OPEN + NewNod
33: parent(NewNod) ← Nod
34: UpperNods ←
nodes(UpperLevels(TOWER,level))
35: for all uppernod of UpperNods do
36: if uncertainty(NewNod) ⊆ uncer-
tainty(uppernod) then
37: remove(TOWER,uppernod)
38: OPEN ← OPEN - uppernod
39: end if
40: end for
41: end if
42: end if
43: end for
44: end while
45: RETURN NoSolution
Figure 1: Example of p
(x,y).
(x,y).
2 SHAPE OF UNCERTAINTIES
SATU* uses the Kalman filter (Kalman, 1960) to
determine the uncertainties associated with the es-
timated positions of the mobile robot. (Smith and
Cheeseman, 1986) gave a method to interpret the re-
sults of this filter. Indeed, thanks to this filter, we
know at each time the covariance matrix C represent-
ing the uncertainties on the position x of the mobile
robot. As we are only interested in the position of the
mobile robot (in x and y), our random vector x varies
in R
2
. As the Kalman filter uses only gaussian ran-
dom vectors, the density of probability p
x
(x) of the
vector x is
p
x
(x) =
1
2π
√
detC
e
−
1
2
(x−
ˆ
x)
T
C
−1
(x−
ˆ
x)
,
where
ˆ
x represents the estimated state vector.
We know the covariance matrix C, which is given
by
C =

σ
2
x
ρσ
y
σ
x
ρσ
y
σ
x
σ
2
y

, (1)
where σ
2
x
represents the variance of x, σ
2
y
the variance
of y and ρ is the correlation coefficient of x and y.
In translating the coordinate frame to the known
point
ˆ
x, the probability density function is
p
x,y
(x,y) =
1
2π
√
detC
e
−
1
2
(
1−ρ
2
)

x
2
σ
2
x
+
2ρxy
σ
x
σ
y
+
y
2
σ
2
y

. (2)
An example of drawing of this probability density
function is given figure 1.
Our goal is to determine the set of positions where
the mobile robot can be for a given confidence thresh-
old p. This corresponds in finding an areo of isoden-
sity contours such that
(x−
ˆ
x)
T
C
−1
(x−
ˆ
x) = k
2
,
where k is a constant. The relation between k and p is
given by k =
p
−2ln(1−p).
INCLUSION OF ELLIPSOIDS
99

Thus, we can determine the equation of the ellip-
soid (centered on the known point
ˆ
x) where the robot
mobile may be with the given probability p:
Ax
2
+ Bxy+Cy
2
= k
2
, (3)
with









A =
1
(
1−ρ
2
)
σ
2
x
,
B =
2ρ
(
1−ρ
2
)
σ
x
σ
y
,
C =
1
(
1−ρ
2
)
σ
2
y
.
(4)
This equation describes an ellipsoid. A classical re-
sult allows us to write this equation in function of the
parameters a, b and ϕ of the ellipsoid. Considering
that a is the half major axis of the ellipsoid, b its half
minor axis and ϕ its orientation, we have











ϕ =
1
2
arctan

B
A−C

,
a =
r
2k
2
A+C−
√
A
2
+C
2
−2AC+B
2
,
b =
r
2k
2
A+C+
√
A
2
+C
2
−2AC+B
2
.
(5)
Using (4) and (5), we can determine the parameters
of the ellipsoid in function of the covariance matrix
given by the EKF.
We are going to work with this ellipsoid in order
to find a test of inclusion of ellipsoids. Then, a simple
test on σ
2
θ
will allow us to complete the test of inclu-
sion of 3-D ellipsoids.
3 DEFINITIONS AND
NOTATIONS
The equation of an ellipsoid E
i
with a half major axis
a
i
and a half minor axis b
i
< a
i
, centered on the origin
of the frame of reference and with no orientation is
given by
x
′
2
a
2
i
+
y
′
2
b
2
i
= 1, (6)
considering x
′
and y
′
the coordinates of the ellipsoid.
To determine the equation of the centered ellipsoid
of orientation ϕ
i
, the use of a simple rotation matrix
of angle −ϕ
i
is enough. The new coordinates of the
ellipsoid are named x and y. Equation (6) then be-
comes
A
i
x
2
+ B
i
y
2
+C
i
xy−1 = 0 (7)
where











A
i
=
(b
i
cosϕ
i
)
2
+(a
i
sinϕ
i
)
2
(a
i
b
i
)
2
B
i
=
(b
i
sinϕ
i
)
2
+(a
i
cosϕ
i
)
2
(a
i
b
i
)
2
C
i
=
(
b
2
i
−a
2
i
)
sin(2ϕ
i
)
(a
i
b
i
)
2
.
(8)
ϕ
2a
2b
E
2
E
1
Figure 2: Ellipsoid’s parameters.
It represents the equation of a centered ellipsoid
of half major axis a
i
, half minor axis b
i
, and of ori-
entation ϕ
i
(figure 2). We will call this ellipsoid
E
i
(a
i
,b
i
,ϕ
i
).
In this paper, we will use two ellipsoids
E
1
(a
1
,b
1
,ϕ
1
) and E
2
(a
2
,b
2
,ϕ
2
).
4 INCLUSION TEST
To ensure an ellipsoid E
1
is included in an ellipsoid
E
2
(see figure 2), we just need to perform two tests.
We must check there is no intersection between E
1
and E
2
and then that E
1
lies within E
2
.
4.1 Sylvester’s Resultant
Checking if two ellipsoids intersect is the same as
finding the tuples (x, y), which are solutions of both
E
1
and E
2
. The use of the resultant allows to find
those tuples.
Definition Let P = u
0
X
p
+···+u
p
and Q = v
0
X
q
+
···+ v
q
be two polynoms of degrees p > 0 and q > 0
respectively. The resultant of two polynomials P and
Q is the determinant of the Sylvester’s matrix of P and
Q, which is the square matrix of size p+ q :


















u
0
u
1
··· ··· ··· u
p
0 ··· 0
0 u
0
u
1
··· ··· ··· u
p
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
0
0 ··· 0 u
0
u
1
··· ··· ··· u
p
v
0
v
1
··· ··· v
q
0 ··· ··· 0
0 v
0
v
1
··· ··· v
q
0 ··· 0
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
0
0 ··· ··· 0 v
0
v
1
··· ··· v
q


















.
Theorem Let P = u
0
X
p
+ ···+ u
p
and Q = v
0
X
q
+
···+ v
q
be two polynomials with their coefficient in
a field K, so that u
0
6= 0 and v
0
6= 0. Let
K be an
ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics
100

algebraically closed field
1
containing K. The two fol-
lowing properties are equivalent:
1. The polynomials P and Q have a common root in
K.
2. P and Q’s resultant is null.
The demonstration is given in (Lang, 1984).
4.2 Intersection Test
Theorem 4.1 indicates that two polynomials have at
least one solution in the algebraic closure of the field
in which the coefficients are defined if the resultant
of those polynomials is nil. As the equations of the
ellipsoids are polynomials (bilinear), we can use the
resultant to determine the intersection point of the el-
lipsoids. The coefficients of the polynomials (ellip-
soids) are defined on R, the use of the Sylvester’s re-
sultant gives the common roots of the polynomial in
an algebraically closed field containing R, for exam-
ple C. We will then need to check if the roots are in
R, as we just need real intersections. To define the
Sylvester’s matrix, we need to rewrite the equation of
the ellispoid in function of only one parameter. Let’s
choose (randomly) x.
(7) ⇔ Ax
2
+ (Cy)x+ By
2
−1 = 0,
which gives, for the equations of E
1
and E
2
:

A
1
x
2
+ (C
1
y)x+ B
1
y
2
−1 = 0
A
2
x
2
+ (C
2
y)x+ B
2
y
2
−1 = 0.
(9)
Sylvester’s matrix of the polynomials is then:
S =




A
1
C
1
y B
1
y
2
−1 0
0 A
1
C
1
y B
1
y
2
−1
A
2
C
2
y B
2
y
2
−1 0
0 A
2
C
2
y B
2
y
2
−1




.
The determinant of S is
|
S
|
= py
4
+ qy
2
+ r, (10)
with
p = −2A
1
A
2
B
1
B
2
+ (A
1
B
2
−A
2
B
1
)C
1
C
2
+ A
1
B
1
C
2
2
+ A
2
B
2
C
2
1
+ A
2
1
B
2
2
+ A
2
2
B
2
1
q = 2A
1
A
2
(B
1
+ B
2
) + (A
2
−A
1
)C
1
C
2
−2A
2
1
B
2
−2A
2
2
B
1
−A
1
C
2
2
−A
2
C
2
1
r = (A
2
−A
1
)
2
.
The change of variables Y = y
2
allows us to
rewrite the determinant:
|
S
|
= pY
2
+ qY + r.
Algorithm 2 ComputeEquation(a,b,ϕ).
1: A ←((bcosϕ)
2
+ (asinϕ)
2
)/(ab)
2
2: B ←((bsinϕ)
2
+ (acosϕ)
2
)/(ab)
2
3: C ← ((b
2
−a
2
)sin(2ϕ))/(ab)
2
4: RETURN (A, B,C)
Algorithm 3 Determinant(A
1
,B
1
,C
1
,A
2
,B
2
,C
2
).
1: p ←−2A
1
A
2
B
1
B
2
+(A
1
B
2
−A
2
B
1
)C
1
C
2
+A
1
B
1
C
2
2
+
A
2
B
2
C
2
1
+ A
2
1
B
2
2
+ A
2
2
B
2
1
2: q ← 2A
1
A
2
(B
1
+ B
2
) + (A
2
−A
1
)C
1
C
2
− 2A
2
1
B
2
−
2A
2
2
B
1
−A
1
C
2
2
−A
2
C
2
1
3: r ← (A
2
−A
1
)
2
4: RETURN (p, q,r)
We are looking for the roots of
|
S
|
in R. If the
discriminant of the resultant (which is q
2
−4rp) is
strictly negative, there is no roots in R. If r is nil,
we can directly conclude that there is at least one in-
tersection point because there exists at least one real
root (y = 0).
If the discriminant of
|
S
|
is positive or nil, there is
at least one root in R. We conclude that the ellispoids
have at least one intersection point in C (y may be
real, x could be complex). We then need to calculate
the values of y that gives an intersection point, then
check that the corresponding values of x are ni R too.
Roots of 10 are given by:
(
y
1,2
= ±
−q−
√
∆
2p
y
3,4
= ±
−q+
√
∆
2p
(11)
Using 11, (9) becomes:
(A
1
−A
2
)x
2
+ y(C
1
−C
2
)x+ y
2
(B
1
−B
2
) = 0, (12)
which discriminant is
∆ = y
2
(C
1
−C
2
)
2
−4y
2
(A
1
−A
2
)(B
1
−B
2
). (13)
If ∆ ≥ 0, then equation (12) has real roots. We con-
clude that the global system as a tuple (x,y) of real
solutions. Thus, ellipsoids have at least one intersec-
tion point. On the contrary, if equation (12) has no
real root in x, then the ellipsoids do not intersect.
4.3 Test of Length
If the ellipsoids do not intersect, then it means that
one of them is fully included in the other (as they have
the same center). To test if it is the first ellipsoid that
is included in the second, we just need to check that
the half major axis of the first ellipsoid, a
1
, is smaller
1
A field
K is algebraically closed if every polynomial of
K[X] is split on K, i.e. product of polynomials of degree 1.
INCLUSION OF ELLIPSOIDS
101

Algorithm 4 ComputeRoots(p,q,∆).
1: y1 ←(−q−
√
∆)/(2p)
2: y3 ←(−q+
√
∆)/(2p)
3: RETURN (y
1
,−y
1
,y
3
,−y
3
)
Algorithm 5 InclusionTest(a
1
,b
1
,ϕ
1
,a
2
,b
2
,ϕ
2
).
1: (A
1
,B
1
,C
1
) ← ComputeEquation(a
1
,b
1
,ϕ
1
)
2: (A
2
,B
2
,C
2
) ← ComputeEquation(a
2
,b
2
,ϕ
2
)
3: (p, q,r) ← Determinant(A
1
,B
1
,C
1
,A
2
,B
2
,C
2
)
4: ∆ ←q
2
−4rp
5: if ∆ ≥0 then
6: (y
1
,y
2
,y
3
,y
4
) ← ComputeRoots(p,q,∆)
7: for all y ∈
{
y
1
,y
2
,y
3
,y
4
}
do
8: ∆
y
← y
2
(C
1
−C
2
)
2
−4y
2
(A
1
−A
2
)(B
1
−B
2
)
9: if ∆
y
≥ 0 then
10: RETURN NO INCLUSION
11: end if
12: end for
13: end if
14: if (a
1
≥ a
2
) or (b
1
≥ b
2
) then
15: RETURN NO INCLUSION
16: end if
17: RETURN INCLUSION
than the half major axis of the second ellipsoid, a
2
.
We can, following the same scheme, check that the
half minor axis of the first ellipsoid b
1
is smaller than
the half minor axis of the second ellipsoid, b
2
.
4.4 Algorithm
The complete algorithm of inclusion of two ellipsoids
which have the same center is given alg. 5. Sub-
functions ComputeEquation (Alg. 2), Determinant
(Alg. 3) and ComputeRoots (Alg. 4) refer respec-
tively to the equations (8), (10) and (11).
5 CONCLUSION
In this article, we presented a method of test of in-
clusion of ellipsoids when those ellipsoids have the
same center. This test of inclusion is necessary in the
use of some path planners such as the SATU* planner
(Pierre and Lambert, 2006). The proposed method
is very easily implementable on computer and have a
constant and low complexity.
REFERENCES
Gonzalez, J. P. and Stentz, A. T. (2005). Planning with un-
certainty in position: An optimal and efficient planner.
In Proceedings of the IEEE International Conference
on Intelligent Robots and Systems (IROS ’05).
Kalman, R. (1960). A new approach to linear filtering
and prediction problems. Transactions of the ASME–
Journal of Basic Engineering, 82(Series D):35–45.
Lang, S. (1984). Algebra. Addison-Wesley.
Latombe, J. C. (1991). Robot Motion Planning. Kluwer
Academic Publisher.
Lozano-Pérez, T. and Wesley, M. A. (1979). An algo-
rithm for planning collision-free paths among polyhe-
dral obstacles. Commun. ACM, 22(10):560–570.
Pepy, R. and Lambert, A. (2006). Safe path planning
in an uncertain-configuration space using rrt. In
Proc. IEEE/RSJ International Conference on Intelli-
gent Robots and Systems, pages 5376–5381, Beijing,
China.
Pierre, E. and Lambert, A. (2006). Path planning for car-like
robots using towers of uncertainties. In Proceedings
IEEE International Conference on Information and
Communication Technologies: from Theory to Appli-
cations, pages 754–759, Damascus, Syria.
Smith, R. and Cheeseman, P. (1986). On the representation
and estimation of spacial uncertainty. International
Journal of Robotics Research, pp 56-68, 5(4).
ICINCO 2007 - International Conference on Informatics in Control, Automation and Robotics
102