Distance Computation Between Non-Holonomic Motions with Constant Accelerations

Abstract

A method for computing the distance between two moving robots or between a mobile robot and a dynamic obstacle with linear or arc-like motions and with constant accelerations is presented in this paper. This distance is obtained without stepping or discretizing the motions of the robots or obstacles. The robots and obstacles are modelled by convex hulls. This technique obtains the future instant in time when two moving objects will be at their minimum translational distance - i.e., at their minimum separation or maximum penetration (if they will collide). This distance and the future instant in time are computed in parallel. This method is intended to be run each time new information from the world is received and, consequently, it can be used for generating collision-free trajectories for non-holonomic mobile robots.

1. Introduction

Detecting a collision in motion planning is still an open research area in robotics. Nowadays, powerful motion planners are developed, where collision tests are an unavoidable step and represent, in general, a decisive time-consuming part of the whole planning algorithm.

A recent example and with important social impact is shown by [1, 2]. An estimated motion for an obstacle and a desired one for the robotized car Boss are stepped. Next, collision tests between the configurations of both objects at each considered time instant are run. The objects are modelled by boxes or circles. This collision-detection technique has several limitations, as shown by [3]. Nevertheless, this approach is common in the literature in order to detect collisions between mobile objects [4].

An extensive group of collision-detection methods is ‘continuous collision detection’ (CCD). In general, these methods provide, when there is a collision, the instant in time of the first contact [3,5–10]. In particular, when no collision is presented, the instant in time when both objects are at their closest position is returned by [10].

Another remarkable collision-detection technique is ‘reciprocal velocity obstacle’ (RVO) [11]. RVO is an oscillation-free navigation method for multiple agents with similar behaviours. RVO is an extension of the collision-detection technique ‘vehicle obstacle’ (VO) [12].

The contribution of this paper consists in obtaining the future instant in time when a robot and an obstacle or two robots, both in motion, will be at their minimum translational distance (MTD) of separation (if they will not collide) or penetration (if they will collide). The penetration distance verifies the definition given by [13].

Given that motions are not stepped, this method might be classified as a CCD technique, but really it is much more. When two robots or a robot and an obstacle will collide, the main advantage of computing the MTD of penetration and the associated instant in time versus the first time of contact is expressed in terms of generating a collision-avoidance trajectory. From the maximum penetration positions, a contact (or with a desired separation) position for the involved objects is obtained by translating, for instance, one of the robot's position by using the translational vector from the computed MTD [13]. In forcing this robot to be in this new position at the instant in time of the MTD, a new collision-free trajectory is proposed for that robot.

The robots and obstacles are modelled by bi-dimensional convex hulls. A trade-off between conservativeness in the models and computation costs has been set.

Considering the previous work in [14] as a collision detector, the main contributions of this paper are twofold: arc-like motions are now considered, and the involved robots and obstacles follow motions with non-null linear or angular accelerations.

The method in this paper is fast enough to be run as frequently as new information from the sensor system is received and, consequently, it is intended to be used as a module for collision detection and avoidance in trajectory planning algorithms dealing with non-holonomic robots.

For this paper, some experiments and simulations were run. The method was tested on the LEGO NXT Mindstorms platform. The velocity and acceleration of each robot is assumed to be measurable or estimable.

The main contributions of this paper with respect to [15] are an improved version of the support function, which is explained in Section 4, and a deepest analysis of our technique by applying it to real and simulated robots.

For convenience, some notations that will be used in this paper are listed here:

A,B: two mobile objects, modelled respectively, by a stope;

S^A(t), S^B(t): the sets of circles (s-topes) that describe the motions of objects A, B for t∊[t _s , t _s +Δt];

s_i ^A (t)=(c_i ^A (t),r_i ^A ); s_j ^B (t)=(c_j ^B (t),r_j ^M ): any circle in S^A(t), S^B(t);

S^A(t_s), S^B(t_s): the start positions of the motions S^A(t), S^B(t);

S^M(t): the Minkowski difference set between two motions;

s_ij ^M (t)=(c_ij ^M (t),r_ij ^M ): any circle or element in set S^M(t);

p ^M (λ),p_i ^M (λ) or p_ij ^M (λ) for λ∊[0, 1]: the trajectory followed by a centre c_ij ^M (t) for t∊[t _s ,t _s +Δt];

O: the origin point;

O^⊥, O _c , p ^M (λ _c ), p_i ^M (λ _c ) or p_ij ^M (λ _c ): the point in the trajectory p ^M (λ), p_i ^M (λ) or p_ij ^M (λ) which is the closest to O;

dO^M: the maximum approach (separation or penetration) achieved by two objects in motion.

tO^M: the future instant in time when two objects in motion will be at their maximum-approach positions;

2. Review of the GJK algorithm

Gilbert, Johnson and Keerthi [16] presented an algorithm, referred to as ‘GJK’, for computing, with linear complexity, the separation distance between two static polytopes.

A polytope is the convex hull of a finite set of points. The convex hull of the set Θ={p₀,p₁,…,p_n−1}, p_i∊ℜ3 ∀i, contains infinite points p that verify:

$$\left\{p:p=p_0+{\displaystyle \sum _{\mathrm{i}=1}^{n-1}{\text{λ}}_{\mathrm{i}}\cdot (p_{\mathrm{i}}-p_0),p_{\mathrm{i}}∊\mathrm{\Theta },{\text{λ}}_{\mathrm{i}}∊[0,1],{\displaystyle \sum _{\mathrm{i}=1}^{n-1}{\text{λ}}_{\mathrm{i}}\le 1}}\right\}$$ (1)

The order of a polytope is the number of points in set Θ.

The GJK algorithm obtains the separation between the origin point O and the Minkowski difference of the involved polytopes. The Minkowski difference between polytopes A, B, defined by the sets P^A={a_i}, P^B={b_j} with a_i, b_jℝ³, i=0,1,…,n–1 j=0,1,…,m–1, is also a polytope defined by a set of n×m points P^A–B={a_i–b_j: a_i ∊ P^A, b_j ∊ P^B, ∀i,j}.

In the GJK algorithm, set V_k always contains from one to four points of set P^A–B. Initially, some points from P^A–B are randomly assigned to V_k. Next, an iterative process starts.

The first step, called the ‘sub-distance algorithm’, consists of computing the distance from O to the polytope defined by the points in V_k. This distance is obtained by projecting O onto the mentioned polytope. This projected point is called ‘O^⊥’. The points in V_k, which are not required to describe the face, edge or vertex where O^⊥ is, are removed from V_k. In particular, after this step, if the set V_k contains four points because O is inside the polytope defined by V_k, then the GJK algorithm will finish immediately without returning a distance. This situation means that polytopes A and B are colliding. The GJK algorithm does not compute a penetration distance.

If the sub-distance algorithm finally returns a distance, the next step in the GJK algorithm consists of selecting the closest point in P^A–B to O in the direction –O^⊥. This point, s_A–B(−O^⊥), is found by applying the support h_A–B and mapping s_A–B functions:

$$\begin{array}{l}{h}_{A-B}(-O^{\perp })=\underset{\text{i,j}}{\max }\left\{(a_{\mathrm{i}}-b_{\mathrm{j}})\cdot (-O^{\perp }),a_{\mathrm{i}}-b_{\mathrm{j}}∊P^{A-B},\forall \text{i,j}\right\}\\ s_{A-B}(-O^{\perp })∊P^{A-B}:h_{A-B}(-O^{\perp })=s_{A-B}(-O^{\perp })\cdot (-O^{\perp })\end{array}$$ (2)

Now, if ||O^⊥||²+h_A–B(−O^⊥)=0 (final condition) is true, then the GJK algorithm ends [16]. Otherwise, point s_A–B(−O^⊥) is added to V_k and the GJK algorithm iterates once more.

The GJK algorithm has a linear complexity O(n+m), because the Minkowski difference is not computed before running the GJK algorithm. This is a consequence of the definition of the support function, since it verifies h_A–B(−O^⊥)=h_A(−O^⊥)+h_B(O^⊥).

The GJK algorithm finishes in less than five or six iterations even when dealing with high-order polytopes [16].

The GJK algorithm was updated in [17] to compute the separation or penetration distance for polytopes and spherically-extended polytopes (s-topes) [18].

3. Review of continuous distance computation for robots following linear motions with null accelerations

A technique for obtaining the future instant in time when a mobile robot and a dynamic obstacle will be at their minimum translational distance (MTD) is shown by [19]. Robots and obstacles are modelled indistinctively by polytopes or s-topes and follow linear motions at a constant speed. The distance and the instant in time are computed without stepping any robot or obstacle's motion.

An s-tope is the convex hull of a finite set of spheres - circles if bi-dimensional - S={s₀,s₁,…,s_n–1} with s_i=(c_i,r_i), where c_i is the centre and r_i is the radius. An s-tope contains an infinite set of swept spheres s (or circles) expressed by:

$$\begin{array}{l}\hfill \{s=(c,r):c=c_0+{\displaystyle {\sum }_{\mathrm{i}=1}^{n-1}{\text{λ}}_{\mathrm{i}}(c_{\mathrm{i}}-c_0)},r=r_0+{\displaystyle {\sum }_{\mathrm{i}=1}^{n-1}{\text{λ}}_{\mathrm{i}}(r_{\mathrm{i}}-r_0)},\\ \hfill s_{\mathrm{i}}=(c_{\mathrm{i}},r_{\mathrm{i}})∊S,{\text{ λ}}_{\mathrm{i}}∊[0,1],{\displaystyle {\sum }_{\mathrm{i}=1}^{n-1}{\text{λ}}_{\mathrm{i}}\le 1}\}\end{array}$$ (3)

If the radii r_i are zero, then (3) matches with the polytope definition (1) (i.e., a polytope is a particular case of an stope).

Let A and B be mobile robots or obstacles whose positions at t_s are given respectively by the sets of circles S ^A (t _s )={s₀ ^A (t _s ),s₁ ^A (t _s ),…,s ^A _n−1(t _s )} and S ^B (t _s )={s₀ ^B (t _s ),s₁ ^B (t _s ),…,s ^B _m−1(t _s )} where c_i ^A (t _s ),c_j ^B (t _s )∊ℜ² and r_i ^A , r_j ^B ∊ℜ are, respectively, the centres and radii ∀i,j. The A and B constant velocities are given by the vectors v_A, v_B ∊ ℝ².

Each one of the infinite intermediate positions of the objects A and B, c_i ^A (t) and c_j ^B (t), from t_s to t_s+Δt are parameterized by λ ∊ [0, 1] as:

$$\begin{array}{l}{c}_{\mathrm{i}}^A(t)=c_{\mathrm{i}}^A(t_s)+\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot v_A;c_{\mathrm{j}}^B(t)=c_{\mathrm{j}}^B(t_s)+\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot v_B;\forall \text{i,\hspace{0.05em}j}\\ \forall t:t=t_s+\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t:t∊[t_s,t_s+\mathrm{\Delta }t]\text{ and λ\hspace{0.05em}}∊\text{\hspace{0.05em}[0, 1]}\end{array}$$ (4)

The GJK-based algorithm in [19] computes the future instant in time when A and B will be at their MTD as the distance between O and the Minkowski difference of all the infinite intermediate positions of A and B.

The Minkowski difference between all the A and B infinite intermediate positions for t ∊ [t_s,t_s+Δt], called ‘S^M(t)’, is defined by the set of n×m circles {s_ij ^M (t)}. s_ij ^M (t) is parameterized by λ ∊ [0, 1] as:

$$\begin{array}{l}\{s_{\mathrm{i}\mathrm{j}}^M(t)\}=\{(c_{\mathrm{i}\mathrm{j}}^M(t),r_{\mathrm{i}\mathrm{j}}^M):c_{\mathrm{i}\mathrm{j}}^M(t)=c_{\mathrm{i}}^A(t)-c_{\mathrm{j}}^B(t),\\ r_{\mathrm{i}\mathrm{j}}^M=r_{\mathrm{i}}^A+r_{\mathrm{i}}^B;\forall \text{i,j, }t=t_s+\text{\hspace{0.05em}λ·}\mathrm{\Delta }t,\text{ λ}∊\text{[0, 1]}\}\end{array}$$ (5)

c_i ^A (t), c_j ^B (t) are defined in (4). Each circle s_ij ^M (t) for all t ∊ [t_s,t_s+Δt] sweeps an area consisting of a rectangle whose ends are capped off with circles. This geometrical figure is referred to as a ‘stadium’ in [20]. Next, S^M(t) is formed by n×m stadiums, and each one is described by three parameters: a start point c_ij ^M (t_s)=c_i ^A (t_s)–c_j ^B (t_s), a radius r_ij ^M =r_i ^A +r_j ^B and a linear axis p^M(λ) ∊ ℝ². p^M(λ) is parametrically defined by λ ∊ [0, 1] as:

$$\begin{array}{l}{p}^M(\text{λ})=c_{\mathrm{i}}^A(t_s+\text{\hspace{0.05em}λ}\mathrm{\Delta }t)-c_{\mathrm{j}}^B(t_s+\text{\hspace{0.05em}λ}\mathrm{\Delta }t)-(c_{\mathrm{i}}^A(t_s)-c_{\mathrm{j}}^B(t_s))=\\ =\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t(v_A-v_B)\end{array}$$ (6)

All the axes of the stadiums have the same length ||p^M(1)|| = Δt · ||v_A–v_B||. Figure 1 shows the Minkowski difference S^M(t) with 4×2 stadiums from two constant-speed and linear motions.

Figure 1.

Eight stadiums in the Minkowski difference S^M(t) from two constant-speed and linear motions. For clarity, only the axes (dashed lines) and the capping circles of the stadiums are depicted. c₁₀ ^M (t_s), c₃₁ ^M (t_s) are the start points of the two stadiums that hold the external edges of S^M(t).

The GJK-based algorithm in [19] mainly requires the definition of a new sub-distance algorithm and the support and mapping functions.

The sub-distance algorithm receives one or two stadiums in set V_k and computes the distance from O to the closest stadium. The distance between O and the stadium with the start point c_ab ^M (t_s)=c_a ^A (t_s)–c_b ^B (t_s) and the radius r_ab ^M is determined by obtaining O^⊥ (i.e., by projecting O onto the stadium's axis). Therefore:

$$\begin{array}{l}{O}^{\perp }=c_a^A(t_s)-c_b^B(t_s)+p^M({\lambda }_c)\\ \text{with }{\lambda }_c=-((c_a^A(t_s)-c_b^A(t_s))\cdot p^M(1))/{\Vert p^M(1)\Vert }^2\end{array}$$ (7)

Finally, the sub-distance algorithm returns the parameters O^⊥ and λ _c and rejects (if it receives two) the furthest stadium (i.e. this stadium is removed from V_k).

The support h_M and mapping S_M functions determine the closest stadium in S^M(t) in the direction given by –O^⊥:

$$\begin{array}{l}{h}_M(-O^{\perp })=\underset{\text{i,j}}{\max }\left\{c_{\mathrm{i}\mathrm{j}}^M(t_s)\cdot (-O^{\perp })+r_{\mathrm{i}\mathrm{j}}^M\cdot \left|\right|O^{\perp }\left|\right|,\text{ ( }{c}_{\mathrm{i}\mathrm{j}}^M(t_s),r_{\mathrm{i}\mathrm{j}}^M)∊S^M(t)\right\}\\ s_M(-O^{\perp })=(c_s,r_s)∊S^M(t):h_M(-O^{\perp })=-c_s\cdot O^{\perp }+r_s\cdot \left|\right|O^{\perp }\left|\right|\end{array}$$ (8)

where s_M(−O^⊥) contains the start point of the stadium that generates the maximum in h_M(−O^⊥).

The complexity of the algorithm in [19] is linear since h_M(−O^⊥)=h_A(−O^⊥)+h_B(O^⊥) is true.

This GJK-based algorithm needs less than four iterations to find the instant in time, t_O^M, when A and B will be at their MTD, called ‘d_O^M’. t_O^M and d_O^M are obtained from the distance between O to the closer external edge of S^M(t) [19] (see figure 1), and mathematically by:

$$\begin{array}{l}{t}_O^M=t_s+{\text{λ}}_c\text{ ·}\mathrm{\Delta }t\hfill \\ d_O^M=\left|\right|O^{\perp }\left|\right|-r_{ab}^M\hfill \end{array}\text{ ; with }{t}_O^M∊[t_s,t_s+\mathrm{\Delta }t]\text{ and\hspace{0.05em}}{\text{ λ}}_c∊\text{[0, 1]}$$ (9)

Note that r_ab ^M is finally the radius of the closest external stadium to O (r_ab ^M is r₁₀ ^M in figure 1).

Nevertheless, when the sub-distance algorithm receives two stadiums, and O is inside the area delimited by the axes of these stadiums, then the distance, d_O^M, between the inner O and the external edge is reformulated as d_OM^M=−(||O^⊥||+r_ab^M), where O^⊥ is the projection of O onto the axis of the stadium in V_k that is the closest to O. d^OM is converted into a negative number and the radius has to be added, since it represents a translational distance of penetration [13]. t^OM is computed as indicated by (9).

4. Continuous distance computation between two mobile objects with constant acceleration

A technique for computing the future instant in time when two mobile objects (robots and obstacles) are at their minimum translation distance (MTD) of separation or penetration is introduced in this section. The mentioned instant in time and MTD are computed without steeping any involved motion.

An object's motion is characterized by its initial position, its start and goal times (time span), its initial velocity, its constant acceleration, and its linear or arc-like (non-holonomic) path.

Three situations are considered in this paper: where the objects follow linear motions (case called LL); where one object follows an arc-like and where the other one follows a linear motion (AL); and where both objects follow arc-like motions (AA).

4.1 Problem Formulation

The s-topes for modelling the robots and obstacles and their motions are formally defined in this subsection.

Let us consider two mobile objects A and B, each modelled respectively by a s-tope. The A and B positions at the start time t_s are given by S^A(t_s) and S^B(t_s) respectively. S^A(t_s) and S^B(t_s) are defined by the set of circles S^A(t_s)={s₀ ^A (t_s),s₁ ^A (t_s),…,s_{n_1} ^A (t_s)} and S^B(t_s)={s₀ ^B (t_s),s₁ ^B (t_s),…, s_m–1 ^B (t_s)} where c_i ^A (t_s), c_j ^B (t_s) ∊ ℝ² and r_i ^A , r_j ^B ∊ ℝ are the centres and radii of the circles s_i ^A (t_s) and s_j ^B (t_s), ∀i, j, respectively.

Motions are considered from t_s to t_s+Δt, where Δt is a time horizon.

When A and B are following linear motions, their initial velocities - i.e., at t_s - are v_A(t_s) and v_B(t_s) ∊ ℝ², respectively, and their constant accelerations are a_A, a_B ∊ ℝ.

If A and B are following arc-like motions centred at c_A and c_B ∊ ℝ², respectively, then each c_i ^A (t_s), c_j ^B (t_s), ∀i, j can be rewritten in polar coordinates by using the arc radius, ρ_i ^A , ρ_j ^B - i.e., its respective distance to the arc centre - and its initial angular position, θ_i ^A (t_s), θ_j ^B (t_s), as:

$$\begin{array}{c}{c}_{\mathrm{i}}^A(t_s)=c_A+{\rho }_{\mathrm{i}}^A(\cos ({\text{θ}}_{\mathrm{i}}^A(t_s)),\sin ({\text{θ}}_{\mathrm{i}}^A(t_s)))\\ c_{\mathrm{j}}^B(t_s)=c_B+{\rho }_{\mathrm{j}}^B(\cos ({\text{θ}}_{\mathrm{j}}^B(t_s)),\sin ({\text{θ}}_{\mathrm{j}}^B(t_s)))\end{array}\text{ ; }\forall \text{i,j}$$ (10)

A and B's angular speed at t_s are, respectively, ω _A (t_s) and ω _B (t_s) ∊ ℝ. Their constant angular accelerations are α _A and α _B ∊ ℝ, respectively.

Each one of the infinite intermediate positions of A from t_s to t_s+Δt is parameterized by λ ∊ [0, 1] as:

$$c_{\mathrm{i}}^A(t)=c_{\mathrm{i}}^A(t_s)+\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot v_A(t_s)+0.5\cdot {\text{λ}}^2\cdot \mathrm{\Delta }{t}^2\cdot a_A\cdot {\widehat{v}}_A(t_s);\forall \mathrm{i}$$ (11)

$$\begin{array}{l}\hfill c_{\mathrm{i}}^A(t)=c_A+{\rho }_{\mathrm{i}}^A\left(\cos ({\text{θ}}_{\mathrm{i}}^A(t)),\sin ({\text{θ}}_{\mathrm{i}}^A(t))\right);\forall \mathrm{i}\\ \hfill \text{with }{\text{θ}}_{\mathrm{i}}^A(t)={\text{θ}}_{\mathrm{i}}^A(t_s)+\text{ λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot w_A(t_s)+0.5\cdot {\text{λ}}^2\cdot \mathrm{\Delta }{t}^2\cdot {\text{α}}_A\end{array}$$ (12)

∀t:t=t _s +λ Δt, λ∊[0, 1], t∊[t _s ,t _s +Δt] and ${\widehat{v}}_A(t_s)=v_A(t_s)/\left|\right|v_A(t_s)\left|\right|$ . A is following a linear motion in case (11) and an arc-like motion in (12). (11) and (12) are analogously modified for B.

A constraint is contemplated from the motions in (11) and (12). A motion with a change from forward to backwards, and from counter-clockwise to clockwise, or vice versa, is not considered. In this case, the motion is properly divided.

The proposed GJK-based algorithms deal with S^M(t), with t ∊ [t_s,t_s+Δt]. S^M(t) is the Minkowski difference between all the A and B infinite intermediate positions while A and B are following their respective motions. S^M(t) has been defined in (5) and, independently of the involved motions, S^M(t) is defined by n×m stadiums, and each stadium is described by its start point c_i ^A (t_s)–c_j ^B (t_s), a radius r_ij ^M =r_i ^A +r_j ^B and an axis p^M(λ) ∊ ℝ² with λ ∊ [0, 1].

The future instant in time when A and B are located at their MTD of separation or penetration is obtained by computing the distance between O and S^M(t) (specifically, from O to its closer external edge of S^M(t)).

4.2 Dealing with Two Straight-Line Motions (LL)

Two GJK-based algorithms are introduced in this subsection. They deal with mobile objects (robot or obstacle) A and B with linear motions and constant accelerations. The LL-GJK algorithm obtains the future instant in time, t^OM, when A and B will be at their MTD of separation, d^OM, or it returns a failure if A and B will collide at t ∊ (t_s,t_s+Δt]. In case of a collision, the LL_in–GJK algorithm is then run and returns the future instant in time t^OM when A and B will be at their MTD of penetration, d^OM.

The Minkowski difference between these two motions is S^M(t), t ∊ [t_s,t_s+Δt], and it has n×m stadiums whose axes, p^M(λ), are parabolic with (see (6)):

$$\begin{array}{l}{p}^M(\text{λ})=\text{ λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\left(v_A(t_s)-v_B(t_s)\right)+\\ +0.5\cdot {\text{λ}}^2\cdot \mathrm{\Delta }{t}^2\left(a_A\cdot {\widehat{v}}_A(t_s)-a_B\cdot {\widehat{v}}_B(t_s)\right)\end{array}$$ (13)

Any GJK-based algorithm to design requires: a sub-distance algorithm, the support and mapping functions, and a final condition.

The sub-distance algorithm computes the distance, d_O, between O and the axis of the stadium whose start point is in V_k. Let us consider the stadium with a start point c_a^A(t_s)–c_b^B(t_s), then such a distance d_O is determined by finding the parameter λ _c that verifies:

$$d\Vert c_a^A(t_s)-c_b^B(t_s)+p^M(\text{λ})\Vert /d\text{λ}=\text{ 0}$$ (14)

λ _c is found by applying the root-finding technique, termed the ‘Secant method’ [21]. Experimentally, λ₀=0.45 and λ₁=0.55 have been confirmed as good choices as initial values for the Secant method. The searching accuracy has been set to 10⁻⁶.

After finding λ _c , O_c ∊ ℝ₂ (the axis's closest point to O) and d_O are obtained as:

$$O_c=c_a^A(t_s)-c_b^A(t_s)+p^M({\text{λ}}_c)\text{ ; }{d}_O=\text{||\hspace{0.05em}}{O}_c\text{\hspace{0.05em}||}$$ (15)

Set V_k contains as maximum of two start points (stadiums). Only when the LL-GJK algorithm is being run and the set V_k contains two stadiums, the sub-distance algorithm first checks whether O is inside the area delimited by the axes of these two stadiums. If the result of this test is true, A and B will collide [16,17,19], and the LL-GJK algorithm finishes immediately with a failure and returns the set V_k. It the result of this test is false, the sub-distance algorithm returns the distance, d_O, from O to the closest stadium in V_k together with the corresponding parameters λ _c , O_c.

When the LL_in–GJK algorithm is being run, the sub-distance algorithm returns the distance from O to the furthest stadium d_O, if more than one in V_k and λ _c , O_c.

In any case, the sub-distance algorithm rejects and removes from V_k the stadium whose distance to O has not been returned.

The support h_M and mapping s_M functions are used for finding the furthest stadium in a given direction η ∊ ². This stadium is theoretically the candidate to be the closest stadium to O.

The GJK-based algorithm in [19] deals with stadiums whose axes are straight lines. Moreover, and for this reason, the support function in (8) works properly. Note that this support function uses the start points of the involved axes, c_ij ^M (t_s).

Now, we are dealing with non-convex (parabolic) axes. Therefore, the support function in (8) is not valid here. This difficulty is overcome by applying the support function to different points of a given axis instead of just to the start point. These points are characterized by the golden ratio parameter r, with $r=\text{\hspace{0.05em}(}\sqrt{5}-1\text{)}/2$ [21].

Let c_a^A(t_s)–c_b^B(t_s) be the start point of a stadium with a radius r_a^A+r_b^B and axis p^M(λ) with λ ∊ [0, 1], the support function h_M is applied to the points in the axis:

$$c_{\mathrm{i}}^A(t_s)-c_{\mathrm{j}}^B(t_s)+p^M(\text{μ})\text{ ; with μ\hspace{0.05em}}=0,1-r,r,1,{\text{λ}}_c;{\text{ λ}}_c∊[0,1]$$ (16)

Then, the support function h_M is defined as:

$$\begin{array}{l}{h}_M(\text{ η,μ })=\underset{\text{i,j}}{\max }\left\{\left(c_{\mathrm{i}}^A(t_s)-c_{\mathrm{j}}^B(t_s)+p^M(\text{μ })\right)\cdot \text{\hspace{0.05em}η\hspace{0.05em}}+(r_{\mathrm{i}}^A+r_{\mathrm{j}}^B)\cdot \left|\right|\text{\hspace{0.05em}η||}\right\}\hfill \\ \text{ with }(c_{\mathrm{i}}^A(t_s),r_{\mathrm{i}}^A)∊S^A(t_s);(c_{\mathrm{j}}^B(t_s),r_{\mathrm{j}}^B)∊S^B(t_s)\hfill \end{array}$$ (17)

The parameter λ _c to be used in (17) is that returned by the last execution of the sub-distance algorithm.

The mapping function s_M(η,µ) contains the start point of the stadium that generates the maximum in h_M(η,µ). Consequently, according to µ, s_M(η,µ) represents up to five different stadiums. The stadium with the minimum distance to O is selected and called s_M(η) in the LL-GJK algorithm. Conversely, s_M(η) contains the stadium with the maximum distance to O for the LL_in–GJK algorithm. In any case, the other stadiums are rejected.

These new h_M and s_M functions are an improvement of those introduced in [15].

If the LL-GJK algorithm is being run, then η is substituted by –O_c in (17). Conversely, if the LL_in–GJK algorithm is being run, then O_c is used in (17) instead of η.

The LL-GJK and LL_in–GJK algorithms finish when the stadium in s_M(η) is the same as that represented by s_M(η,λ _c ) - i.e., s_M(η)=s_M(η,λ _c ) - and a final condition is also verified. Therefore, the LL-GJK algorithm finishes successfully, after obtaining the separation distance from O to the closest external edge in S^M(t) (see figure 1), when the conditions s_M(−O_c)=s_M(−O_c,λ _c ) and g_M(−O_c,λ _c )=0 are true, with:

$$g_M(-O_c,{\text{λ}}_c)=\left|\right|O_c|{|}^2-radius(s_M(-O_c,{\text{λ}}_c))+h_M(-O_c,{\text{λ}}_c)$$ (18)

Given that the LL_in–GJK algorithm deals with O inside the area delimited by the axes from S^M(t), it finishes when the conditions s_M(O_c)=s_M(O_c,λ _c ) and ĝ_M(O_c,λ _c )=0 are true, with:

$${\widehat{g}}_M(O_c,{\text{λ}}_c)=-\left|\right|O_c|{|}^2-radius(s_M(O_c,{\text{λ}}_c))+h_M(O_c,{\text{λ}}_c)$$ (19)

The function radius returns the radius of the stadium represented by s_M(η,λ _c ). The final conditions g_M and ĝ_M are updated from their equivalent in [19].

If the LL-GJK algorithm finishes with a failure (A and B will collide), it returns a set V_k containing two stadiums. Next, the LL_in–GJK is run twice to obtain the MTD of penetration and the associated instant in time. Each execution is started by providing as initial set, namely an element from the mentioned set V_k.

Each execution of the LL_in–GJK algorithm returns a parameter λ _c and a negative distance d_O^M' (computed as indicated in the last paragraph of Section 3). The maximum of these two negative distances holds d_O^M (i.e., the MTD of penetration between A and B). From the execution that gives value to d_O^M, the future instant in time t_O^M is calculated by using the returned λ _c as indicated by (9).

The parameter O_c returned by both the LL-GJK and LL_in–GJK algorithms holds a translational vector (i.e., if, for instance, the position of A at t^OM is translated as d_O^M in the direction O_c, then A and B will be in contact at t^OM).

For clarity, the LL-GJK and LL_in–GJK algorithms are presented in pseudocode.

A discrete motion representation of a mobile robot A and a dynamic obstacle B, both following linear motions with constant accelerations, is shown in figure 2.a. The positions where A and B are at their MTD of penetration, are depicted in red. The Minkowski difference S^M(t) between both continuous motions is shown in figure 2.b. A is modelled by a fourth-order stope (polytope) and B is a second-order s-tope. d_O^M and t_O^M have been obtained in 5.1 µs in an Intel® Core™ i5 650 processor at 3.2 GHz.

Figure 2.

The distance between two mobile objects A and B following linear motions, with ||v_A(t_s)||=2.2 m/s, a_A=1 m/s², ||v_B(t_s)||=3 m/s, a_B=−0.5 m/s², t_s=0s and Δt=5s. (a) The A and B positions are only depicted at t_s, 0.5s, 1.25s, 2s, 2.75s and 3.5s. The positions at t_O^M=2.75s where A and B are at their MTD of penetration, d_O^M, are in red. (b) The Minkowski difference between the A and B motions. S^M(t) has eight stadiums. For clarity, only the S^M(t) external edge close to O, its associated capping circles, its distance, d_O^M, to O, and all the axes, are depicted.

Input: S^A(t_s), S^B(t_s), t_s, Δt, p^M(λ), r=(√5 − 1) / 2 Output: (λ _c , t_O^M, d_O^M,O_c,V_k) or (failure, V_k) 1:      k=0, V_k={c₀ ^A (t_s)–c₀ ^B (t_s),r₀ ^A +r₀ ^B } with (c₀ ^A (t_s),r₀ ^A )∊S^A(t_s), and (c₀ ^B (t_s),r₀ ^B )∊S^B(t_s) 2:    do 3:      (λ _c ,d_O,O_c,V_k,O_inside) ← sub-distance_algorithm(V_k) 4:      if O_inside then return(failure,V_k) 5:      compute h_M(−O_c, µ), s_M(−O_c,µ) for µ=0,1–r,r,1,λ _c 6:      s_M(−O_c)=select_closest_stadium_to_O(s_M(−O_c,µ)) 7:      if s_M(−O_c)=s_M(−O_c,λ _c ) & g_M(−O_c,λ _c )=0 then exitloop endif 8:      V_k+1=V_k∪{s_M(−O_c)} 9:      k=k+1 10: while true 11: tO^M=t_s+λ_c·Δ_t; dO^M=dO⁻(r_p^A+r_q^B)         where V_k={c_p^A(t_s)−c_q^B(t_s),r_p^A+r_q^B};         with (c_p^A(t_s)∊S^A(t_s), (c_q^B(t_s)∊S^B(t_s) 12: return(λ _c , t_O^M, d_O^M, O_c, V_k)

Algorithm 1. LL–GJK algorithm

Input: S^A(t_s), S^B(t_s), t_s, Δt, p^M(λ), r=(√5 − 1) / 2, and a one-element set V_in Output: (λ _c , d_O^M',O_c,V_k) 1: k=0, V_k=V_in 2: do 3:      (λ _c ,d_O,O_c,V_k) ← sub-distance_in_algorithm(V_k) 4:      compute h_M(O_c,µ), s_M(O_c,µ) for µ=0,1–r,r,1,λ _c 5:      s_M(O_c)=select_furthest_stadium_to_O(s_M(O_c,µ)) 6:      if s_M(O_c)=s_M(O_c,λ _c ) & ĝ_M(O_c,λ _c )=0 then exitloop endif 7:      V_k+1=V_k∪{s_M(O_c)} 8:      k=k+1 9: while true 10:    dO^M′=−(d_O+(r_p^A+r_q^B)) with V_k={c_p^A(t_s)-c_q^B(t_s),r_p^A+r_q^B};           and (c_p^A(t_s),r_p^A)∊(t_s), (c_q^B(t_s),r_q^B)∊(t_s) 11: return(λ _c , d_O^M', O_c, V_k)

Algorithm 2. LL_in–GJK algorithm

4.3 Dealing with Arc-like and Straight-line Motions (AL)

Now, a mobile robot A is following an arc-like motion while the other mobile robot or obstacle B follows a linear motion. These motions are described in subsection 4.1.

The future instant in time, when the mobile A and B will be at their MTD of separation, is obtained by applying the AL–GJK algorithm. If it fails, because A and B will collide at a time instant t ∊ (t_s,t_s+Δt], then the AL_in–GJK algorithm is run to obtain their MTD of penetration and the corresponding instant in time.

As already mentioned, this future instant in time and the corresponding MTD are obtained by computing the distance from O to the closest external edge of the Minkowski difference of the involved motions.

The AL-GJK and AL_in–GJK algorithms are, respectively, analogous to the LL-GJK and LL_in–GJK algorithms. Only the subtle differences are pointed out here.

In accordance with the motion definition in (11) and (12), the Minkowski difference S^M(t) between both motions has n×m stadiums whose axes are now cycloid-like. Furthermore, there are n different cycloid-like axes. Each of these n axes p_i ^M (λ) ∊ ℝ² ∀i is described by λinsin;[0, 1] as:

$$\begin{array}{l}{p}_{\mathrm{i}}^M(\text{λ})={\rho }_{\mathrm{i}}^A\left(\cos ({\text{θ}}_{\mathrm{i}}^A(t)),\sin ({\text{θ}}_{\mathrm{i}}^A(t))\right)-\text{λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot v_B(t_s)-\\ -0.5\cdot {\text{λ}}^2\cdot \mathrm{\Delta }{t}^2\cdot a_B\cdot {\widehat{v}}_B(t_s);\text{ i}=\text{0,1}\dots ,n-\text{\hspace{0.05em}1\hspace{0.05em}}\end{array}$$ (20)

θ_i ^A (t) depends upon λ - see (12) - with t=t_s+λ·Δt. Let us consider a stadium described by c_a^A(t_s)–c_b^B(t_s), r_a^A+r_b^B and p_a^M(λ) with (c_a^A(t_s),r_a^A) ∊ S^A(t_s) and (c_b^B(t_s),r_b^B) ∊ S^B(t_s). The sub-distance algorithm obtains the desired distance by finding the λ _c that minimizes ||c_A–c_b^B(t_s)+p_a^M(λ)|| by solving:

$$d\Vert c_A-c_b^B(t_s)+p_a^M(\text{λ})\Vert /d\text{λ}=\text{ 0}$$ (21)

λ _c is then found by applying the Secant method to (21), but this method will work properly if there is one minimum in ||c_A–c_b^B(t_s)+p_a^M(λ)||. Given that the axes of the stadiums are cycloid-like, if A's angular displacement is lower than π, then ||c_A–c_b^B(t_s)+p_a^M(λ)|| for all λ ∊ [0, 1] contains, in the worst case, one maximum and one minimum (apart from the extremes of the search interval). Consequently, if such a condition is false, then the A and B motions are properly divided before running the AL-GJK and AL_in–GJK algorithms.

The support function h_M is also required to be modified as:

$$\begin{array}{l}{h}_M(\text{η,μ })=\underset{\forall \text{i,j}}{\max }\left\{\left(c_A-c_{\mathrm{j}}^B(t_s)+p_{\mathrm{i}}^M(\text{μ })\right)\cdot \text{\hspace{0.05em}η\hspace{0.05em}}+(r_{\mathrm{i}}^A+r_{\mathrm{j}}^B)\cdot \left|\right|\text{\hspace{0.05em}η|| }\right\}\\ \text{ with μ\hspace{0.05em}}=0,1-r,r,1,{\text{λ}}_c;{\text{ λ}}_c∊[0,1]\end{array}$$ (22)

An example of the execution of these algorithms is shown in figure 3. A is modelled by a fourth-order s-tope (polytope), while B is a second-order s-tope. d_O^M and t_O^M have been obtained in 10.2 µs in an Intel® Core™ i5 650 processor at 3.2 GHz.

Figure 3.

The distance between A and B. A follows an arc-like motion. B follows a linear motion. The motions are defined by ω _A (t_s)=−18°/s, α _A =−0.5°/s², ||v_B(t_s)||=3 m/s, a_B=1 m/s², t_s=0 s and Δt=5 s. (a) The A and B positions are stepped at t_s, 0.6s, 1.1s, 1.6s and 2.1s. The positions where A and B are at their MTD of penetration, d_O^M, are in red with t_O^M=1.1s. (b) The Minkowski difference between the A and B motions. S^M(t) has eight stadiums and four different cycloid-like axes. For clarity, only the S^M(t) external edge close to O, its associated capping circles, its distance, d_O^M, to O, and all the axes, are depicted.

4.4 Dealing with Two Arc-like Motions (AA)

In this subsection, a mobile robot A and a robot or obstacle B are following arc-like motions with constant angular accelerations. These motions have been described in subsection 4.1.

The future instant in time, when the mobile A and B will be at their MTD, is also obtained by computing the distance from O to the closest external edge of the Minkowski difference of the involved motions.

The AA-GJK algorithm computes the future instant in time when A and B will be at their MTD of separation. If this algorithm fails - i.e., if it detects that A and B will collide at t ∊ (t_s,t_s+Δt] - then the AA_in–GJK algorithm computes the future instant in time when both objects will be at their MTD of penetration. These algorithms are analogous to the previous ones. Only the differences are shown here.

The axes of the stadiums in S^M(t) are now rose-like (a rhodonea curve) [22]. S^M(t) has n×m different axes p_ij ^M (λ) ∊ ℝ² ∀i,j. These axes are parameterized by λ ∊ [0, 1] as:

$$\begin{array}{l}{p}_{\mathrm{i}\mathrm{j}}^M(\text{λ})={\rho }_{\mathrm{i}}^A(\cos ({\text{θ}}_{\mathrm{i}}^A(t)),\sin ({\text{θ}}_{\mathrm{i}}^A(t)))-{\rho }_{\mathrm{j}}^B(\cos ({\text{θ}}_{\mathrm{j}}^B(t)),\sin ({\text{θ}}_{\mathrm{j}}^B(t)))\\ t=t_s+\text{\hspace{0.05em}λ\hspace{0.05em}}\cdot \mathrm{\Delta }t\text{ ; }t∊\text{\hspace{0.05em}[}{t}_s,t_s+\mathrm{\Delta }\text{t]}\end{array}$$ (23)

where θ_i ^A (t) and θ_j ^B (t) are described by (12).

Let c_a^A(t_s)–c_b^B(t_s), r_a^A+r_b^B and p_ab^M(λ) be a stadium in S^M(t) with (c_a^A(t_s),r_a^A) ∊ S^A(t_s) and (c_b^B(t_s),r_b^B) ∊ S^B(t_s). The sub-distance algorithm computes the distance from O to the mentioned stadium by finding λ _c that minimizes ||c_A–c_B+p_ab^M(λ)||, i.e., by applying the Secant method to:

$$d\Vert c_A-c_B+p_{ab}^M(\text{λ})\Vert /d\text{λ}=\text{ 0}$$ (24)

If A and B's angular displacements are lower than π, then ||c_A–c_b^B(t_s)+p_a^M(λ)|| with λ ∊ [0, 1] contains, in the worst case, a maximum and a minimum (apart from the extremes of the interval of the search). If the mentioned condition is not verified, then any involved arc-like motions have to be divided before running any algorithm.

The support function h_M has to be updated to:

$$\begin{array}{l}{h}_M(\text{η,μ })=\underset{\forall \text{i,j}}{\max }\left\{\left(c_A-c_B+p_{\mathrm{i}\mathrm{j}}^M(\text{μ })\right)\cdot \text{\hspace{0.05em}η\hspace{0.05em}}+(r_{\mathrm{i}}^A+r_{\mathrm{j}}^B)\cdot \left|\right|\text{\hspace{0.05em}η|| }\right\}\\ \text{ with μ\hspace{0.05em}}=0,1-r,r,1,{\text{λ}}_c;{\text{ λ}}_c∊[0,1]\end{array}$$ (25)

An example dealing with a mobile robot A and a mobile obstacle B with arc-like motions and with constant angular accelerations is shown in figure 4. Its MTD of separation, d_O^M, and the corresponding instant in time, t_O^M, have been obtained in 9.7 µs in an Intel® Core™ i5 650 processor at 3.2 GHz.

Figure 4.

Distance between A and B with arc-like motions. The motions are described by ω _A (t_s)=50°/s, α _A =5°/s², ω _B (t_s)=−30°/s, a_B=−2.5°/s², t_s=0s and Δt=3s. (a) The A and B positions are stepped at t_s, 1s, 1.5s, 2s and 2.5s. The positions at t_O^M=2s, where A and B are at their MTD of separation, d_O^M, are in red. (b) The rose-like axes of the eight different stadiums in S^M(t) and the distance d_O^M (at a different scale). For clarity, only the S^M(t) external edge close to O, its associated capping circles, its distance, d_O^M, to O, and all the axes, are depicted.

5. Algorithm analysis and experimental results

All the support functions in this paper verify:

$$h_M(\text{η,μ})=h_{S^A(t)}(\text{η,μ })+h_{S^B(t)}(-\text{η,μ})$$ (26)

A's motion is described by n stadiums, while B's motion is defined by m stadiums. For this reason, the support functions can be applied separately to A's motion (i.e., h_S^A_(t)(ρ,µ)) and to B's motion (i.e., h_S^B_(t)(−ρ,µ)).

The condition in (26) is true and is proved for the case of A and B following, respectively, arc-like and linear motions. The proof is entirely similar to the other two cases.

In accordance with the definition of A's motion in (12), then:

$$h_{S^A(t)}(\text{η,μ })=\underset{\forall \mathrm{i}}{\max }\left\{\left(c_A+{\rho }_{\mathrm{i}}^A\left(\cos ({\text{θ}}_{\mathrm{i}}^A(t)),\sin ({\text{θ}}_{\mathrm{i}}^A(t)\right)\right)\text{ η }+r_{\mathrm{i}}^A\left|\right|\text{\hspace{0.05em}η\hspace{0.05em}}\left|\right|\right\}$$ (27)

with ${\text{θ}}_{\mathrm{i}}^A(t)={\text{θ}}_{\mathrm{i}}^A(t_s)+\text{μ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot w_A(t_s)+0.5\cdot {\text{μ}}^2\cdot \mathrm{\Delta }{t}^2\cdot {\alpha }_A$ .

According to the definition of B's motion in (11), then:

$$\begin{array}{l}{h}_{S^B(t)}(-\text{η,μ })=\\ \underset{\forall \mathrm{j}}{\max }\left\{\left(c_{\mathrm{j}}^B(t_s)+\text{\hspace{0.05em}μ\hspace{0.05em}}\cdot \mathrm{\Delta }t\cdot w_B(t_s)+0.5{\text{μ}}^2\mathrm{\Delta }{t}^2{\alpha }_B\right)(-\text{η })+r_{\mathrm{j}}^B\left|\right|\text{\hspace{0.05em}η\hspace{0.05em}}\left|\right|\right\}\end{array}$$ (28)

Adding (27) and (28), the h_M definition given by (22) is reached. p_i ^M (µ) verifies the definition in (20).

The condition in (26) has an important consequence: the Minkowski difference S^M(t) does not need to be computed before running any of the LL-GJK, LL_in–GJK, AL-GJK, AL_in–GJK, AA-GJK or AA_in–GJK algorithms. Therefore, the complexity of all these algorithms is linear, namely O(n+m) instead of O(n×m)

These algorithms have been implemented in C and run in an Intel® Core™ i5 650 processor at 3.2 GHz. S-topes A and B, and their motions, have been randomly generated in order to analyse all of these algorithms. The s-tope orders, n and m, have been fixed in order to consider the following situations: n+m = 10, 50, 100, 250, 500, 1,000, 1,500 and 2,000. Approximately 5,000 different experiments have been run.

It is important to note that when the two involved s-topes do collide, then the corresponding LL_in–GJK, AL_in–GJK and AA_in–GJK algorithms will be run twice, returning two penetration distances. Next, the collision cases have a significant influence on the analysis of the algorithms.

The runtime of the algorithms per computed distance is shown in figure 5. The linear complexity of these algorithms is verified in figure 5. The sub-distance algorithm (the Secant method) requires more time when dealing with arc-like motions and, for this reason, the LL-GJK and LL_in–GJK algorithms present a minor computational cost.

Figure 5.

Computational cost of the algorithms by distance.

The total number of iterations for all the algorithms is convex, as with the original GJK algorithm [16]. Figure 6 shows the average number of iterations per distance. The number of collision cases affects the linearity in the number of iterations.

Figure 6.

Average number of iterations in the algorithms.

The average number of iterations in the Secant Method for finding a minimum is shown in figure 7. The results in figure 7 show that the number of iterations in the Secant-method procedure is also convex.

Figure 7.

Average number of iterations in the Secant method, with the searching accuracy set to 10⁻⁶.

Sometimes, the interval of searching in the Secant method contains a maximum and a minimum; moreover, if the Secant method first finds a maximum, then it is started again in order to search for the desired minimum, increasing the total number of iterations in the procedure. This situation is presented randomly as a consequence of how the data for this analysis has been created. Consequently, the experiments where a maximum is found by the Secant method have not been considered in the analysis shown in figures 5, 6 and 7.

In order to validate and analyse the proposed algorithms, two wheeled mobile robots have been used. These robots are based on the LEGO NXT Mindstorms platform. They are differential vehicles, and so they use independent velocities in both left and right wheels to move in the 2D plane.

The control unit of the Lego robots is based on an ARM7 microcontroller. This 32 bit CPU provides most of the control logic for the robot, including analogue-to-digital converters, timers, Bluetooth and USB communications ports, 256 Kbytes of FLASH memory and 64 Kbytes of static RAM. The actuators of these robots are high quality permanent-magnet dc motors that can provide torques of about 16.7 N.cm working at 117 rpm.

Different development tools can be chosen for programming the NXT microcontroller. In this work, RobotC has been used. It is a powerful programming language based on C that works in a Windows environment. RobotC is a cross-platform language that also allows for the debugging of the robot's applications in real-time.

Using RobotC, a pure-pursuit control algorithm for the path following was implemented. This algorithm follows any kind of path given by a series of points. In this case, linear and arc-like paths were specified.

Assuming that the initial position prior to the movement is known, the actual robot position can be estimated by using the local information of the robot motion (the wheels' velocities) obtained from several sensors in order to calculate the distance travelled from the initial point. This procedure has the benefit of a fast response time but also a disadvantage: between two position estimations, an error (between the actual and the estimated positions) is accumulated over time. Due to this, and after some navigation time, the position estimation can be very different from the actual value.

As it is important to have good local position estimation, the several sensors available on the robot - measuring the variables associated with the motion - should be used to increase the measurement accuracy, reduce the measurement noise and correct the deviation of the actual position value. In this case, the problems about how to integrate the different sensors into a single measurement that can be used by the control algorithm - taking into account the different accuracy and noise levels of the sensors or else how to determine which information should be discarded and which should be used to perform control - arise.

One of the most well-known and efficient techniques for data fusion is the Kalman Filter (KF) [23–24].

In this work, a linear KF has been used to obtain the global position of the robot. The implemented technique performs the sensor fusion locally by means of the wheel encoders (to measure the displacement of the left and right wheels), a gyroscope (to obtain the robot's angular velocity), a compass (to measure the heading angle) and two accelerometers placed above each wheel.

The main advantage of the linear KF proposed is its low computational cost. Because it uses small-sized matrices to obtain the Kalman gain, it can be calculated in realtime in the LEGO control unit.

Three different experiments were run. Each LEGO robot, L_A and L_B, has been simply modelled by a circle (a first-order s-tope). L_A and L_B's radii are r^A=110 and r^B=140 mm. Each radius is 25 mm greater than necessary for security.

Two linear motions with constant acceleration are considered in the first experiment (LL). L_A and L_B's motions' parameters are c_A(t_s)=(933,400), v_A(t_s)=(−58,0), a_A=0.42 mm/s², c_B(t_s)=(400,1051.6), v_B(t_s)=(0,–57), a_B=0.95 mm/s² with t_s=0 s and Δt=11 s. L_A and L_B's positions at t_s are given by c_A(t_s) and c_B(t_s), respectively. Figure 8 shows three plots: the centres' abscissa, their ordinates and the distance between them minus L_A and L_B's radii. The t_O^M and d_OM obtained from the algorithm and the experiment differ, respectively, 55 ms and 5.5 mm. As the robots collide, the experimental MTD of penetration was obtained by running one of the robots with a delay. See figure 9. Figure 10 shows the control actions for the motors of robot L_B. Because the robot executes a linear trajectory with constant acceleration, the left and right wheels' control actions increase uniformly with the same slope.

Figure 8.

Experiment LL. The robots' positions and their distances (in millimetres) t_s=0 s. to t_s+Δt=11 s.

Figure 9.

Experiment LL until collision. L_A's motion is delayed two seconds for appreciating the maximum penetration instant.

Figure 10.

Control actions of the L_B robot's motors for the LL experiment.

In the AL experiment, L_A follows a linear motion with c_A(t_s)=(597,400), v_A(t_s)=(−60,0) and a_A=0.42 mm/s², while L_B follows an arc-like motion centred at (400,400) with an arc radius of 400 mm, θ ^B (t_s)=−90°, w_B(t_s)=24.2°/s and α _B =0.79°/s². θ ^B (t_s) holds L_B's initial angular position. The time parameters are t_s=0 s and Δt=5 s. L_A and L_B's positions and their respective distances are shown in figure 11. The t_O^M and d_O^M obtained from the algorithm and the experiment differ, respectively, 98 ms and 3.9 mm. Some snapshots of this experiment are shown in figure 12. Figure 13 shows the control actions of robot L_A for this experiment.

Figure 11.

Experiment AL. The robots' positions and their distances (in millimetres) t_s=0 s. to t_s+Δt=5 s.

Figure 12.

Snapshots from the AL experiment. The robots' positions at: (a) t_s=0 s, (b) 1 s, (c) 1.75 s (minimum separation) (d) 2.5 s, (e) 3.25 s and (f) 4 s

Figure 13.

Control actions of the L_B robot's motors for the AL experiment.

In the AA experiment, robot L_A follows an arc-like motion centred at (1250,200) with an arc radius 200 mm, θ ^A (t_s) = −147.5°, w_A(t_s) = −19.4°/s and α _A =−0.21°/s². Robot L_B follows the arc-like motion shown in the AL experiment. The time parameters are t_s=0 s and Δt=6 s. L_A and L_B's positions and their respective distances are show in figure 14. The t_O^M and d_O^M obtained from the algorithm and the experiment differ, respectively, 75 ms and 0.9 mm. The experiment results are shown in figure 15. Figure 16 shows the control actions for the motors of robot L_B. Because the robot executes a circular (arc-like) trajectory with constant acceleration, the left and right wheels' control actions increase uniformly, but in this case the slopes of each action are different.

Figure 14.

Experiment AA. The robots' positions and their distances (in millimetres) t_s=0 s. to t_s+Δt=6 s.

Figure 15.

Experiment AA. L_A and L_B are shown at t_s=0, 1, 2, 3 (minimum separation), 4, 5 and 6s.

Figure 16.

Control actions of robot L_B's motors for the AA experiment.

A simulation involving five different robots with non-holonomic motions is also shown. Robot R₁ is modelled by a circle and follows a linear motion with c_R1(t_s)=(19.5,45.6), r^R1=7 mm, v_R1(t_s)=(4.4,8.9), ||v_R1(t_s)||=9.95 mm/s and a_R1=0.4 mm/s². Robot R₂ also follows a linear motion, but it is modelled by a fourth-order s-tope whose position S^R2 at t_s is S^R2(t_s)={(169.6,180.3), (176.4,176.5), (181.5,185.1) and (174.6, 189.1)}. The radii of the circles in S^R2(t_s) are, respectively, 2, 2, 3 and 3 mm. R₂'s initial velocity and acceleration are v_R2(t_s)=(−10.3,–17,4), ||v_R2(t_s)||=20.25 mm/s and a_R2=−0.8 mm/s². Robots R₃, R₄ and R₅ follow arc-like motions. R₃'s motion is centred at (1,0) and it is modelled by a second-order s-tope. The two circles' centres are given in polar coordinates as θ₀ ^R3 (t_s)=14.4° where the arc radius ρ₀ ^R3 =170 mm, and θ₁ ^R3 (t_s)=9.3° where ρ₁ ^R3 =170.7 mm. The radii of the two circles are, respectively, 5 and 3 mm. R₃'s initial angular speed and acceleration are w_R3(t_s)=6.1°/s and α _R3 =−0.25°/s². R₄'s motion is centred at (−115,120) and it is modelled by a third-order s-tope. R₄'s position at t_s is given by ∞₀ ^R4 (t_s)=15.5° where ρ₀ ^R4 =240 mm, θ ^1R4(t_s)=18.2° where ρ₁ ^R4 =233.9 mm, and θ₂ ^R4 (t_s)=18.1° where ρ₂ ^R4 =246.8 mm. The radii of the circles are, respectively, 3, 0, and 0 mm. R₄'s initial angular speed and acceleration are w_R4(t_s)=−2.7°/s and α _R4 =−0.1°/s². Finally, R₅'s motion is centred at (210,210) and modelled by a fourth-order stope. R₅'s position at t_s is given by θ₀ ^R5 (t_s)=−169.2° where ρ₀ ^R5 =192 mm, θ₁ ^R5 (t_s)=−173.9° where ρ₁ ^R5 =192.7 mm, θ₂ ^R5 (t_s)=−173.7° where ρ₂ ^R4 =203.6 mm, and θ₃ ^R5 (t_s)=−169.2° where ρ₃R5=203 mm. The radii of the four circles are 0 mm. R₅'s initial angular speed and acceleration are w_R5(t_s)=4°/s, α _R5 =0.15°/s². The time parameters are t_s=0 s and Δt=12 s.

For clarity, the positions of the involved mobile robots have been stepped into two different figures (see figures 17.a and 17.b).

Figure 17.

Simulation with five robots. (a) The robots' positions at t=0, 2, 4, 6, 8, 10 and 12 s are shown. (b) The positions at t=1, 3, 5, 7, 9 and 11 s are shown.

For each pair of robot motions, the future instant in time, t_O^M, when both robots will be at their MTD, d_O^M, has been computed. The results are shown in a matrix. See (29). The upper triangular submatrix contains the obtained d_O^M. The lower triangular submatrix shows the corresponding t_O^M. In this way, for instance, the MTD between robots R₂ and R₄ is 6.63 mm and it is presented at 5.36 s.

$$\begin{array}{r}{R}_1{R}_2{R}_3{R}_4{R}_5\hfill \\ \hfill \begin{array}{c}{R}_1\\ R_2\\ R_3\\ R_4\\ R_5\end{array}\left[\begin{array}{ccccc}& \text{56.24}& -\text{8.36}& \text{56.81}& -\text{13.15}\\ \text{6.96}& & -\text{14.98}& \text{6.63}& -\text{15.06}\\ \text{10.26}& \text{4.72}& & -\text{11.1}& 59.05\\ \text{7.27}& \text{5.36}& \text{5.93}& & -\text{10.33}\\ \text{5.44}& \text{9.5}& 7.23& 10.88& \end{array}\right]\end{array}$$ (29)

The distances between each pair of robots while they are following their respective motions is shown in figure 18. Observing this figure, robot R₁ collides with R₃ and R₅; R₂ also collides with R₃ and R₅; R₃ collides with R₄; and R₄ collides with R₅ (see the negative distances in figure 18).

Figure 18.

Distances between each pair of robots for t ∊ [0, 12].

The evolution of the linear speed (in mm/s) of robots R₁ and R₂, and the angular speed (degree per second) of R₃, R₄ and R₅ is shown in figure 19.

Figure 19.

Speed evolution of the robots in motion for t ∊ [0, 12].

6. Discussion

Our proposed collision detection technique is compared with some representative continuous collision detection (CCD) techniques.

Comparing our algorithms with the reciprocal volume object (RVO) in [11], the RVO is a robust collision-avoidance technique based on the VO concept [12]. VO contains the set of all the velocities of a robot that will result in a collision with another robot. Determining VO implies the computation of the Minkowski difference of the involved objects. Our technique does not compute the Minkowski difference, otherwise its complexity would be O(n×m) instead of O(n+m). The RVO considers neither explicitly non-holonomic motions nor the current agents' accelerations. RVO has been applied to thousands of disc-shaped agents, while our technique is suitable for robots modelled by convex-hulls defined by thousands of spheres (circles).

The work in [9] is also a CCD technique. The objects are modelled by swept sphere volumes and follow translational and rotational motions with constant velocities. The method returns the first time of contact if the objects collide. If not, the minimum separation is calculated. The method requires a separation distance computation function and a motion bound calculation. The method assumes that one of the objects is fixed (without motion) and computes a lower time bound. The mobile object is advanced according to the mentioned lower bound. This method does not compute penetration distances and does not consider trajectories with non-null accelerations.

The first contact time, the contact positions and the normal contact between two mobile rigid objects which are going to collide are obtained in [5]. The technique relies upon the effective interpolation of interval arithmetic and hierarchies of oriented bounding boxes. This CCD method finds the first time of contact by applying collision tests between object features (vertex, edge and face). The features are in motion and, iteratively, the time interval is reduced until the discovery of the instant in time when they are in contact. This technique does not consider non-null acceleration motions and is not intended to find the minimum separation when the objects do not collide. The method applies the same test each time with a minor interval of time.

7. Conclusions

This paper has given a method for more than detecting a collision between two mobile robots or between a mobile robot and a dynamic obstacle without stepping their motions. Specifically, this method obtains the future instant in time when two mobile objects will be at their minimum translational distance of separation or penetration (if collision). The mentioned translational distance and instant in time are computed in parallel. These results have been returned by certain proposed algorithms with linear complexity.

The involved robots and obstacles are modelled by spherically-extended polytopes (convex hulls). Their motions are non-holonomic (linear or arc-like) with constant accelerations. The positions of the robots or obstacles are assumed to be measurable and their motions (path, speed and acceleration) are estimable.

Some simulations and experiments with real robots have been run to conclude that the method is fast, robust, convex in the number of iterations, and accurate.

Additionally, our method is so fast that it can be run as frequently as any new information from the sensor system is received.

A direct extension of this work consists of updating these algorithms to compute, in the case of collision, the first time of contact. Another future and challenging work will be to deal with three-dimensional motions.