Distance Computation Between Non-Holonomic Motions with Constant Accelerations

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.


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 collisiondetection 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 collisiondetection 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 [ts,ts+Δt]; si A (t)=(ci A (t),ri A ); sj B (t)=(cj B (t),rj M ): any circle in S A (t), S B (t); S A (ts), S B (ts): the start positions of the motions S A (t), S B (t); S M (t): the Minkowski difference set between two motions; sij M (t)=(cij M (t),rij M ): any circle or element in set S M (t); p M (),pi M () or pij M () for [0,1]: the trajectory followed by a centre cij M (t) for t [ts,ts+Δt]; O: the origin point; O  , Oc, p M (c), pi M (c) or pij M (c): the point in the trajectory p M (), pi M () or pij 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 Θ={p0,p1,…,pn−1}, pi3 i, contains infinite points p that verify: 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 ={ai}, P B ={bj} with ai,,bj 3 , 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 ={ai-bj: aiP A , bjP B , i,j}.
In the GJK algorithm, set Vk always contains from one to four points of set P A-B .Initially, some points from P A-B are randomly assigned to Vk. Next, an iterative process starts.
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 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].

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 spherescircles if bi-dimensional -S={s0,s1,…,sn−1} with si=(ci,ri), where ci is the centre and ri is the radius.An s-tope contains an infinite set of swept spheres s (or circles) expressed by: If the radii ri 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 ts are given respectively by the sets of circles S A (ts)= {s0 A (ts),s1 The GJK-based algorithm in [19] All the axes of the stadiums have the same length ||p M (1)||=t||vAvB||.Figure 1 shows the Minkowski difference S M (t) with 42 stadiums from two constantspeed and linear motions.
The GJK-based algorithm in [19] 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.c10 M (ts), c31 M (ts) are the start points of the two stadiums that hold the external edges of S M (t).
The support hM and mapping sM functions determine the closest stadium in S M (t) in the direction given by O  : where sM(O  ) contains the start point of the stadium that generates the maximum in hM(O  ).
The complexity of the algorithm in [19] is linear since This GJK-based algorithm needs less than four iterations to find the instant in time, tO M , when A and B will be at their MTD, called 'dO M '. tO M and dO M are obtained from the distance between O to the closer external edge of S M (t) [19] (see figure 1), and mathematically by: Note that rab M is finally the radius of the closest external stadium to O (rab M is r10 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, dO M , between the inner O and the external edge is reformulated as , where O  is the projection of O onto the axis of the stadium in Vk that is the closest to O. dO M is converted into a negative number and the radius has to be added, since it represents a translational distance of penetration [13].tO M is computed as indicated by (9).

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 (nonholonomic) 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 arclike motions (AA).

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 ts are given by S A (ts) and S B (ts) respectively.S A (ts) and S B (ts) are defined by the set of circles S A (ts)={s0 A (ts),s1 A (ts),…,sn  1 A (ts)} and S B (ts)={s0 B (ts),s1 B (ts),…,sm  1 B (ts)} where ci A (ts), cj B (ts) 2 and ri A , rj B  are the centres and radii of the circles si A (ts) and sj B (ts), i,j, respectively.
Motions are considered from ts to ts+t, where t is a time horizon.
When A and B are following linear motions, their initial velocities -i.e., at ts -are vA(ts) and vB(ts) 2 , respectively, and their constant accelerations are aA, aB.
If A and B are following arc-like motions centred at cA and cB 2 , respectively, then each ci A (ts), cj B (ts), 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 (ts), j B (ts), as: A and B's angular speed at ts are, respectively, A(ts) and B(ts).Their constant angular accelerations are A and B, respectively.

Each one of the infinite intermediate positions of
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[ts,ts+Δ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 ci A (ts)cj B (ts), a radius rij M =ri A +rj B and an axis p M () 2 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) ).

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, tO M , when A and B will be at their MTD of separation, dO M , or it returns a failure if A and B will collide at t(ts,ts+t].In case of a collision, the LLin-GJK algorithm is then run and returns the future instant in time tO M when A and B will be at their MTD of penetration, dO M .The Minkowski difference between these two motions is S M (t), t [ts,ts+Δt], and it has nm stadiums whose axes, p M (), are parabolic with (see (6)): Any GJK-based algorithm to design requires: a subdistance algorithm, the support and mapping functions, and a final condition.
The sub-distance algorithm computes the distance, dO, between O and the axis of the stadium whose start point is in Vk.Let us consider the stadium with a start point ca A (ts)cb B (ts), then such a distance dO is determined by finding the parameter c that verifies: c is found by applying the root-finding technique, termed the 'Secant method' [21].Experimentally, λ0=0.45 and λ1=0.55 have been confirmed as good choices as initial values for the Secant method.The searching accuracy has been set to 10 -6 .
After finding c, Oc 2 (the axis's closest point to O) and dO are obtained as: Set Vk contains as maximum of two start points (stadiums).Only when the LL-GJK algorithm is being run and the set Vk 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 Vk.It the result of this test is false, the subdistance algorithm returns the distance, dO, from O to the closest stadium in Vk together with the corresponding parameters c, Oc.
When the LLin-GJK algorithm is being run, the subdistance algorithm returns the distance from O to the furthest stadium dO, if more than one in Vk and c, Oc.
In any case, the sub-distance algorithm rejects and removes from Vk the stadium whose distance to O has not been returned.
The support hM and mapping sM functions are used for finding the furthest stadium in a given direction  2 .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, cij M (ts).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 ( 5 1) / 2   [21].
Let ca A (ts)cb B (ts) be the start point of a stadium with a radius ra A +rb B and axis p M () with [0,1], the support function hM is applied to the points in the axis: Then, the support function hM is defined as: The parameter c to be used in (17) is that returned by the last execution of the sub-distance algorithm.
The mapping function sM(,) contains the start point of the stadium that generates the maximum in hM(,).Consequently, according to , sM(,) represents up to five different stadiums.The stadium with the minimum distance to O is selected and called sM() in the LL-GJK algorithm.Conversely, sM() contains the stadium with the maximum distance to O for the LLin-GJK algorithm.In any case, the other stadiums are rejected.
These new hM and sM functions are an improvement of those introduced in [15].
If the LL-GJK algorithm is being run, then  is substituted by −Oc in (17).Conversely, if the LLin-GJK algorithm is being run, then Oc is used in (17) instead of .
The LL-GJK and LLin-GJK algorithms finish when the stadium in sM() is the same as that represented by sM(,c) -i.e., sM()=sM(,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 sM(−Oc)=sM(−Oc,c) and gM(−Oc,c)=0 are true, with: Given that the LLin-GJK algorithm deals with O inside the area delimited by the axes from S M (t), it finishes when the conditions sM(Oc)=sM(Oc,c) and ĝM(Oc,c)=0 are true, with: The function radius returns the radius of the stadium represented by sM(,c).The final conditions gM 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 Vk containing two stadiums.
Next, the LLin-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 Vk.
Each execution of the LLin-GJK algorithm returns a parameter c and a negative distance dO M' (computed as indicated in the last paragraph of Section 3).The maximum of these two negative distances holds dO M (i.e., the MTD of penetration between A and B).From the execution that gives value to dO M , the future instant in time tO M is calculated by using the returned c as indicated by (9).
The parameter Oc returned by both the LL-GJK and LLin-GJK algorithms holds a translational vector (i.e., if, for instance, the position of A at tO M is translated as dO M in the direction Oc, then A and B will be in contact at tO M ).
For clarity, the LL-GJK and LLin-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 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(ts,ts+t], then the ALin-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 ALin-GJK algorithms are, respectively, analogous to the LL-GJK and LLin-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 cycloidlike axes.Each of these n axes pi M () 2 i is described by [0,1] as: i A (t) depends upon  -see ( 12) -with t=ts+•Δt.Let us consider a stadium described by ca A (ts)cb B (ts), ra A +rb B and pa M () with (ca A (ts),ra A )S A (ts) and (cb B (ts),rb B )S B (ts).The sub-distance algorithm obtains the desired distance by finding the c that minimizes ||cAcb B (ts)+pa M ()|| by solving: c is then found by applying the Secant method to (21), but this method will work properly if there is one minimum in ||cAcb B (ts)+pa M ()||.Given that the axes of the stadiums are cycloid-like, if A's angular displacement is lower than , then ||cAcb B (ts)+pa 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 ALin-GJK algorithms.
The support function hM is also required to be modified as: 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.dO M and tO M have been obtained in 10.2 s in an Intel® Core™ i5 650 processor at 3.2 GHz.

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(ts,ts+t] -then the AAin-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 pij M () 2 i,j.These axes are parameterized by [0,1] as: where i A (t) and j B (t) are described by (12).
Let ca A (ts)cb B (ts), ra A +rb B and pab M () be a stadium in S M (t) with (ca A (ts),ra A )S A (ts) and (cb B (ts),rb B )S B (ts).The subdistance algorithm computes the distance from O to the mentioned stadium by finding c that minimizes ||cAcB+pab M ()||, i.e., by applying the Secant method to: If A and B's angular displacements are lower than , then ||cAcb B (ts)+pa 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 hM has to be updated to:

Algorithm analysis and experimental results
All the support functions in this paper verify: 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., hS A (t)(,)) and to B's motion (i.e., hS 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: According to the definition of B's motion in (11), then: Adding ( 27) and ( 28), the hM definition given by ( 22) is reached.pi 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, LLin-GJK, AL-GJK, ALin-GJK, AA-GJK or AAin-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 LLin-GJK, ALin-GJK and AAin-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 LLin-GJK algorithms present a minor computational cost.
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.
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 Secantmethod procedure is also convex.
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.
Total number of circles (n+m) from the involved s-topes 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, LA and LB, has been simply modelled by a circle (a first-order s-tope).LA and LB'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).LA and LB's motions' parameters are cA(ts)=(933,400), vA(ts)=(58,0), aA=0.42 mm/s 2 , cB(ts)=(400,1051.6), vB(ts)=(0,57), aB=0.95mm/s 2 with ts=0 s and Δt=11 s.LA and LB's positions at ts are given by cA(ts) and cB(ts), respectively.Figure 8 shows three plots: the centres' abscissa, their ordinates and the distance between them minus LA and LB's radii.The tO M and dO M 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 LB.Because the robot executes a linear trajectory with constant acceleration, the left and right wheels' control actions increase uniformly with the same slope.
The distances between each pair of robots while they are following their respective motions is shown in figure 18. Observing this figure, robot R1 collides with R3 and R5; R2 also collides with R3 and R5; R3 collides with R4; and R4 collides with R5 (see the negative distances in figure 18).
The evolution of the linear speed (in mm/s) of robots R1 and R2, and the angular speed (degree per second) of R3, R4 and R5 is shown in figure 19.

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.

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.

Figure 2 .
Figure 2. The distance between two mobile objects A and B following linear motions, with ||vA(ts)||=2.2m/s, aA=1 m/s 2 , ||vB(ts)||=3 m/s, aB=−0.5 m/s 2 , ts=0s and Δt=5s.(a) The A and B positions are only depicted at ts, 0.5s, 1.25s, 2s, 2.75s and 3.5s.The positions at tO M =2.75swhere A and B are at their MTD of penetration, dO 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, dO M , to O, and all the axes, are depicted.

Figure 3 .
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(ts)=−18/s, A=−0.5/s 2 , ||vB(ts)||=3 m/s, aB=1 m/s 2 , ts=0 s and Δt=5 s.(a) The A and B positions are stepped at ts, 0.6s, 1.1s, 1.6s and 2.1s.The positions where A and B are at their MTD of penetration, dO M , are in red with tO 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, dO M , to O, and all the axes, are depicted.
c c h (η,μ) max c c p (μ) η (r r )||η|| with μ 0,1 r,r,1,λ ; λ [0,1] 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, dO M , and the corresponding instant in time, tO M , have been obtained in 9.7 s in an Intel® Core™ i5 650 processor at 3.2 GHz.

Figure 4 .
Figure 4. Distance between A and B with arc-like motions.The motions are described by A(ts)=50/s, A=5/s 2 , B(ts)=−30/s, aB=−2.5/s 2 , ts=0s and Δt=3s.(a) The A and B positions are stepped at ts, 1s, 1.5s, 2s and 2.5s.The positions at tO M =2s, where A and B are at their MTD of separation, dO M , are in red.(b) The rose-like axes of the eight different stadiums in S M (t) and the distance dO M (at a different scale).For clarity, only the S M (t) external edge close to O, its associated capping circles, its distance, dO M , to O, and all the axes, are depicted.

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

Figure 10 .
Figure 10.Control actions of the LB robot's motors for the LL experiment.

Figure 16 .
Figure 16.Control actions of robot LB's motors for the AA experiment.
For clarity, the positions of the involved mobile robots have been stepped into two different figures (see figures 17.a and 17.b).

Figure 19 .
Figure 19.Speed evolution of the robots in motion for t[0,12].
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, sA-B(-O  ), is found by applying the support hA-B and mapping sA-B functions: The first step, called the 'sub-distance algorithm', consists of computing the distance from O to the polytope defined by the points in Vk.This distance is obtained by projecting O onto the mentioned polytope.This projected point is called 'O  '.The points in Vk, which are not required to describe the face, edge or vertex where O  is, are removed from Vk.In particular, after this step, if the set Vk contains four points because O is inside the polytope defined by Vk, 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.
A (ts),…,s A n  1(ts)} and S B (ts)={s0 B (ts),s1 B (ts),…,s B m  1(ts)} where ci A (ts),cj B (ts) 2 and ri A , rj B  are, respectively, the centres and radii i,j.The A and B constant velocities are given by the vectors vA, vB 2 .
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.