Collision Cones for Quadric Surfaces in $n - ePrints@IISc

(1)

Collision Cones for Quadric Surfaces in n -Dimensions

Animesh Chakravarthy and Debasish Ghose

Abstract—In this letter, we present analytical expressions for col- lision cones associated with a class of hyperquadric surfaces mov- ing inn-dimensional configuration space. Using a relative velocity paradigm, a geometric analysis of the distance, time, and point of closest approach between moving objects inn-dimensional space, is carried out to obtain a characterization of the collision cone between a point and a hyperspheroid as well as a constrained hy- perboloid, which represent an interesting and useful class of objects in configuration space. It is shown that thesen-dimensional colli- sion cones can be integrated with sampling-based motion planners, avoiding the need to evaluate waypoints that lie inside the collision cone. The cones can also consist of the heading angles toward de- sirable regions in the configuration space, in which case planners may evaluate more waypoints inside the cone. Finally, analytical expressions of the collision cones are used, in conjunction with the concept of level sets, and incorporated into a Lyapunov-based design approach, to determine analytical expressions of nonlinear guidance laws that can manipulate the velocity vector of an object inn-dimensional space.

Index Terms—Collision avoidance, motion and path planning, collision cones, n-dimensions, configuration space.

I. INTRODUCTION

P

ATH planning inn-dimensional spaces is a problem that has attracted much interest [1]–[4]. In general, these spaces can contain both stationary and moving obstacles. Many sampling based planners such as Probabilistic Road Maps (PRMs) [3] and Rapidly-exploring Random Trees (RRTs) [4] explore regions of thisn-dimensional space and look for potential collisions with obstacles. It would be immensely helpful if we could characterize collision-free regions within this space because the sampling based planners then need to explore only these regions [1]. Such a characterization of the collision-free spaces for ellipsoidal bodies with ellipsoidal obstacles is given in [1], which relies on the Minkowski sum and difference of two ellipsoids which have been characterized in [5], [6].

In this letter, we perform an alternative characterization of the collision free regions, by using the notion of collision cones

Manuscript received September 10, 2017; accepted November 1, 2017. Date of publication November 22, 2017; date of current version December 11, 2017.

This letter was recommended for publication by Associate Editor E. Johnson and Editor J. Roberts upon evaluation of the reviewers’ comments. This work was supported by the National Science Foundation under Grants IIS-1351677 and CNS-1446557. The work of A. Chakravarthy was supported by the National Science Foundation. (Corresponding author: Animesh Chakravarthy)

A. Chakravarthy is with the Department of Aerospace Engineering and the Department of Electrical Engineering and Computer Science, Wichita State Uni- versity, Wichita, KS 67260 USA (e-mail: [email protected]).

D. Ghose is with the Department of Aerospace Engineering, Indian In- stitute of Science, Bengaluru, Bengaluru 560012, India (e-mail: dghose@

aero.iisc.ernet.in).

Digital Object Identifier 10.1109/LRA.2017.2776347

Fig. 1. An objectAnavigating through ann-dimensional configuration space cluttered with moving obstaclesB1,B2, . . .modeled by hyperquadrics.

[7]. A collision cone provides the set of heading directions that would cause an object A to be on a collision course with a moving objectB. With knowledge of these cones, the sampling planner can then evaluate waypoint samples that lie outside these cones. See Fig. 1 for an illustration, whereArepresents a point object having to navigate through an n-dimensional configuration space with multiple moving obstaclesB1,B2,B3, . . . , moving with velocitiesVB1,VB2,VB3, . . .. The collision cones fromAto each of these obstacles are represented asC1,C2,C3, ..., and can represent forbidden zones which the sampler either need not evaluate at all, or alternatively, evaluate only with a low probability.

In cluttered environments where the objects are moving in close proximity, the shapes of the objects play an important role in the determination of collision avoidance maneuvers. In this letter, we provide analytical characterizations of the collision cone when the object B belongs to a class of hyperquadric surfaces [8] which can serve as efficient bounding volumes for a large class of objects. Hyperquadric surfaces include hyperspheres, hyperspheroids, hyperellipsoids, etc. While hyperspheres can be used to bound many object shapes because of the analytical convenience they provide, they can often be conservative when the objects are more elongated in some directions than in other directions. In such scenarios, hyperspheroids serve as better approximations than spheres because they can form a tighter bounding volume on elongated objects. Hyper- quadrics such as n-dimensional hyperboloids can be used as bounding volumes for non-convex objects. Objects may also be represented as a union of hyperspheroids andn-dimensional hyperboloids.

Hyperspheroids are also of interest in RRT problems for another reason. In RRT methods, it has been shown that for problems that seek to minimize path length, the subset of states that can improve a solution can be described by a prolate hyperspheroid, whose focal points are the start and goal points [9].

In such problems, the hyperspheroid is not an entity that needs

See http://www.ieee.org/publications standards/publications/rights/index.html for more information.

(2)

to be avoided, but instead is a region within which the trajectories need to lie, since the optimal path is contained inside the hyperspheroid. Informed RRT* [9] focuses the search on this informed subset of the state space that contains all the states that can improve the current solution. In such problems, we would like to determine the cone of heading angles that lead into the hyperspheroid, so that the sample-based planner then samples only the waypoints that lie within this cone.

This letter determines analytical expressions for collision cones in high-dimensional spaces. The collision conditions for a large class of object shapes moving in 2-D [7] and 3-D [10] has been proposed in the literature. Exact collision conditions for objects of entirely arbitrary shapes (convex and non-convex) in 2-D have been determined in [7], while the same has been determined for objects modeled by quadric surfaces in 3-D in [10]. These conditions have been used to determine analytical expressions of guidance laws for collision avoidance in [11], and for safe-passage through narrow, moving orifices in [12]. At its most fundamental level, the collision cone has some connections with the maneuvering board approach [13], velocity obstacles [14], and forbidden regions [15] in that these approaches determine cones of forbidden heading directions or velocities that cause collisions. However, while all these letters model the objects as circles or spheres, the collision cone approach can handle a larger class of object shapes.

In this letter, we determine collision cones associated with a class of moving hyperquadric surfaces inn-dimensions. The rest of this letter is organized as follows. Section II discusses the collision dynamics between a pair of point objects moving in n-dimensional space. Sections III and IV present the collision cone between a point object and a hyperspheroid, and that between a point object and a hyperboloid, inn-dimensional space.

Section V illustrates the collision cone equations with examples, while Section VI illustrates how one can determine analytical guidance laws to facilitate either avoidance of collision between two objects, or rendezvous of the objects. Section VII presents the conclusions.

II. RELATIVEVELOCITYKINEMATICSBETWEEN

TWOPOINTOBJECTS INRⁿ

Consider two point objectsAandBmoving inRⁿ. Let the origin ofRⁿbe atA. In this frame, the co-ordinates ofBare expressed as[x1 x2 . . . xn]. They can be equivalently expressed in terms of hyperspherical coordinates (r, φ₁, φ2, ..., φn−1), whereris the distanceAB, andφ1,φ2, ...,φn−1are the bearing angles ofBwith respect to A. The two coordinate systems are related as follows [16], [17]:

x1 = rcosφ1

x2 = rsinφ1cosφ2

x3 = rsinφ1sinφ2cosφ3

... ...

xn−1 = rsinφ1sinφ2. . .sinφn−2cosφn−1

xn = rsinφ1sinφ2. . .sinφn−2sinφn−1 (1)

Fig. 2. Schematic showing definition of angles in a 3-D case.

The above equations are illustrated for a 3-D coordinate system in Fig. 2. Let the velocities ofAandB beVA andVB, respectively. These velocity vectors, [VA ,x1 VA ,x2 ... VA ,xn]^T, and [VB ,x1 VB ,x2 ... VB ,xn]^T have their equivalent representation in spherical co-ordinates as follows:

VA =

⎡

⎢⎢

⎣

VAcosα1

VAsinα1cosα2

...

VAsinα1sinα2...sinαn−2cosαn−1

VAsinα1sinα2...sinαn−2sinαn−1

⎤

⎥⎥

⎦ ,

VB =

⎡

⎢⎢

⎣

VBcosβ1

VBsinβ1cosβ2

...

VBsinβ1sinβ2...sinβn−2cosβn−1

VBsinβ1sinβ2...sinβn−2sinβn−1

⎤

⎥⎥

⎦

(2)

where, VA and VB are the speeds of A and B, and (α1, α2, ..., αn)and(β1, β2, ..., βn)are the heading angles of VA andVB. Define VR E L =VB −VA as the relative velocity of Bwith respect toA. Letˆr represent the unit vector along the line AB, andˆφ1,ˆφ2, ...,ˆφn−1 representn−1 mutually orthogonal unit vectors that are also orthogonal to AB. These unit vectors can be obtained as follows. From (1), we see that the vectorris represented as:

r=r

⎡

⎢⎢

⎣

cosφ1

sinφ1cosφ2

...

sinφ1sinφ2. . .sinφn−2cosφn−1

sinφ1sinφ2. . .sinφn−2sinφn−1

⎤

⎥⎥

⎦

(3)

dr dφn−1/|_dφ^dr

n−1|. Since (3) has the formr=rˆr, the expression forˆris readily evident, while those corresponding to the other

(3)

unit vectors are as follows:

ˆφ1 =

⎡

⎢⎢

⎣

−sinφ1

cosφ1cosφ2

cosφ1cosφ3sinφ2

...

cosφ1sinφ2. . .sinφn−2cosφn−1

cosφ1sinφ2. . .sinφn−2sinφn−1

⎤

⎥⎥

⎦

ˆφ2 =

⎡

⎢⎢

⎣

0

−sinφ2

cosφ2cosφ3

...

cosφ2. . .sinφn−2cosφn−1

cosφ2. . .sinφn−2sinφn−1

⎤

⎥⎥

⎦ ,

ˆ φ3 =

⎡

⎢⎢

⎣

0 0

−sinφ3

...

cosφ3. . .sinφn−2cosφn−1

cosφ3. . .sinφn−2sinφn−1

⎤

⎥⎥

⎦ ,

ˆφn−1 =

⎡

⎢⎢

⎣ 0 0 0 ...

−sinφn−1

cosφn−1

⎤

⎥⎥

⎦

(4)

Differentiating (3), it can be shown that:

r˙= ˙rˆr+rφ˙1ˆφ1 +rsinφ1φ˙2ˆφ2

+rsinφ1sinφ2φ˙3ˆφ3+. . .

+rsinφ1sinφ2. . .sinφn−2φ˙n−1φn−1 (5) We next resolveVR E Linto itsncomponentsVr,Vφ1,Vφ2, . . . , Vφn−1, where Vr represents the relative velocity component alongAB andVφi, i= 1,2, ..., n−1are the relative velocity components orthogonal toAB. These relative velocity components are as follows:

Vr ≡VR E L.ˆr= ˙r Vφ1 ≡VR E L.ˆφ1 =rφ˙1

Vφ2 ≡VR E L.ˆφ2 =rsinφ1φ˙2

...

Vφn−1 ≡VR E L.ˆφn−1 =rsinφ1sinφ2. . .sinφn−2φ˙n−1

(6) After substituting (3)–(4) in (6), differentiating the resulting expressions, and re-arranging the algebraic terms, we obtain the n differential equations governing the quantities V˙r,

V˙φ1, V˙φ2, . . . , V˙φn−1. We initially assume that both A and B move with constant velocities (this assumption is relaxed in Section VI). The differential equation governing V˙r is as follows:

rV˙r =V_φ1² +V_φ2² +. . .+V_φn−1² (7) The differential equations governingV˙φ1,V˙φ2,. . .,V˙φn−1 are fairly lengthy, and not presented here due to space consid- erations. However, as an illustration, the equations for a 4- dimensional scenario, involving the componentsV˙φ1,V˙φ2,V˙φ3

are given in the Appendix.

From the differential equations for the relative velocity components, (8) and (10) presented below, can be determined. (See the Appendix for an illustration of how these equations are determined in a4-D case). The relative velocity components satisfy the following equation:

V˙rVr+ ˙Vφ1Vφ1+ ˙Vφ2Vφ2+. . .+ ˙Vφn−1Vφn−1 = 0 (8) which can be integrated to yield:

V_r²+V_φ1² +. . .+V_φn² ₋₁=Vr(0)²+Vφ1(0)²+. . .+Vφn−1(0)² (9) where,Vr(0),Vφ1(0),Vφ2(0), ...,Vφn−1(0)are the initial values of the corresponding quantities.

Furthermore, it can be shown that:

V˙φ1Vφ1+ ˙Vφ2Vφ2+. . .+ ˙Vφn−1Vφn−1

V_φ1² +V_φ2² +. . .+V_φn−1² =−Vr

r (10)

Integrating both sides of (10) with respect to time, we obtain:

V_φ1² +V_φ2² +. . .+V_φn−1²

Vφ1(0)²+Vφ2(0)²+. . .+Vφn−1(0)² =r(0)²

r² (11) Letrm represent the distance betweenAandB at the instant of closest approach, and let this closest approach occur at time t=tm. Since Vr = ˙r, we therefore have Vr(tm) = 0. When evaluated att=tm, (11) thus becomes:

V_φ1m² +V_φ2m² +. . .+V_φn² _−1m

Vφ1(0)²+Vφ2(0)²+Vφ3(0)² = r(0)²

r_m² (12) where,Vφim =Vφi(tm), i= 1, . . . , n. Evaluating (9) att=tm, and substitutingVr m = 0in (9), we obtain:

V_φ²_1m+. . .+V_φn−1m² =Vr(0)²+Vφ1(0)²+. . .+Vφn−1(0)² (13) From (12) and (13), we obtain the miss-distance as:

r_m² = r(0)²(V_φ1(0)²+Vφ2(0)²+. . .+Vφn−1(0)²) Vr(0)²+Vφ1(0)²+Vφ2(0)²+. . .+Vφn−1(0)² (14) The objectsAandBare at a distancermapart, at the miss time tm, which represents the time instant at whichAandBachieve their closest approach, and is given by:

tm = −r(0)Vr(0)

Vr(0)²+Vφ1(0)²+Vφ2(0)²+. . .+Vφn−1(0)² (15) Note that in the above equation,tmis positive only whenVr(0)is negative. If the initial conditions are such thatVr(0)is positive, thentm is negative and corresponds to the time at which closest approach would occur if the trajectories of bothAandBwere projected backwards in time.

(4)

Fig. 3. A point and a hyperspheroid.

In the next sections, we demonstrate how these results can be used to determine analytical expressions for the collision cones between a point object and ann-dimensional hyperspheroid and a hyperboloid, moving inn-dimensional space.

III. COLLISIONCONEBETWEEN APOINTOBJECT AND A

HYPERSPHEROID

Consider a point objectAand a hyperspheroidBmoving in n-dimensional space. Here,Bis defined by the equation:

x²₁

a² +x²₂+x²₃+. . .+x²_n

b² = 1 (16)

and represents the surface generated by the locus of points such that the sum of distances from the foci ofBis a constant equal to2a. When reduced to2-dimensions,Bis an ellipse, while in 3-dimensions, it is a spheroid.

Refer Fig. 3, which for easy visualization gives the hyperspheroid in 3-D, but the notations used carry through in n- dimensions. Let C1 andC2 represent the two foci of B. Let Vr,C1,Vφ1,C1,Vφ2,C1,. . .,Vφn−1,C1 represent the relative velocity components ofC1with respect toA, andVr,C2,Vφ1,C2, Vφ2,C2,. . .,Vφn−1,C2represent the relative velocity components ofC2 with respect toA. Here,Vr,C1 (respectively,Vr,C2) represent the relative velocity component alongAC1(respectively, AC2). Letrm1 represent the distance betweenAandC1at the instant of closest approach, andrm2represent the corresponding distance betweenAandC2 at the instant of closest approach.

Let the corresponding times, at which these closest approaches occur, betm1 andtm2, respectively. Then, from (14), we see thatrm i, i= 1,2is as follows:

r_{m i}² = ri(0)²(Vφ1,C i(0)²+. . .+Vφn−1,C i(0)²)

Vr,C i(0)²+Vφ1,C i(0)²+. . .+Vφn−1,C i(0)² (17) and, from (15),tm i, i= 1,2is as follows:

tm i= −ri(0)Vr,C i(0)

Vr,C i(0)²+Vφ1,C i(0)²+. . .+Vφn−1,C i(0)² (18) Consider (9), when applied individually to the linesAC1and AC2. Using (7) in (9), we get the equation:

V_{r,C i}² +riV˙r,C i =Vr,C i(0)²+Vφ1,C i(0)²

+. . .+Vφn−1,C i(0)², i= 1,2 (19) WhenAandB move with constant velocities, the right hand side of (19) is the same fori= 1,2, and we refer to this asV_C².

ReplacingV_{r,C i}² byVr,C ir˙iin the above equation, we obtain:

Vr,C ir˙i+riV˙r,C i=V_C², i= 1,2 (20) Integrating the above equation for i= 1 from limits tm1 tot (where we assume, without loss of generality, thattm1 < t <

tm2), we get:

_t

tm1

d(r1Vr,C1) = _t

tm1

V_C²

⇒r1Vr,C1−rm1Vr m1 =VC²(t−tm1) (21) SinceVr m1= 0, therefore (21) becomes:

r1r˙1 =V_C²(t−tm1) (22) Integrating (22), we get:

_t

tm1

r1r˙1 = _t

tm1

V_C²(t−tm1)

⇒r1(t)² =r_m²₁+V_C²(t−tm1)² (23) Next, integrating (20) for i= 2from limitst totm2, and following steps similar to (21)–(23), we get:

⇒r2(t)² =r²_m2+V_C²(tm2−t)² (24) From (23), (24), we then get the following:

r1+r2 = r_m²₁+V_C²(t−tm1)²

+ r²_m2+V_C²(tm2−t)² (25) Lettmrepresent the time at whichr1+r2is a minimum. This corresponds to the time instant whenAandBare at their closest approach, and this can be found by differentiating (25) with respect totand equating to zero. The equation^d^(r^C¹_dt^+r^C²⁾ = 0 yields the expression:

V_C²(t−tm1)

r²_m1+V_C²(t−tm1)² − V_C²(tm2−t)

r_m²₂+V_C²(t_m2−t)² = 0 (26) The above equation is solved fortm as follows:

tm =rm2tm1+rm1tm2

rm1+rm2 (27)

Substitutingt=tm in (25), wheretm is obtained from (27), we can obtainrm as follows:

r_m² =(V_r²+V_φ1² +. . .+V_φn−1² )(tm2−tm1)²+(r_m1+rm2)² (28) Since collision occurs when rm ≤2a, therefore the collision condition betweenAandBis given by the following expression:

[r1Vr,C1−r2Vr,C2]² +

r1 V_φ²_1,C1+. . .+V_φn−1,C² ₁ +r2 V_φ²_1,C₂+. . .+V_φn−1,C² ₂

₂

≤4a²V_C² (29) which is obtained after substitutingrm1,rm2from (17) andtm1, tm2 from (18) in (29). Additionally, since it is required thattm

(5)

in (27) always satisfiestm ≥0(that is, the collision condition is computed only for all future time), this means that:

Vr,C2 V_φ1,C² ₁+V_φ²_2,C₁+. . .+V_φn−1,C² ₁

+Vr,C1 V_φ1,C² ₂+V_φ2,C² ₂+. . .+V_φn−1,C2² ≤0 (30) Thus, if a point and a hyperspheroid are moving such that (29) and (30) are satisfied, then they are on a collision course with each other.

As a footnote, we observe that when B is a hypersphere of radius R, then the two foci merge into the center ofB, in which case,Vr,C1 =Vr,C2 ≡Vr,Vφj,C1 =Vφj,C2≡Vφj, j= 1,2, . . . , n−1,a=R, and (29) and (30) collapse to the conditions:

r²(V_φ1² +. . .+V_φn² ₋₁)≤R²(V_r²+V_φ1² +. . .+V_φn−1² ) (31)

Vr <0 (32)

Considering (29) and (30), we define two scalar quantitiesy andVr,A B as follows:

y= [r₁Vr,C1−r2Vr,C2]² +

r1 V_φ1,C² ₁+. . .+V_φn² _−1,C₁ +r2 V_φ1,C² ₂+. . .+V_φn² _−1,C₂

₂

−4a²VC² (33) Vr,A B =Vr,C2 V_φ1,C² ₁+. . .+V_φn² _−1,C₁

+Vr,C1 V_φ1,C² ₂+. . .+V_φn² _−1,C₂ (34) Here,y is essentially a miss-distance function such that, when VR ,A B <0, then y= 0 corresponds to the scenario whereA will graze the surface ofB,y <0corresponds to the scenario whereAwill enterB, andy >0implies thatAwill pass around B. The equationy=c(wherecis a constant) represents a man- ifold in the (rC i, Vr,C i, Vφ1,C i, Vφ2,C i, Vφ3,C i, . . . , Vφn−1,C i) space. The collision cone X is then defined as follows:

X ={α1, α2, . . . , αn−1 :y≤0, Vr,A B <0} (35) and represents the instantaneous set of heading angles ofAfor whichAis on a collision course withB.

IV. COLLISIONCONEBETWEEN APOINTOBJECT AND A

CONFOCALQUADRIC

We next consider a hyperboloid in n-dimensions, which is represented by the following equation:

x²₁

a²_h −x²₂+x²₃+. . .+x²_n

b²_h = 1 (36)

Such a hyperboloid is a two-sheeted hyperboloid, and is characterized by the property that it is the locus of all points that satisfy the relation |r₁−r2| ≤2ah, where r1 and r2 represent the distances of a point on the hyperboloid to the two foci, and ah represents the semi-major axis of the hyperboloid. The hyperboloid is non-convex, and of infinite extent.

Fig. 4. A point and a non-convex confocal quadric.

However, the object B formed by the intersection of the n- dimensional hyperboloid with an-dimensional hyperspheroid (with both objects sharing the same foci) is of finite extent, and non-convex, as is illustrated in Fig. 4. Such a confocal quadric can be used to approximate a class of non-convex shapes.

As before, letr1 andr2 represent the distances of the point objectAto the two fociC1andC2 ofB. We note that in order for the pointAto collide withB, it must first hit the associated hyperspheroid, sinceB is contained inside the hyperspheroid.

Lettm represent the time of closest approach of Ato the hyperspheroid, as given in (27). The point A is on a collision course withBif|r₁(tm)−r2(tm)| ≤2ah. Substitutingr1 and r2 from (23), (24) andtm from (27), this leads to the following condition:

⎡

⎣rC1

ⁿ⁻¹

j=1

V_φj,C² ₁−rC2

ⁿ

j=1

V_φj,C² ₂

⎤

⎦

2

+ (r_C₂Vr,C2−rC1Vr,C1)²

×

⎡

⎢⎣rC1 n−1

j=1V_φj,C² ₁−rC2 n−1 j=1V_φj,C² ₂ rC1 n

j=1V_φj,C1² +rC2 n−1 j=1 V_φj,C² ₂

⎤

⎥⎦

2

≤4a²_hV_C² (37)

Accordingly, we define a scalar quantityyh, which represents a miss-distance function fromAtoBas follows:

yh =

⎡

⎣rC1

ⁿ⁻¹

j=1

V_φj,C² ₁−rC2

ⁿ

j=1

V_φj,C² ₂

⎤

⎦

2

+ (rC2Vr,C2−rC1Vr,C1)²

×

⎡

⎢⎣rC1 n−1

j=1V_φj,C² ₁−rC2 n−1 j=1 V_φj,C² ₂ rC1 n

j=1V_φj,C² ₁+rC2 n−1 j=1V_φj,C² ₂

⎤

⎥⎦

2

−4a²_hVC² (38) The collision cone betweenAandBis then defined as:

X ={α1, α2, . . . , αn−1 :y≤0, Vr,A B <0, yh ≤0} (39) and represents the set of heading angles ofAfor whichAis on a collision course withB.

(6)

Fig. 5. Example illustrating the collision cone in a4-D space.

V. EXAMPLE

We now illustrate the utility of the collision cone concept with an example. Consider a point object A and a hyperspheroid B moving in a 4-dimensional space. At some time tk, the position ofA is(−10,−15,−20,−25), while the position of the foci C1 and C2 of the hyperspheroid B are (5,0,0,0)and(−5,0,0,0), respectively. The semi-major axis of the hyperspheroid is taken asa= 7.Ais moving with speed VA= 7 m/sec, whileBhas a speedVB = 2 m/sec. The heading angles ofVB are defined by(β₁, β2, β3) = (20⁰,25⁰,35⁰).

Using (33)–(35), Fig. 5 shows the point cloud comprising the instantaneous set of heading angles(α₁, α2, α3)that will cause A to be on a collision course with B. If B continues to move with constant velocity, andAhas its velocity vector anywhere inside this point cloud, then it is guaranteed thatA will collide withB. We note that for each heading direction that lies in the point cloud, an approximate time to collision can be determined.

The point cloud can be integrated with an RRT algorithm as follows. If the hyperspheroid B represents an ob- stacle, then the RRT algorithm may choose the heading for the next waypoint for A such that it avoids the point cloud altogether. At a second level, the RRT algorithm may as- sign a time-to-collision threshold, based on which it could ig- nore points in the point cloud for which the time-to-collision lies above this threshold. It could then consider the subse- quent reduced-size point cloud and choose the heading for the next waypoint such that it avoids this reduced-size point cloud.

On the other hand, if the hyperspheroidBrepresents an area wherein the trajectory ofAshould lie, then the RRT algorithm may choose the heading for the next waypoint such that it lies in the point cloud, or at least lies in the point cloud with a higher probability.

VI. PATHPLANNING

The scalar quantities y defined in (33) (or yh defined in (38)) can be used to provide a foundation for path planning in n-dimensional spaces. They have the property that whenAand Bmove at constant velocities,y= constant(yh = constant), a fact that is readily demonstrated by verifying thatdy/dt= 0 (dyh/dt= 0) when evaluated along the system trajectories.

Furthermore, the equations y=c and yh =c, where c is a constant, can be viewed as level sets. Level sets withy=c >0

Fig. 6. Illustration of level sets corresponding to hyperspheroidB.

correspond to “virtual” hyperspheroids that contain the original hyperspheroid B, while level sets with y=c <0 correspond to virtual hyperspheroids that lie inside B. By applying an acceleration aA, the “aiming point” of the velocity vector of A can be shifted from one level set to another. Thus, when A has the objective of avoiding a collision with B, aA may be applied to modify the relative velocity vector such that y changes from a negative value to a non-negative one. On the other hand, when A has the objective of per- forming a rendezvous with B, then aA may be applied to drive the relative velocity vector into the y <0, Vr,A B <0 region.

This is schematically illustrated in Fig. 6. The position of A and the hyperspheroid B at time t= 0 are shown. Also shown is the position of B at time t=tm, and two repre- sentative level sets of B, represented as B andB, that are confocal withB and lie inside and outsideB, respectively, at t=tm. The level set corresponding to B is associated with y= 0. In order forAto graze the boundary of this level set, the velocity vector VA needs to be aligned with the boundary of the collision cone X. The level sets corresponding to BandBare associated withy=c1 <0andy=c2>0, respectively, and forAto graze the boundary of either of these level sets, the velocity vector ofAneeds to be as shown in the figure.

The acceleration vector ofAis represented by its components inn-dimensions as:

aA =aA

⎡

⎢⎢

⎣

cosγ1

sinγ1cosγ2

sinγ1sinγ2cosγ3

...

sinγ1sinγ2. . .sinγn−2cosγn−1

sinγ1sinγ2. . .sinγn−2sinγn−1

⎤

⎥⎥

⎦

where, γ1, γ2, . . . , γn−1 represent the angles associated with aA. Then, whenA has a non-zero acceleration, the kinematic differential equations governing the relative velocity ofBwith

(7)

respect toAare as follows:

⎡

⎢⎢

⎣ r˙i

φ˙1,C i

φ˙2,C i

... φ˙n−1,C i

V˙r,C i

V˙φ1,C i

... V˙φn−1,C i

⎤

⎥⎥

⎦

=

⎡

⎢⎢

⎢⎣

Vr,C i

Vφ1,C i

ri

Vφ2,C i

risinφ1,C i

... Vφn−1,C i

risinφ1,C isinφ2,C i...sinφn−2,C i

V_{φ1,C i}² +V_{φ2,C i}² +V_{φ3,C i}² ri

. . . . . . . . .

⎤

⎥⎥

⎥⎦

+

⎡

⎢⎢

⎢⎣ 0 0 0 ... 0

−ˆaA.ˆr,C i

−ˆaA.ˆφ1,C i

. . .

−ˆaA.ˆφn−1,C i

⎤

⎥⎥

⎥⎦

aA, i= 1,2 (40)

where,aˆArepresents the unit acceleration vector ofA. Equation (40) is given for a4-D scenario in the Appendix.

We now determine a guidance law foraAthat will enableVA

to change its aiming point from its current level set to another level set. Toward this end, define a Lyapunov function:

L= 1

2(y−w)² (41)

where,wcorresponds to the level set that needs to be attained.

We then determine an acceleration magnitudeaAthat will make L˙ negative definite, and this is a necessary condition to ensure that the level set corresponding toy=wis attained. SinceL˙ = (y−w)( ˙y−w), where˙ y˙is given by:

dy dt = ²

i=1

∂y

∂riVr,C i

+ ∂y

∂φ1,C i

φ˙1,C i+. . .+ ∂y

∂φn−1,C i

φ˙n−1,C i

+ ∂y

∂Vφ1,C i

V˙φ1,C i+. . .+ ∂y

∂Vφn−1,C i

V˙φn−1,C i

+ ∂y

∂Vr,C i

V˙r,C i

(42) It can then be seen that if we choose:

aA = K/2(y−w)

D (43)

where, D= ²

i=1

∂y

∂Vφ1,C i(ˆaA.ˆφ1,C i) +. . .

+ ∂y

∂Vφn−1,C i(ˆaA.ˆφn−1,C i) + ∂y

∂Vr,C i(ˆaA.ˆr,C i)

(44) then this will make the Lyapunov equation follow the dynam- icsL˙ =−KL. Assuming Vr,A B <0, we require thatK >0 forL˙ to be negative definite. Furthermore, since the Lyapunov function follows the dynamicsL(t) =L(0)e^{−K t}, we need that K >_t¹_m ln^L(0) (where < L(0)) in order to ensure thatLde- cays to(that is, the aiming point ofVA comes within√

of they=wlevel set by the timetm given in (27)).

VII. CONCLUSION

In this letter, analytical expressions of collision cones associated with a class of hyperquadric surfaces moving inn- dimensional configuration spaces, are determined. The collision cone provides the set of instantaneous heading angles of an object inn-dimensional space that will cause it to be on a collision course with another object. Analytical expressions of the collision cone are immensely helpful because of the com- putational savings they can provide. For instance, when used in conjunction with sampling-based planners such as RRTs and PRMs, the motion planners need not evaluate sample waypoints that lie inside a collision cone, or they may evaluate them with low probability. The cones can also be constructed to lead to desirable regions of the configuration space, in which case, the motion planners may evaluate samples that lie inside the cone, with higher probability.

Analytical expressions of these cones also serve as an efficient platform for the design of acceleration laws that enable an object to drive its velocity vector out of the collision cone (when the application is one of collision avoidance), or alternatively, drive its velocity vector into the cone (when the application is one of capturing a target, or of entering a desired region in the configuration space). A Lyapunov-based approach is used to determine such a nonlinear guidance law, that can effectively change the heading of the velocity vector of the object, as desired.

APPENDIX

ILLUSTRATIVE4-D E^XAMPLE

In a4-D scenario, after writing the full form expressions of each of the four relative velocity components, differentiating the resulting expressions, and re-arranging the algebraic terms, we obtain the following four differential equations:

V˙r = ˙φ1Vφ1+ sinφ1φ˙2Vφ2+ sinφ1sinφ2φ˙3Vφ3

V˙φ1 = −φ˙1Vr+ cosφ1φ˙2Vφ2+ cosφ1sinφ2φ˙3Vφ3

V˙φ2 = ˙φ2(−V_φ1cosφ1−Vrsinφ1) + cosφ2φ˙3Vφ3

V˙φ3 =−φ˙3(V_φ2cosφ2+Vφ1cosφ1sinφ2+Vrsinφ1sinφ2) (45)