Thibaud Duhautbout

Reine Talj

Véronique Cherfaoui

François Aioun

Franck Guillemard

Generic Trajectory Planning Algorithm for Urban Autonomous Driving

ou non, émanant des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

I. INTRODUCTION

Autonomous driving is a complex task and remains a challenge for industrial and academic actors. While more and more advanced assistance systems become available on highways, urban navigation is still highly manual. Urban environment is very complex: the network is dense, the roads can be narrow and have sharp turns, lots of driving rules have to be respected and the space is shared with other users, such as cars, pedestrians, bicycles, etc. To handle this complexity, the autonomous driving system has to be divided in different modules working together, each of which realizing a particular task. This paper describes a generic planning architecture, focuses on a local planning method which uses a path and velocity decomposition, and introduces an algorithmic speed planning method to improve existing path planning approaches.

In Section II, existing works about motion planning are covered. This work focuses entirely on a local trajectory planning algorithm, which has to be included in a larger architecture described in Section III. The proposed method is presented in Section IV and some results are discussed in Section V. Finally, a conclusion and some thoughts about future works are given in Section VI.

II. RELATED WORKS

Motion planning for self-driving cars has been widely covered in the literature, with various techniques and algorithms [START_REF] Katrakazas | Real-time motion planning methods for autonomous on-road driving: State-ofthe-art and future research directions[END_REF]. Methods based on random trees have been used for urban navigation [START_REF] Kuwata | Motion planning for urban driving using RRT[END_REF], using incremental sampling to find a path from the initial position of the vehicle to a goal point. Each sampled configuration is checked for collision with the environment, so that only valid positions are added to the tree. Lattices planners [START_REF] Pivtoraiko | Efficient constrained path planning via search in state lattices[END_REF] make use of a graph structure and efficient search algorithms to find a valid path to a goal point in a given environment. Both of these methods perform a direct search of one final path with static obstacle avoidance and do not account for moving obstacles. In [START_REF] Ziegler | Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios[END_REF], a temporal dimension is added to the lattice to allow the vehicle to adapt its speed to moving obstacles. However, the corresponding speed profile depends strongly on the time-discretization of the lattice.

Recent works try to solve the self-driving problem with an end-to-end machine learning process. In [START_REF] Bojarski | End to end learning for self-driving cars[END_REF], a neural network is trained to steer the vehicle directly from a camera sensor. However, it is hard to prove that such methods always provide appropriate results. To provide interpretable results, [START_REF] Zeng | End-to-end interpretable neural motion planner[END_REF] train a neural network to compute a cost volume from raw sensor data. A set of candidate trajectories is then sampled and evaluated with this cost volume to select the optimal trajectory. Although this provides interesting results, large amounts of labelled data are required to train the networks.

The tentacles method presented in [START_REF] Von Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF] explicitly generates a fixed number of candidate paths and evaluates them against the environment. The best path is then tracked by the controllers for a short time period and the process is restarted. Initially represented by circle arcs, [START_REF] Himmelsbach | Autonomous Off-Road Navigation for MuCAR-3: Improving the Tentacles Approach: Integral Structures for Sensing and Motion[END_REF] enhances the method by constructing paths with clothoid arcs to ensure curvature continuity. The clothoid shape is well-suited to account for vehicle dynamics, but is not consistent with the behaviour of a vehicle on urban roads because most of the curves end up out of the road. In [START_REF] Thrun | Stanley: The robot that won the DARPA Grand Challenge[END_REF], [START_REF] Urmson | Autonomous Driving in Urban Environments: Boss and the Urban Challenge[END_REF] and [START_REF] Chu | Local Path Planning for Off-Road Autonomous Driving With Avoidance of Static Obstacles[END_REF], the paths are constructed as "parallel tentacles" and are defined by a lateral offset to the reference road, usually the center of the current lane. In [START_REF] Mouhagir | Evidential-based approach for trajectory planning with tentacles, for autonomous vehicles[END_REF], moving obstacles are considered by being extended in the underlying occupancy grid according to their heading and speed to provide some anticipation with respect to their expected motion. In these methods, the speed is not explicitly defined along the path.

In [START_REF] Kim | A probabilistic optimization approach for motion planning of autonomous vehicles[END_REF], speed values are defined on each point of the path depending on the curvature of the path, the speed of a preceding vehicle and the applicable speed limit. In [START_REF] Zhang | Hybrid trajectory planning for autonomous driving in highly constrained environments[END_REF], an hybrid trajectory planner for static environment is introduced, which separates the path and speed planning in two distinct tasks. In [START_REF] Lim | Hybrid trajectory planning for autonomous driving in on-road dynamic scenarios[END_REF], the method is improved to consider moving obstacles in highway scenarios. Both of these methods use numerical optimization to compute speed profiles. [START_REF] Artuñedo | Real-time motion planning approach for automated driving in urban environments[END_REF] considers the urban environment by adapting the speed to the predictions of the moving obstacles. In this work, a similar approach is used: the "parallel tentacles" path-planning algorithm is used to sample candidate paths on the road, and a discrete adaptation method is introduced to compute a safe and comfortable speed profile on each

Environment

Sensors

Perception

(sensor data fusion)

Road network (HD map) Destination

Global planning

(Route planning in the road network)

Decision

(high-level semantic analysis)

Local planning

(generation of a local trajectory)

Control

Actuators Vehicle

Tactical components

Fig. 1: Global architecture path, by taking into account static and dynamic obstacles, and visibility limits. This algorithm is generic and adapted to different urban situations.

III. PLANNING ARCHITECTURE

As explained in the introduction, this work focuses on a local planning algorithm. However, it is clear that local planning alone can't provide all necessary functions for an autonomous vehicle. Figure 1 presents the complete architecture considered in this article, in which local planning is integrated. The dashed lines materialize the limits of the tactical components: global planning, decision and local planning. The aim of each of these components and the assumptions made are detailed in the following. Sensor data acquisition and processing, as well as vehicle control, are not covered in this paper. It is assumed that perception modules provide the data required for the tactical layer, and that efficient and predictive controllers are available to track the generated trajectories.

The global planning module is used to define the route from the starting point to the destination. The output of this module is called the mission path and contains the geometrical description of the route. If an HD map is available, the mission path can be enhanced with a priori information about the lane to use, the maximum allowed speed, expected signalization and any other useful information.

The decision module is the high-level reasoning module of this architecture. Its role is to define the decision path, representing the local path to follow to achieve the mission path. This path is an ordered set of N D points defined by Equation 1. Each point P d is defined in a global 2D reference frame with coordinates x and y, heading θ and curvature κ, and contains a speed value v dec representing the maximum speed allowed, determined from the analysis of the situation (speed limit, stop signs, traffic lights, priorities...).

D = P d = x, y, θ, κ, v dec , d ∈ [1, N D ] (1)
The decision module also defines a lateral displacement interval Q lat = [q min , q max ] which defines the drivable space around D and allows or forbids the vehicle to overtake. Finally, the local planning module is responsible for the generation of a reference trajectory which will be tracked by the control system. This trajectory is a timely discretized given by the decision, avoid static and moving obstacles, ensure dynamical feasibility and safety and maximize lateral and longitudinal comfort for the passengers.

The three planning modules do not function at the same rate. The global planning module can run at a very low frequency, or on-demand when the initial mission path can't be followed. The decision module should run at a moderate frequency to provide responsiveness to complex situations while keeping consistent and stable constraints for the local planning, which should run at a high frequency to adapt the reference trajectory to fast changes in the environment. In this paper, we detail the local planning module.

IV. PATH AND SPEED PLANNING ON TENTACLES

Method overview

The proposed planning algorithm is derived from the tentacles method. Figure 2 illustrates the different steps of the proposed method, each of which being described in the next sections. As in [START_REF] Zhang | Hybrid trajectory planning for autonomous driving in highly constrained environments[END_REF] and [START_REF] Lim | Hybrid trajectory planning for autonomous driving in on-road dynamic scenarios[END_REF], path and speed planning are separated. A set of local candidate paths is generated from the trajectory constraints and the vehicle state. On each generated path, a speed profile is defined and adapted with respect to the environment to produce feasible and safe candidate trajectories. Defining a speed profile is key to provide anticipation and comfort for the passengers. Finally, the best trajectory is selected and provided to the controllers. All this process is executed at regular time steps to integrate new perception data.

A. Paths generation

As proposed in [START_REF] Chu | Local Path Planning for Off-Road Autonomous Driving With Avoidance of Static Obstacles[END_REF], candidate paths are composed of a transition part followed by a part parallel to D, which seems close to a human behaviour.

The final points of the transition parts are defined by lateral offsets q i from D and longitudinal offsets l j from X 0 , as in [START_REF] Li | A unified approach to local trajectory planning and control for autonomous driving along a reference path[END_REF]. The lateral offsets are used to define multiple alternatives to avoid eventual obstacles, and are sampled in the Q lat interval. The longitudinal offsets provide different transition shapes for each lateral offset, to generate more or less aggressive transitions. The l j values can be fixed, or chosen relative to the vehicle speed. Figure 3 illustrates the The transition part is generated from the initial position of the vehicle to one final point as a parametric curve, with x and y being 5 th degree polynomial functions. This representation is chosen because the paths can be computed by a direct linear system resolution. The parameters of each functions are determined to ensure position, heading, and curvature continuity at both extremities of the transition part.

• D • • • • • • • • • • • • • • • | | s q l1 l2 l3 lmax qmin = q1 q2 q3 q4 qmax = q5
Once the transition part of a path is generated, it is extended by a part parallel to D to reach a final length l max . This value should be at least the same as the comfortable stop distance of the vehicle to provide enough anticipation. The transition and the parallel parts are discretized using a fixed number of points for each. Finally, a set of K paths {P k } k∈[1,K] is computed. Equation 2 defines the path P k composed of N P k successive 2D points in a global reference frame. Each point holds speed v and acceleration a components which will be used in the adaptation steps.

P k = P i = (x, y, θ, κ, v, a, s) , i ∈ 1, N P k (2)

B. Paths adaptation

The paths adaptation steps (see Fig. 2) define a speed profile on each candidate path P k to ensure safety and compliance with the given constraints. In these steps, the spatial components of P k are fixed, and only the speed profile is adapted to satisfy the different constraints.

1) Speed constraints: The first step is to integrate speed constraints on the path. First, each point P i of P k is projected on D to retrieve the maximum speed v dec i . The speed is also adapted to the curvature of the path to maintain lateral comfort. Lateral acceleration a lat is linked to v and κ by a lat = κ • v 2 . Therefore, given a lateral acceleration bound

a comf lat > 0, one can compute v comf i = a comf lat • |κ i | -1 .
Then, the speed value of P i is set to

v i = min(v dec i , v comf i ) .
2) Static obstacles and visibility limits: The second step is to define an eventual stop point linked to static obstacles and visibility limits. Figure 4 presents the algorithm used to adapt the speed on P k . Static obstacles, such as curbs or stopped cars, will not move and then require the ego-vehicle to fully stop to avoid a collision on the fixed path. For each point of the path, the algorithm checks for overlaps between the polygon of the ego-vehicle and the polygon representing the obstacles, given by the perception. The advantages of this geometric method are to be generic over the shape and the position of the obstacles, and to consider the full shape of the car. The contact point C is the last position without collision on P k . If the ego-vehicle stops there before an obstacle, it will not likely be able to maneuver due to non-holonomic constraints. To prevent this, a longitudinal safety distance s saf e is introduced to move back the stop point. Visibility limits are treated as static obstacles, but without safety distance to ensure security while limiting conservatism when driving in occluded turns. The stop point is then inserted on P k with v i = 0.

3) Moving obstacles: The third step of the adaptation concerns dynamic obstacles. The process for a path P k is described on Figure 5. To predict the trajectory of the egovehicle, the speed profile on P k needs to be smoothed, because no dynamic consideration about acceleration has been introduced yet. The smoothing algorithm is presented in the next section. From the smoothed speed profile on P k , a time-based trajectory T k with a sample time δ t can be computed, as shown on Equation 3.

T k = P t = (x, y, θ, κ, v, a) , t ∈ 1, N T k (3)
The value of δ t must match the sample time of the predictions of moving obstacles. Here, collisions are checked against the predicted state of moving obstacles given by the perception module. If a collision is detected at P t , the point P t-1 (which is collision-free) is set as stop point. Then, a new verification is required because the modification of the speed profile induces a new behaviour of the vehicle, which may collide with other moving obstacles. Figure 6 illustrates this iterative process with an overtaking example.

To ensure a safety distance between the ego-vehicle and moving obstacles, the shape of each actor used for the inter-

Iteration 1 ego-vehicle moving obstacle static obstacle • t1 • t2 • t3 • t4 • t5 • t6 • t1 • t2 • t3 • t4 • t5 • t6 Iteration 2 • t1 • t2 • t3 • t4 • t5 • t6 • t7 • t1 • t2 • t3 • t4 • t5 • t6 • t7 stop point stop point
. . . Fig. 6: Illustration of the adaptation of the speed profile with a moving obstacle. The ego-vehicle plans to overtake a static obstacle and a vehicle comes on the opposite lane. At iteration 1, a collision is detected at time t 6 , the point at t 5 is then marked as stop point. At iteration 2, the vehicle slows down to stop at the right position but is still in collision with the moving obstacle at time t 7 .

section tests is extended to a time period dt corresponding to the desired inter-vehicular time. To determine that there is no collision at time t, there must be no collision on the predicted evolution from time t to t+dt. This brings some anticipation in the planning process and induces safety distances when crossing or following other vehicles. Contrary to the second step of adaptation (IV-B.2), no dichotomy is performed here to precisely estimate the stop point, in order to save processing time. This speed adaptation process can be seen as conservative. Indeed, the global policy induced is to stop and pass after all other vehicles. This policy is chosen to ensure safety and to plan solutions in the worst case. However, the reactive scheme allows to adjust the plan according to the evolution of the environment.

4) Speed smoothing: On a path, the speed profile is represented as a spatial profile, because the constraints we conider are linked to a certain position. Each point is defined by its curvilinear abscissa s i and holds speed v i and longitudinal acceleration a i values. It is assumed that values of v 0 and a 0 are set from the current state of the vehicle. The following smoothing algorithm is used to create a speed profile which integrates a target longitudinal behaviour while ensuring the respect of the speed constraints set during the adaptation steps. It is based on three temporal profiles: comfortable acceleration, comfortable deceleration and maximal deceleration. Each profile is defined by a fixed acceleration and a variation rate. These profiles are illustrated on Figure 7.

The comfort acceleration profile is used when the car needs to accelerate. When the car needs to slow down without stopping, the comfort deceleration profile is used, but if the car needs to stop, the algorithm is allowed to find an optimal deceleration profile between the comfort and the maximal ones to ensure that the stop will occur at the right position. This optimization trades comfort for safety: in this case, in order to respect the stop point and avoid a potential collision, comfort requirements may be discarded.

The speed profile is smoothed in a two-pass iterative way. The first pass smoothes the accelerations: for each point (s i , v i , a i ) of the path, the maximum reachable acceleration

v i+1 ← min(v i+1 , v max i+1 )
, and a i+1 is updated the same way. The second pass smoothes the decelerations, which are treated the same way as accelerations when the path is processed backwards. This algorithm makes it possible to define smooth speed and acceleration profiles and to anticipate the decelerations before a stop or a sharp curve.

The smoothing algorithm also considers the initial speed of the vehicle to ensure the generation of a coherent profile. Due to that, in some cases, speed constraints may be impossible to meet, and the resulting profile may present higher speeds than requested. This usually happens when processing a path with a sharp lateral transition, which creates high lateral acceleration values. To avoid consequent dangerous maneuvers, the selection step performs a check to filter these cases. Also, if a stop is requested too close from the initial position of the vehicle, the maximal braking speed profile will be computed but the stop constraint might not be met.

C. Trajectory selection

When all the candidate trajectories have been adapted, the algorithm needs to decide which one will be sent to the control systems. The first step is to remove the trajectories that could threaten the stability of the vehicle. The lateral acceleration is computed on each candidate trajectory, and if it exceeds a security bound a sec lat , the trajectory is discarded. In the case where all trajectories are discarded, the filter is considered irrelevant and all trajectories are kept for the rest of the selection.

The trajectories are then divided into 4 groups: 1) trajectories without collision; 2) trajectories without collision but which stop too close to static obstacles; 3) trajectories colliding with a static obstacle; 4) trajectories colliding with a moving obstacle. Only the trajectories of the first non-empty group are kept and all others are discarded. This gives a first layer of selection based on a safety principle.

Then, a cost function C is defined as:

C(T k ) = C k + δ k,1 • σ 1 + δ k,2 • σ 2 (4)
where the cost C k is given by:

C k = δ t × N T k t=1 a lon,t 2 + a lat,t
and the two decision variables δ k,1 and δ k,2 are defined by: 4, C k represents the integral of the squared total acceleration of the trajectory T k , with a lon,t and a lat,t representing the longitudinal and lateral acceleration. σ 1 and σ 2 are high costs defined such that ∀k, σ 2 > σ 1 > C k . Those costs are linked to the decision variables δ k,1 and δ k,2 . δ k,1 activates the cost σ 1 to penalize stopping trajectories, and δ k,2 activates the cost σ 2 to penalize non-comfortable trajectories. Finally, the trajectory with the lowest cost value is selected and provided to the control modules.

δ k,1 = 1 if T k leads to a stop, 0 otherwise δ k,2 = 1 if ∃t ∈ [1, N T k ], |a lat,t | > a comf lat , 0 otherwise In Equation

D. Processing time optimization

All candidate paths are not systematically generated. To improve processing time, especially in easy situations, a first set of paths without lateral offset (i.e., with q i = 0) is generated. Those paths are then processed by the adaptation steps previously described. If at least one resulting path does not lead to a full stop, the algorithm directly goes to the selection step. If all paths lead to a full stop, it means that the decision path can't be followed and that a lateral displacement might be required. A second set of paths with lateral offsets around D is then generated, adapted, merged with the first one and provided to the selection function.

V. RESULTS

Table I summarizes the theoretical differences between the proposed algorithm and the closest works of Section II.

Simulations have been conducted to study the results of the presented algorithm in different situations. The algorithm is implemented on MATLAB-Simulink R with code generation and tested on a laptop with an Intel R i7 processor running at 2.70GHz. The environment is manually defined, and is perfectly known by the vehicle. The global trajectory of the moving obstacles is also fully defined, and the exact future motion is used for the predictions. No sensors were simulated and no uncertainties were introduced in the scenarios.

The local planning algorithm is executed every 200ms and the reference trajectory is perfectly followed. For path generation, l j values are taken in {5, 10, 15, 20, 25, 30}, and l max = 80m to cover the visibility range of the vehicle. 11 q i values with a 0.5m lateral interval are used to cover the road, leading to a total of 66 paths. The paths are discretized with N P k = 100 points, and the trajectories with N T k = 51 points with δ t = 0.1s. Speed and acceleration constraints used are described in Table II.

Results are presented on 3 different environments: 1) a classic crossing with occulting obstacles, 2) a two-lanes roundabout and 3) a straight road with a static obstacle on the ego-vehicle's lane. In the following figures, better viewed on the PDF version of the paper, the blue lines represent the center of the different lanes of the network. A simple implementation of the decision uses a local portion of the mission path given by the scenario to compute D. The gray areas represent the static obstacles and the outside of the road, and the red squares represent obstacles limiting the visibility of the vehicle. The trajectory followed by the vehicle is displayed with a color ramp indicating the speed. The first environment presents the behaviour of the vehicle without moving obstacles and with occlusions in the turns. As can be seen on Figure 8, the speed of the vehicle decreases before entering the turn due to the limited visibility and to respect the lateral acceleration bounds.

The second environment shows the results of the algorithm with curved roads with another vehicle (displayed in light purple). Figure 9 shows that the ego-vehicle slows down before entering the roundabout to let the other vehicle pass before. The dots on the trajectory show the position of each actor at the indicated time. When the way is clear, the vehicle reaches the maximum speed which respects the comfort bound.

The last environment presents a complex case where the ego-vehicle's lane is blocked by an obstacle, and two other vehicles come on the opposite lane. On Figure 10, the egovehicle starts an overtaking trajectory but then detects the other vehicles and comes back in its lane to stop before the static obstacle, and waits until the left lane is free to perform the overtaking. III summarizes the processing time of the local planning step for the presented results. The scenarios with the highest processing times correspond to scenarios with moving obstacles, which are complex cases and thus take more time to process. The highest processing time obtained for the presented results is lower than the replanning time (200ms). Generally, the median processing time seems efficient enough to use the algorithm in embedded conditions.

VI. CONCLUSIONS AND FUTURE WORKS

In this paper, a new trajectory planning algorithm is presented. Based on the tentacle algorithm, it introduces explicit speed planning in urban environment by finding stop points with respect to static and dynamic obstacles, and by computing smoothed speed profiles on each candidate path. Simulations are carried out to analyze the behaviour of the algorithm and show encouraging results.

Future works will focus on studying the behaviour of the vehicle in a realistic closed-loop simulation, in challenging situations with more moving obstacles. Uncertainties, occlu-

Fig. 2 :

 2 Fig.2: Architecture of the proposed local planning method representation of the path and speed profile to follow. The local planning tries to respect the lateral and speed constraints given by the decision, avoid static and moving obstacles, ensure dynamical feasibility and safety and maximize lateral and longitudinal comfort for the passengers.The three planning modules do not function at the same rate. The global planning module can run at a very low frequency, or on-demand when the initial mission path can't be followed. The decision module should run at a moderate frequency to provide responsiveness to complex situations while keeping consistent and stable constraints for the local planning, which should run at a high frequency to adapt the reference trajectory to fast changes in the environment. In this paper, we detail the local planning module.

Fig. 3 :

 3 Fig. 3: Example of 15 generated paths around D result of the path generation process with 5 lateral and 3 longitudinal offsets.The transition part is generated from the initial position of the vehicle to one final point as a parametric curve, with x and y being 5 th degree polynomial functions. This representation is chosen because the paths can be computed by a direct linear system resolution. The parameters of each functions are determined to ensure position, heading, and curvature continuity at both extremities of the transition part.Once the transition part of a path is generated, it is extended by a part parallel to D to reach a final length l max . This value should be at least the same as the comfortable stop distance of the vehicle to provide enough anticipation. The transition and the parallel parts are discretized using a fixed number of points for each. Finally, a set of K paths {P k } k∈[1,K] is computed. Equation 2 defines the path P k composed of N P k successive 2D points in a global reference frame. Each point holds speed v and acceleration a components which will be used in the adaptation steps.

• i ← 0 Fig. 4 : 1 projectFig. 5 :

 0415 Fig. 4: Static obstacles and visibility adaptation process for P k . • is the entry point of the algorithm, and • the endpoint. • smooth Pk compute Tk the time-based trajectory on Pk is maximum braking reached ?

Fig. 7 :

 7 Fig. 7: Illustration of the different acceleration/deceleration profiles used a max i+1 and speed v max i+1 at the next position s i+1 are computed from the appropriate profile. The next speed is then updatedv i+1 ← min(v i+1 , v max i+1 ), and a i+1 is updated the same way. The second pass smoothes the decelerations, which are treated the same way as accelerations when the path is processed backwards. This algorithm makes it possible to define smooth speed and acceleration profiles and to anticipate the decelerations before a stop or a sharp curve.The smoothing algorithm also considers the initial speed of the vehicle to ensure the generation of a coherent profile. Due to that, in some cases, speed constraints may be impossible to meet, and the resulting profile may present higher speeds than requested. This usually happens when processing a path with a sharp lateral transition, which creates high lateral acceleration values. To avoid consequent dangerous maneuvers, the selection step performs a check to filter these cases. Also, if a stop is requested too close from the initial position of the vehicle, the maximal braking speed profile will be computed but the stop constraint might not be met.

Fig. 8 :Fig. 9 :

 89 Fig. 8: Right turn with low visibility

Fig. 10 :

 10 Fig. 10: Overtaking with moving obstacles on the left lane TableIIIsummarizes the processing time of the local planning step for the presented results. The scenarios with the highest processing times correspond to scenarios with moving obstacles, which are complex cases and thus take more time to process. The highest processing time obtained for the presented results is lower than the replanning time (200ms). Generally, the median processing time seems efficient enough to use the algorithm in embedded conditions.

TABLE I :

 I Comparison summary of the method

	10
		26s	26s	24s	24s	22s	22s	20s	20s	18s	18s
	5
	0	14s	16s	18s	20s 22s 24s	26s	28s	30s
		90	100	110	120	130	140	150	160	170	180
			14	16	18	20	22	24	26	28	30
							Time (s)

TABLE II :

 II Speed and acceleration arameters constraints used

	Scenario	min time (ms)	median time (ms)	max time (ms)
	1 (fig. 8)	2.6	3.6	6.5
	2 (fig. 9)	3.9	7.2	51.7
	3 (fig. 10)	3.1	4.9	76.6

TABLE III :

 III Processing time distribution for the presented simulations sions, and tracking errors will be introduced to study the strengths and limits of this algorithm.