Trajectory Planning for Car-Like Robots using Rapidly Exploring Random Trees*

3rd IFAC Symposium on Telematics Applications The International Federation of Automatic Control November 11-13, 2013. Seoul, Korea

Trajectory Planning for Car-Like Robots using Rapidly Exploring Random Trees ? Robin Hess ∗ Florian Kempf ∗∗ Klaus Schilling ∗ ∗

Chair of Computer Science VII Robotics and Telematics University of Wuerzurg, Am Hubland, D-97074 Wuerzburg, Germany; e-mail: {hess,schi}@ informatik.uni-wuerzburg.de ∗∗ Chair of Computer Science VII Robotics and Telematics University of Wuerzurg, Am Hubland, D-97074 Wuerzburg, Germany; e-mail: f [email protected] Abstract: This work presents the RRT-planning algorithm adapted to a mobile car-like robot. Based on an short overview of the RRT-approach, all necessary components, adapted to target vehicle, are shown. A possible state space model, including a valid metric, is presented, as well as a control input selection approach. In order to avoid undesired circular motion, we introduced a circle limitation for the control input selection algorithm. Several simulation results are presented for different environments. We show the limitation and propose a solution implementing this algorithm on real hardware. Finally a conclusion is drawn, summarizing the results and giving an outlook to future work. Keywords: 1. INTRODUCTION

and local obstacle information) maximizing a cost function (considering the goal).

In the field of mobile robotics, collision free locomotion of mobile robots is one of the main problems and has been researched for several decades. The task of collision free motion in know, unknown, and partially unknown environment comprises several research domains, such as path planning, path tracking, obstacle detection, and obstacle avoidance. Algorithms from the mentioned domains can be classified into reactive, deliberative, hybrid, and cognitive approaches. While deliberative algorithms are mainly used for path planning, requiring global knowledge, reactive methods enable robots to react to dynamic changes in the environment and inaccuracies in sensing, using mainly local sensor information. Hybrid approaches combine advantages of reactive and deliberative behavior.

Early deliberative static planning algorithms construct a graph representation of the configuration space, where all nodes and edges lie in the obstacle free subset of the configuration space, by using e.g. voronoi diagrams ´ unlaing and Yap [1985]), visibility graphs Lozano(O’D´ P´erez and Wesley [1979] or cell decomposition (Brooks and Lozano-Perez [1985]). With the help of graph search algorithms, e.g. A*, a collision free path can be found in this graph. Since kinematics and dynamics of the robot are neglected, the path may not be trackable and because of the static characteristics, dynamic changes of the environment are not considered. Sampling based algorithms are a later but still actively researched topic Karaman and Frazzoli [2011b]. Two of the most important approaches within this topic are Probabilistic Road Maps (PRM) (Kavraki et al. [1996] and Rapidly Exploring Random Tree (RRT). PRM algorithms select random configurations of the robots configuration space and connect these with an exiting road map of already selected states, using a local controller, which considers the robots kinematics and dynamics.

Well known online reactive approaches, often used in application, are the Vector Field Histogram (VFH) method (Borenstein and Koren [1991]) and its successor VFH+ (Ulrich and Borenstein [1998]). VFH utilizes local range information to generate a polar histogram, which are used to determine the target direction, by searching for valleys. In order to meet kinematic constraints, VFH+ masks regions in the histogram which are not reachable. Furthermore, VFH+ improves smoothness of generated trajectories and considers robot width explicitly. Another popular reactive approach is the Dynamic Window Approach (DWA) (Fox et al. [1997]) with proven convergence Ogren and Leonard [2002]. This approach selects target velocities of a dynamic window consisting of allowed translatory and rotatory velocities (considering dynamic and kinematic constraints

Hybrid approaches often combine successful parts of existing approaches, VFH* (Ulrich and Borenstein [2000]) e.g. extends VFH+ by look ahead techniques using A* search (Nilsson [1980] algorithm (Ulrich and Borenstein [2000]. D*(Stentz [1995]), D* lite (Koenig and Likhachev [2002]) and Anytime A* extend the static A* search algorithm in order to react to dynamic changes in the graph. The Artificial Potential Field (APF) is an old, but yet elegant hybrid method. Obstacles add a repulsive, targets an attractive potential to an APF (Khatib [1986]), from which the target trajectory is determined using gradient descent

? This work was partially supported by the Europischen Fonds f¨ ur regionale Entwicklung (EFRE) der EU und des Freistaates Bayern.

978-3-902823-56-4/2013 © IFAC

44

10.3182/20131111-3-KR-2043.00018

IFAC TA 2013 November 11-13, 2013. Seoul, Korea

sampling algorithm. The nearest neighbor node qT , related to a given metric of the state space, is selected from the actual tree. The next step is selecting the control input tuple (us , Ts ), which is necessary to transfer qT to qS . Using this control input and the state transition function, a forward simulation over time Ts is performed, checking for collisions with the environment. In case of such a collision at time tN , the simulation is aborted and the last valid state qN is added to the RRT. If this new state qN is within the distance tg towards the goal (using the metric of the state space), a valid trajectory is found from q1 to qG . In order to reach the goal, the control inputs, stored at the edges of the tree, have to be applied to the robot step by step, which will lead to a collision free trajectory. Considering a dynamic environment, a replanning has to be performed if the calculated trajectory crosses an obstacle, unknown during the planning phase.

methods. Many extensions to the original approach try to solve inherent problems of APFs, e.g. local minima Juli´a et al. [2008] point mass abstraction and oscillation Xu et al. [2012]. This work is based on the sampling based RRTs especially extended to meet the requirements of car-like robots with non-holonomic constraints. The remainder of the work is organized as follows. First a short conceptional overview of the RRT algorithm is given. Subsequent necessary extensions for car-like robots are discussed, including the used state space model, the control input selection algorithm and the circle limitation. Afterward, problems applying the algorithm to real robots are shown and finally, results are presented and discussed in a conclusion. 2. RRT ALGORITHM In contrast to many other approaches, sampling based methods consider the high dimensional state space of a robot for planning. The RRT was first proposed by Kuffner Jr and LaValle [2000] and LaValle and Kuffner [2001] and has been researched intensively until today. RRTs are designed as single query applications, which are a probabilistic complete solution of the trajectory planning problem. With RRT* an extension has been proposed by Karaman and Frazzoli [2011a], which is not only probabilistic complete, but also asymptotically optimal. Another extension, the Particle RRT (PRRT) has been introduced by Melchior and Simmons [2007], which additionally considers uncertainties.

Even though the base RRT algorithm, used in this work, is very simple, the challenge is finding a good state space representation, a metric for this state space, an appropriate sampling strategy, an algorithm determining the nearest neighbor, the necessary control input, and a good collision detection. While this problem already has been discussed for robots with only moderate constraints, such as space crafts, hover crafts, quatrocopters, the application of RRTs for car-like robots have not been researched intensively so far. Furthermore, many works are satisfied with simulation results, not considering problems, which occur in real world applications. 3. RRT FOR CAR-LIKE ROBOTS

The base RRT algorithm used here works as follows:

The car-like robot, used in this work is the small fourwheel driven robot Outdoor Mobile Experimental Robot for Locomotion and Inteligent Navigation (MERLIN) with ackermann steering (see Eck et al. [2007]).

Algorithm 1: RRT trajectory planning Data: initial state q1 , goal state qG , min. expansion step dTs , max. expansion count nimax , goal threshold tG Result: solution trajectory P or ∅ if no solution in nimax iterations found V ← {q1 }; E ← ∅; TRRT = (V, E); P ← ∅; while ni < nimax do qs ← SampleState (void); qT ← N earest (qs , V ); for i = 1 to k do (usi , Tsi ) ← SelectControl(qs , qT ); qNi ← ExtendW hileV alid (qT , usi , δt, Tsi ); end (qN , uN S , TN ) ← mini=1...k (Distance (qNi , qs )); V ← V S{qN }; E ← E {(qT , qN , uN , TN , cost (qT , qN ))}; if Distance (qN , qG ) ≤ tG then P ← Backtrack(qN , V, E); break; end ni ← ni + 1; end return P;

3.1 State Space Representation and its Metric Since this robot is equipped with differentials on both axles, the kinematic model could be reduced to the model of a single track vehicle. The transfer function is given in eq. 1.       cos θ x˙ 0   sin θ  y˙    0   q˙ =  ˙  = f (q, u) =  tanφ  u1 +   u2 (1) θ 0   l 1 φ˙ 1     u1 vt with u = = u2 vs Hence the state q of the robot with length l consists of the x- and y-position of the robot, the orientation θ and the steering angle φ. The control inputs for this model are given with the translational velocity vt and the angular velocity of the steering angle vs , both constrained by lower and upper limits and a given resolution. The transition function is not linear and contains coupled variables. Therefore an analytic solution of eq. 1 is difficult and is only used for vs = 0. For vs 6= 0 θ and φ are determined using an analytical solution, but x and y are calculated using 3rd-order Runge-Kutta-Integration. In contrast to

Starting from the initial tree, consisting only of the start state q1 , states qS are selected from state space, using an 45

IFAC TA 2013 November 11-13, 2013. Seoul, Korea

the presented and well known kinematic model, the control inputs of the MERLIN robot differ from the ones presented before. Because of the digital servo used for the steering, the desired angle θ has to be set directly rather then angular its velocity. In order to avoid discontinuities in the calculated trajectories, the changed control space is not considered in the state space directly, however a solution to this problem will be shown later. In summary, for this approach the state space χ is identical to the configuration space of the used robot.

a normal distributed random state with the goal as mean is selected with the probability GB ≤ 1 (goal bias) and with the probability of (1 − GB ) a random point of the state space is selected, where selection of all states has the same probability (uniform sampling). Adapting GB , one can set the grade of exploration (uniform sampling) vs. exploitation (goal biased sampling). An example of this trade off is depicted in figure 2, in which a high goal bias can lead to a trap situation, comparable to local minima in APFs.

The metric used is defined as η(qa , qb ) = w1 d(qa , qb ) + w2 r(qa , qb ) + w3 p(qa , qb ) (2) p Where d(qa , qb ) = (xa − xb )2 + (ya − yb )2 is the euclidean norm of the two dimensional positions of the states, p(qa , qb ) = kφa − φb k is the absolute difference of the steering angle and r(qa , qb ) : [0, 2π]2 → [0, π] is a cyclic metric of the robot orientation, defined as: qa,θ − qb,θ qa,θ − qb,θ c) (3) r(qa , qb ) = π 2( − b0.5 + 2π 2π

(a) pSG = 0.05

(b) pSG = 0.5

Fig. 2. RRT with solution path (green) for different pSG in a typical trap situation after 5min of calculation

Figure 1 shows the image of r(qa , qb ) for different robot orientations.

3.3 Control Input Selection and Expansion Once a new state qs is sampled and its nearest neighbor in the existing RRT qT is selected, using the techniques described before, appropriate control inputs have to be found, to transfer qT to a state close or equal to qs w.r.t. the metric defined before. Several approaches may be used here, such as an optimization over the finite, bounded, and discrete space of control inputs; usage of Dubins/ReedsSheeps curves LaValle [2006] or a (k-best) control input sampling. In our implementation we use the last method, the control input sampling. In contrast to Dubins/Reeds-Sheeps curves, this allows for variable velocities and a straight forward integration of the collision check. In order to generate the necessary trajectories, first a set of control inputs us and an expansion time Ts is chosen, by uniformly sampling the control space and an expansion time interval. These inputs are iteratively applied to the solution of the transfer function using a short time interval δt until the time limit Ts is reached. During this process every single state of the trajectory

Fig. 1. image of metric r of the robot orientation θ w1 , w2 , w3 and w4 are weighting factors, which allow the preference of individual properties and may be the subject of parameter tuning. 3.2 Sampling Strategy and Nearest Neighbor

Ziδt qi = f (qT , us )dt

Employing the metric defined before, there are several approaches finding a nearest neighbor in the existing RRT. Using the brute force method, every node has to be considered, which leads to a complexity linear to the number of nodes n in the tree O(n).  The kd-tree  approach 1− 1 has a worst case complexity of O dχ n dχ , where dχ is the dimensionality of the state space χ. Since the number of nodes of RRTs grows fast, the kd-tree method has to be preferred for state spaces with dimension less then 10 and is used for this work.

∀i : iδt <= Ts

0

is checked for collisions with known obstacles, using for example occupancy grids. Once a collision is detected, the iteration for these inputs is aborted. Finally, the last state RT of a collision free trajectory qN = 0 N f (qT , us )dt is added to the tree. Since the probability to come close to the sampled state is quite low using only one control input sampling step, we extended the control input selection to use a k-best sampling strategy. Therefore the algorithm described before, is performed k times and only the trajectory with control inputs UN and expansion time TN ,

The sampling strategy used here, combines uniform sampling with goal biased sampling. For the biased sampling, 46

IFAC TA 2013 November 11-13, 2013. Seoul, Korea

leading closet to the sampled state w.r.t. the defined metric is added to the tree. [ [ V {qN } and E {(qT , qN , uN , TN )}

problem. These trajectories possibly lead to a state quite near to sampled one qs , but the circle or helix formed trajectories are suboptimal w.r.t. traveled distance and time. Therefore we extend the control input selection (see section 3.3)by a circle limitation. In order to avoid circular or helix formed trajectories, we limit the allowed orientation change in one expansion step |∆θ| ≤ ∆θmax by limiting the control space.

3.4 Control space As stated in section 2, the control space of the used model RC =

Using the initial model, shown in 3.1 and control space RC the limitation leads to the inequation cos φ0 ln cos (φ0 +vs Tservo ) Ts − Tservo |vt | + tan (φ0 + vs Tservo ) vs l l

[vtmin , vtmin + δv, . . . , vtmax − δv, vtmax ×

vsmin , vsmin + δvs , . . . , . . . , vsmax − δvs , vsmax ] differs from control space of the real robot RCR = [vmin , vmin + δv, . . . , vmax − δv, vmax ] × φmin , φmin + δφ, . . . , φmax − δφ, φmax ] This is due to the limitations of the used actuators. The digital servo, used as steering actuator, is driven by setting a target angle φs , rather then a rotational velocity vs . As in the model the target angle φs and the target velocity v are bounded, and have a dedicated resolution δφs and δv.

≤ ∆θmax Ts and vs are chosen as free parameters, selected by a first control input sampling step. Afterwards the interval of translational velocities of the control space RC is limited according to the inequality shown before. This approach limits the velocity during sharp turns and limits the corning speed.

The control input sampling described before, uses the state space model defined in section 3.1, and hence assumes control inputs from RC . This inconsistency can be dissolved, by either including a two-stage forward simulation in the control input selection and expansion step or by adapting the control space.

For the adapted model with control space RCR the circle limitation can be implemented in an easier way. Starting from the inequality v t tan (φ)Ts ≤ ∆θmax l

For the two-stage simulation approach, first the transition from the actual steering angle φ(t) to the desired steering angle φs is performed assuming a constant regulation time of the servo Tservo and a translational velocity v = 0, φs − φ(t) ˙ φ(q(t), φs ) = Tservo

and a chosen Ts the control space RRC can be limited to: RCR L = [−vl , −vl + δv, . . . , vl − δv, vl ] × −φl , φl + δφ, . . . , φl − δφ, φl ] with φl =

Leading to a temporary state

and vl =

∆θmax l tan φl Ts

The effect of the limitation is depicted in figure 3. In this simulation the RRT is grown using the kinematic robot model from eq. 1 and consists of about 900 robot states or nodes. Obstacles in the state space are visualized as red polygons and the goal region is located in the top area of figures 3a, 3b. The green path denotes the solution within

 0 0   = q(t + Tservo ) = q(t) +   0 ˙ φ(q(t), φs ) 

qtemp

∆θmax l Ts |vmin |

In the second step, the target velocity v is applied to the temporary state wtemp for the remaining expansion time T − Tservo using a fixed steering angle (vs = 0) leading to qexp . If the remaining expansion time is higher than the minimal expansion step δt the result of the forward simulation is qexp otherwise, qtemp is taken. It is obvious, that for a fast and robust steering Tservo approaches zero and therefore the first step can be omitted. The analysis of the steering servo of the robot MERLIN showed an average regulation time in the order of Tservo = 0.1s. Preliminary experiments showed, that is sufficient to assume an instantaneous change of the steering angle w.r.t. the minimal expansion time δt used in our implementation. Thus, in our real world experiments, we use an adapted state space with RCR as control space.

(a) no limitation

(b) circle limitation with ∆θ = 45 degree

Fig. 3. Effect of circle limitation the tree, where the red line is the determined trajectory of the solution. While the trajectory in 3a shows a lot of circles, the solution with circle limitation (figure 3b) is better w.r.t. path length and smoothness.

3.5 Circle Limitation

3.6 Parameter Optimization

Determining the control inputs using the approach described in section 3.3 could lead to undesired trajectories. Especially closed, or nearly closed circles are a big

The approach presented so far involves different parameters, such as the goal bias rate GB , the number of control input sampling steps k, the expansion time interval 47

IFAC TA 2013 November 11-13, 2013. Seoul, Korea

tuned sets w.r.t. relative path length and time to find a solution. The RRT-planer with automatically tuned parameters generates smother trajectories that approach the goal in less time.

[TSmin , TSmax ] and the maximal allowed change of rotation ∆θmax . Experiments have shown, that the performance of the RRT-planner is very sensitive to these parameters. Therefore we analyzed their influence on different scenarios and tuned them to achieve best results.

The analysis of the number of trails, where no solution has been found within 20s (Nf ail ) shows an increasing value for larger scenes. This can be explained with the high maximum value for the expansion time TSmax . This high value is necessary to quickly explore large areas without penetrating the maximum planning time. On the other hand, this decreased the probability of sampling small values for TS in the control input selection part and therefore makes it hard for the planner to reach a very small goal area.

We selected three different scenarios, trajectory planners are often faced with. Figure 4 depicts these scenes, an urban scene (100mx100m), an office environment (14mx17m) and a natural landscape (100mx100m). The parameters

A solution to this problem, tested in our next version, could be an adaptive expansion time interval, that is changed according to the goal distance. 3.7 Implementation on real hardware

(a) urban

Despite the simulation experiments, the presented approach has been implemented on the mobile robot MERLIN. After a calibration (Lemmer et al. [2010]) the limits and the resolution of the control space RCR have been set to the values of the existing hardware. Figure 5 shows the result for a trail where the final pose of the robot was set to [x, y, θ]T = [3.5m, 0.0m, 180]T . The light red curve depicts the target trajectory, calculated by the RRT-Planner. The final solution consisted of 74 nodes and needed a calculation time of 12s. The real driven path is shown in dark red. Obviously, the real robot was not able to follow the calculated path exactly, using the determined control inputs. The reason for this can be found in inaccuracies caused by the mechanics and the relatively simplistic state space model, used for the first implementation.

(b) office

(c) landscape

Fig. 4. Tested scenes were tuned for each of these scenes independently, using a simulated robot, a genetic algorithm and several performance criteria, such as relative length of the trajectory LT = LLd ( L denotes the absolute path length and Ld is the air-line distance), relative rate of curvature CR ,the final deviation to the goal AS , the duration the robot needs to travel along the path DP and the time needed to find a solution DS . We compared the results of RRT-planner with automatic tuned parameters with hand tuned parameter sets, performing 18 trails for each scene using a simulated robot. The results are shown in table 1 (hand tuned parameter runs are marked with a star).

Fig. 5. Result of experiment with real robot The results show clearly, that a local position controller is very important for real application. For this future extension we propose a 3-stage position correction. The first stage should be applied, if only slight deviations from the planned trajectory are detected. This stage should be implemented as a simple PID controller, taking the calculated control inputs as initial values. If the difference between the planned and the driven path gets to high, the second stage is enabled, performing correction maneuvers based on reed-sheep/dubins curves. The third stage starts a re-planning, which is the most expensive operation and should only be performed if the first two stages fail.

Table 1. urban* urban office* office landscape* landscape

Nf ail 7 7 8 4 8 9

AS 0.43 0.44 0.39 0.42 0.42 0.43

CR 0.23 0.15 0.22 0.13 0.20 0.30

DP 44.92 37.5 16.92 8.9 75.15 99.92

LT 1.57 2.38 3.57 5.6 1.78 2.356

DS 2.75 8.14 2.6 5.48 3.62 2.674

The comparison between hand and automatically tuned parameter sets shows, slightly better results for the hand 48

IFAC TA 2013 November 11-13, 2013. Seoul, Korea

4. CONCLUSION AND FUTURE WORK

Lydia E Kavraki, Petr Svestka, J-C Latombe, and Mark H Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. Robotics and Automation, IEEE Transactions on, 12(4):566–580, 1996. Oussama Khatib. Real-time obstacle avoidance for manipulators and mobile robots. The international journal of robotics research, 5(1):90–98, 1986. Sven Koenig and Maxim Likhachev. Improved fast replanning for robot navigation in unknown terrain. In Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, volume 1, pages 968–975. IEEE, 2002. James J Kuffner Jr and Steven M LaValle. Rrt-connect: An efficient approach to single-query path planning. In Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on, volume 2, pages 995–1001. IEEE, 2000. Steven M LaValle and James J Kuffner. Randomized kinodynamic planning. The International Journal of Robotics Research, 20(5):378–400, 2001. Steven Michael LaValle. Planning algorithms. Cambridge university press, 2006. L´aszl´o Lemmer, Robin Heß, Markus Krauss, and Klaus Schilling. Calibration of a car-like mobile robot with a high-precision positioning system. In Telematic Applications, pages 174–179, 2010. Tom´as Lozano-P´erez and Michael A Wesley. An algorithm for planning collision-free paths among polyhedral obstacles. Communications of the ACM, 22(10):560–570, 1979. Nik A Melchior and Reid Simmons. Particle rrt for path planning with uncertainty. In Robotics and Automation, 2007 IEEE International Conference on, pages 1617– 1624. IEEE, 2007. Nils J Nilsson. Principles of artificial intelligence, volume 1211. Tioga publishing company Palo Alto, CA, 1980. ´ unlaing and Chee K Yap. A retraction method Colm O’D´ for planning the motion of a disc. Journal of Algorithms, 6(1):104–111, 1985. Petter Ogren and Naomi E Leonard. A tractable convergent dynamic window approach to obstacle avoidance. In Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference on, volume 1, pages 595–600. IEEE, 2002. Anthony Stentz. The focussed dˆ* algorithm for realtime replanning. In IJCAI, volume 95, pages 1652–1659, 1995. Iwan Ulrich and Johann Borenstein. Vfh+: Reliable obstacle avoidance for fast mobile robots. In Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on, volume 2, pages 1572–1577. IEEE, 1998. Iwan Ulrich and Johann Borenstein. Vfh*: local obstacle avoidance with look-ahead verification. In Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on, volume 3, pages 2505– 2511. IEEE, 2000. Zhihao Xu, Robin Heß, and Klaus Schilling. Constraints of potential field for obstacle avoidance on car-like mobile robots. In Embedded Systems, Computational Intelligence and Telematics in Control, number 1, pages 169–175, 2012.

In this paper, we presented the well know RRT planning algorithm especially for car-like robots. A possible, albeit simple, state space model has been shown, including a valid metric. Based on this, we described our solution for the control input selection of the RRT algorithm, including a limitation of undesired circular motion. The simulation results showed, that the presented RRT-planner is able to guide a robot towards a goal in different environments. However, the ability of finding a solution within a short time decreases with increasing size of the area. The source of this problem has already been discovered and a solution, an adaption of the expansion time interval has been proposed. The implementation and the experiments with real hardware, showed a discrepancy between the ideal model and the hardware. To solve this problem we proposed a 3-stage position controller/correction, that will be implemented in the next steps. Despite the two discussed extensions, we will adapt the underlying state space model, to fit better to the real hardware. Furthermore, we will extend our implementation to the RRT*-planner, which could generate trajectories close to the optimal solution. ACKNOWLEDGEMENTS The authors appreciated the financial support from the European Union and the State of Bavaria in the EFRE program as well as the joint technical efforts and discussions with all team members of the project Biological Sensor Networks for Environmental Monitoring of Large Areas. REFERENCES Johann Borenstein and Yoram Koren. The vector field histogram-fast obstacle avoidance for mobile robots. Robotics and Automation, IEEE Transactions on, 7(3): 278–288, 1991. Rodney A Brooks and Tomas Lozano-Perez. A subdivision algorithm in configuration space for findpath with rotation. Systems, Man and Cybernetics, IEEE Transactions on, (2):224–233, 1985. D. Eck, M. Stahl, and K. Schilling. The small outdoor rover merlin and its assistance system for teleoperations. In Proceedings of the 6th International Conference on Field and Service Robotics, Chamonix, France, 2007. Dieter Fox, Wolfram Burgard, and Sebastian Thrun. The dynamic window approach to collision avoidance. Robotics & Automation Magazine, IEEE, 4(1):23–33, 1997. Miguel Juli´ a, Arturo Gil, Luis Pay´ a, and Oscar Reinoso. Local minima detection in potential field based cooperative multi-robot exploration. International Journal of Factory Automation, Robotics and Soft Computing, 3, 2008. Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894, 2011a. Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894, 2011b. 49