Realtime motion path generation using subtargets in a rapidly changing environment

Realtime motion path generation using subtargets in a rapidly changing environment

Robotics and Autonomous Systems 55 (2007) 470–479 Realtime motion path generation using subtargets in a rapidly changin...

2MB Sizes 0 Downloads 19 Views

Robotics and Autonomous Systems 55 (2007) 470–479

Realtime motion path generation using subtargets in a rapidly changing environment Dennis Bruijnen ∗ , Jeroen van Helvoort, Ren´e van de Molengraft Control Systems Technology group, Technische Universiteit Eindhoven, Eindhoven, The Netherlands Received 27 October 2005; accepted 24 January 2007 Available online 12 February 2007

Abstract In this work an algorithm is proposed for path planning in a rapidly changing environment. The algorithm is computationally cheap and generates a sub-optimal smooth path with bounds on the allowed velocity, acceleration, and jerk. The algorithm is designed for holonomic omniwheel platforms. It outperforms potential field algorithms regarding both convergence and optimality. Furthermore, it is able to adapt fast in a rapidly changing environment due to the low computational cost in the order of ms for a single update, in contrast with computationally more expensive methods such as wavefront algorithms and global optimization methods, where the computational cost is mostly on the order of seconds. The algorithm will be tested via simulations and experiments. c 2007 Elsevier B.V. All rights reserved.

Keywords: Realtime motion path planning; Changing environment; Collision avoidance

1. Introduction Path generation is an essential item in the field of autonomous robotics. There are a lot of interesting application areas including UAVs (Unmanned Aerial Vehicles) [1–3], autonomous underwater vehicles [4], rescue vehicles, Mars landers, and robot soccer [5,6]. The main challenge for an autonomous robot is to move to a target position as fast as possible without colliding with other objects in a rapidly changing environment. A rapidly changing environment is defined as a real-time environment where objects are moving at similar speeds as the autonomous robot. Moreover, to be sure that the robot is indeed able to track the desired path, the path planning algorithm should at all times comply with the physical limitations of the robot, such as acceleration and velocity bounds and possibly non-holonomic constraints. In a changing environment, offline strategies fall short, since they require a priori knowledge of all environmental variables. For online strategies to be effective, the algorithms ∗ Corresponding address: Mechanical Engineering, Technische Universiteit Eindhoven, Den Dolech 25612 AZ Eindhoven, 5600 MB Eindhoven, N Brabant, The Netherlands. E-mail address: [email protected] (D. Bruijnen).

c 2007 Elsevier B.V. All rights reserved. 0921-8890/$ - see front matter doi:10.1016/j.robot.2007.01.003

need to be computationally cheap and respond fast to changes in the environment. Our application is robot soccer, where computation time is crucial since each robot is equipped with a single laptop that performs all the necessary computations. To obtain a good response time for ball-handling/interception and object avoidance (of other similar moving robots), a frame rate of the vision system of around 30 fps is implemented, resulting in 33 ms of computation budget per frame. Among the large number of computational tasks, including color segmentation, self-localization, object-recognition, strategy, communication, and motion control, the motion path generation may only cost a few ms. In the literature, a huge number of path generation strategies have been proposed, both for offline and online computation. Potential fields [4,6] are suited for our application because they are computationally cheap and are easily implemented; however, they show a lack of convergence in RoboCup soccer games because groups of objects cause local minima in the potential field where the robot gets trapped. Wavefront algorithms [5] return the shortest path to the target for a gridded environment. Although convergence is guaranteed, computation time is on the order of seconds, which is too much for our application. Moreover, a motion path still has to be computed which complies with the physical limitations

D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479


Fig. 1. Some pictures of our tech united robot.

of the robot. Motion path planners that can be added are e.g. [7,8]; however, these also require computation time on the order of s instead of in the order of ms. For replanning algorithms [9,10], sampling-based planning techniques [11,12] or heuristic search methods such as Neural Network/Genetic Algorithm methods [13–15], the same reasoning as for the Wavefront algorithm, holds. These methods have the ability to approximate an optimal global path at the expense of computational costs which are on the order of seconds or higher. Because of these computational costs, the path cannot be updated frequently, which renders such methods unsuitable in a rapidly changing environment. Our new motion path algorithm is computationally cheap and is able to adapt rapidly in a changing environment. Moreover, it takes into account the robots’ physical limitations by imposing a maximum velocity and acceleration on the generated path. Most existing path planning algorithms lack this feature and result often in infeasible motion paths with respect to the robots’ physical limitations. This shortcoming can also be seen in many RoboCup soccer games, where wheel slippage is often observed during motion, implying that the trajectories for the wheels do not comply with the robots’ physical limitations. The algorithm has been designed for a holonomic omniwheel platform. However, the algorithm can be adjusted slightly such that it also works for a differential drive platform, which is a non-holonomic system. First, the application area of this research and the environment are introduced. After that, the procedure to determine a subtarget is explained. Next, a method to generate a smooth path is presented. Then, several extensions to increase

robustness are presented. After that, simulation results both with static and with dynamic/moving obstacles are shown and discussed. Furthermore, experimental results are shown where the path planning algorithm is implemented on a soccer robot and finally, some conclusions are drawn. 2. Application area As a benchmark problem, a rapidly changing environment is chosen, namely, the application of robot soccer. In Fig. 1, some pictures are shown of our team Tech United to get an impression of the application area. In robot soccer, path planning is crucial and, moreover, the changes in the environment are on the same order of magnitude as the movements of the robot itself. The robots can exchange position information and use that for strategy and path planning purposes; however, the robots of the other team can only be observed using sensors, e.g. an omni-vision camera is often used for this purpose. Furthermore, the little a priori information available is a constraint to the size of the field and the maximum size of the robots. The environment is highly unpredictable, so changes in other players’ positions and target position should be coped with flexibly. Recent progress in the robot soccer competition RoboCup ( has led to the by now frequently applied “omniwheels” [16] as shown in Fig. 1. This finding allows for the holonomous movement of the robots, thereby removing the non-holonomous path planning constraints. However, physical limits such as the maximum acceleration and maximum velocity of the robot are still an important issue.


D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

where ai is the length of the projected vector and bi is the distance between the position of object i to the projection point, as indicated by the arrows in Fig. 2. The set of all objects, represented by their indices, is defined as O = {1, . . . , n} ∈ Nn ,


with n giving the amount of objects in the field. The set of blocking objects B contains all objects that the robot would hit if it moved in a straight line to the target and can be computed as follows by using (1)–(3) B = {i ∈ O|0 < ai < |tE − oEr | ∩ |bi | < ri + rr } ∈ Nm


where m is the amount of blocking objects and ri , rr ∈ R+ are the radii of object i and the robot respectively. Using (1) and (4) the first obstructing object f can be determined f = min B. Fig. 2. An example of a situation with blocking objects between the target and the robot.



3. Find a subtarget

3.2. Clustering

Fig. 2 shows a typical situation that an autonomous robot might encounter during operation. The objective of the robot is to move as fast as possible to the target without colliding with other objects in the field. The global idea of our proposed algorithm is to determine a subtarget that the robot can go to in a straight line after each position update of the robot and surrounding objects. This way, the robot is able to adapt to a changing environment including movement of objects, (dis)appearing objects (e.g., due to sensing limitations) and a target position change. The procedure of determining the subtarget is explained in 4 steps: first obstructor, grouping, location of subtarget, and iteration procedure. For simplicity, the robot and the objects are all modeled as circles, which bound the physical geometry. However, this is not essential for the global idea of the proposed path planning method. Arbitrary polygons could be used instead, resulting in a higher computational cost because the clustering becomes more complex. For our application, circular bounds are sufficient, because all robots can be described well with a circle and e.g. a goal can be described with several circles on a row. Furthermore, the accuracy of the on-board vision system is not able to detect the actual shape of another robot projected onto the ground. In the remainder, we suppose that all obstructors can sufficiently closely be described by circles.

The next step is to determine the largest set G, containing f , in which for each object i holds that the gap between at least one other object j ∈ G, j 6= i is smaller than the diameter 2rr of the robot. This is also called agglomerative clustering with a 2rr threshold. In our approach, the group G will be avoided instead of the object f only, otherwise the robot could get stuck in the group G.

3.1. First obstructor To determine which objects are obstructing and which side to pass, the centers of all the objects are projected onto the line between the robot and the target. For the projection of oEi − oEr onto tE − oEr , we obtain (tE − oEr ) · (E oi − oEr ) ai = ∈R |tE − oEr | bi =

(tE − oEr ) × (E oi − oEr ) ∈R E |t − oEr |

(1) (2)

3.3. Location of subtarget The side to pass the group G is determined by selecting the side with the smallest outer distance, |bi | + ri ∀i ∈ G. This selection results in an unstable equilibrium point when moving towards group G, which is desired; that is, if the robot moves to one side of the group, the largest outer distance |bi | + ri on that side becomes smaller, and on the opposite side, this absolute distance becomes larger. This is not true if the smallest angle to pass group G would be chosen, in that case, limit cycling in front of group G can occur if group G consists of more than one object resulting in not reaching the target. s = arg max (max sbi + ri ). s={−1,1} i∈G


If s is positive, then the right side is chosen to pass group G, else the left side is chosen. To determine the subtarget position, the angle to pass each object in the group on the selected side,     2rr + ri bi − s arcsin (7) αi = arctan ai |E oi − oEr | is computed. Object a is defined as the object with the largest absolute angle, αa as computed by (7), and determines the position of the subtarget sE which becomes   cos αa −sin αa tE − oEr sE = oEr + |E o − oEr |. (8) sin αa cos αa |tE − oEr | a

D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

For the chosen side, αa is the smallest angle with respect to tE − oEr , such that the robot passes group G on the selected side without colliding with an object in group G. The subtarget is put at a distance |E oa − oEr |. 3.4. Iteration procedure A situation might occur where, between the robot and the computed subtarget, an object of another group in the field is obstructing. Previous steps should be recomputed while setting the subtarget as the target. This iteration will converge to a collision free subtarget if group G does not imprison the robot. During each iteration cycle, only those objects are considered which are closer to the robot than the (sub)target. In the worst case, n iterations will be necessary, but for our application, 1 or 2 iterations turn out to be sufficient. 4. From subtarget to path The procedure as presented in the previous section, returns a subtarget after each environment update. However, especially in a changing environment, the location of the subtarget might change fast, for instance to reflect sudden changes in the location of the various objects or in the location of the target. Although it is necessary to address these changes, it might result in a ragged path with accelerations that exceed the robots’ capabilities. These physical limitations jeopardize the ability of the robot to actually follow the prescribed path. Therefore, a smoothing algorithm is included. A path is defined to be smooth if the accelerations necessary to track the path are continuous and bounded functions in time. This also implies that the velocity and position are continuous functions in time. The jerk (the time-derivative of the acceleration), however, is allowed to be a discontinuous, yet bounded, function of time. To impose these constraints on the generated path, a filter containing a feedback loop and saturations on the jerk, acceleration, and velocity is used, which ‘controls’ the velocity to the desired direction by manipulating the (2-dimensional) jerk of the path. In Section 4.1, the linear feedback controller design is presented. After that, the saturations on the jerk, acceleration, and velocity are added to the linear feedback controller. Finally, how to choose the desired velocity is explained.

Consider again Fig. 2. The frame {O 2 , eE2 } is fixed onto the robot position and eE12 is directed towards the position of the subtarget. eE2 = A21 eE1


where A21 =

cos θ −sin θ

sin θ cos θ

The velocity of the robot with respect to frame {O 1 , eE1 } is T oE˙ r = o˙ r1 eE1 .

and θ is the counter clockwise rotation of frame {O 2 , eE2 } with respect to frame {O 1 , eE1 }.


From (9) and (10), we obtain the velocity of the robot with respect to the origin O 1 and the orientation eE2 T −1 oE˙ r = o˙ r1 A21 eE2 | {z } o˙ r2


o˙ r2 = A21 o˙ r1 .


The robot has to travel with a desired velocity vdesired in the direction of the subtarget. Note that this is accomplished by setting the velocity along the path, o˙r21 to vdesired and the velocity perpendicular, o˙r22 to 0. However, as stated before, this might result in a ragged path, e.g., if the robot has an initial velocity perpendicular to the direction of the subtarget. If a controller is defined that can only influence the jerk of the desired movement, a smooth path results. Since the location of the subtarget is updated regularly, it suffices to control the direction of travel, instead of the path. This behavior is more natural, and results in a smooth, short path. Assume that the path is to be generated using a fixed sampling rate, which might, for instance, be the sampling rate of the motion controllers which control the robot wheels (typically ∼1 kHz). Take the sampling rate to be 1 kHz. The aim is to build a controlled system ...2 o r 1 = C(z)(vdesired − o˙r21 ) (12a) ...2 2 o r 2 = −C(z)o˙r 2 (12b) ...2 with o r the jerk (third time derivative of position) of the robot relative to the origin O 1 and orientation eE2 , such that o˙r21 converges to vdesired , the desired velocity, and o˙r22 converges to ... 0. The Tustin transformation at 1 kHz of the jerk o r21 to the velocity o˙r21 is given by P(z) =

(0.001)2 z −1 + z −2 . 2 1 − 2z −1 + z −2


A discrete time controller that accomplishes the convergence of the velocities, is given by C(z) =

4.1. Smoothening


520z −1 − 518.6z −2 . 1 − 1.726z −1 + 0.7545z −2


The layout of this control setup is shown in Fig. 3. A similar ... setup can be drawn for o˙r22 and o r22 , with zero desired velocity. In Fig. 4, a bode plot of the open loop Hol = C P is shown. It can be seen that the bandwidth of the controlled system is about 3 Hz. This could be increased to around 100 Hz; however, this will not improve the performance due to limited acceleration. In Fig. 5, the step response of the closed loop system of Fig. 3 is shown. It can be seen that indeed the controlled system converges to the desired velocity, with a small overshoot and rise time of 0.1 s, which is reasonable regarding the application area. For other applications, the bandwidth of the controller can be adapted at will.


D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

... Fig. 3. Setup of the controlled system, where the jerk of the path, o r21 , is manipulated to smoothen the velocity and acceleration profile. The parallel velocity o˙r21 ‘tracks’ the desired velocity vdesired . For the perpendicular velocity o˙r22 , a similar scheme can be drawn, with vdesired = 0.

and after saturation of the velocity is reached is given by |0 − asat | = amax . Then, the maximum jerk is given by jmax =

amax dT


with dT the sampling interval. Hence, although possibly large, the maximum jerk is still bounded. Simulations show that the controller (14) is robust to sudden changes in the target, changes in the desired velocity, and the disturbances caused by the saturation. Simulations show that the linear control system as presented in the previous section subjected to the state constraints is stable. In literature, a lot of work has been carried out to prove stability for linear systems with state constraints, e.g. [17], however, this goes beyond the scope of this work.

Fig. 4. Bode plot of the open loop Hol (z) = C(z)P(z).

4.3. Desired velocity

Fig. 5. Stepresponse of the controlled system as shown in Fig. 3.

4.2. Saturation From the jerk, as derived in (12), the update of the acceleration and velocity profile is computed. However, an acceleration and velocity can result that exceed the capabilities of the robot at hand. Therefore, saturations are imposed on the acceleration and velocity. Let the maximum velocity of the robot be given by vmax and the maximum acceleration by amax . Then, if the bound of the maximum acceleration is exceeded by the controller output as given in Fig. 3, the acceleration is reset to its maximum value asat = sat(a, amax )

(15) if x < −xmax if −xmax ≤ x ≤ xmax if x > xmax .

The velocity is treated similarly: vsat = sat(v, vmax ).

acentrif. =

v2 . r

Considering (18), an appropriate choice for vdesired is p  vdesired = sat |Es − oEr |amax , vmax ,

where  −xmax sat(x, xmax ) = x  xmax

As input for the controlled system as shown in Fig. 3, a desired velocity is given. Typically, for a straight trajectory, the desired velocity will be close to the maximum allowed velocity of the robot in order to reach the target as fast as possible. However, when the robot has to make a fast turn, then the desired velocity must be lower due to the acceleration limit. The distance of the robot to the subtarget is a qualitative measure of how close an object is. Because the omniwheels render the robot a holonomic system, every change in movement requires the power of the motors. This is in contrast with a non-holonomic system, e.g. a car, which can change direction using steering wheels without using the power of the engine. Suppose that the robot has to make a turn with turning radius r ; then the desired velocity follows from the maximum acceleration of the robot. For a circular movement, this maximum acceleration is equal to the centrifugal acceleration


However, if indeed the velocity is saturated, the acceleration has to be computed which would have resulted in this velocity, since |vsat | ≤ |v|. The maximum change in acceleration before



which is the maximum possible velocity to circle around an object, where its radius plus the radius of the robot is equal to |Es − oEr |. Furthermore, if the subtarget is equal to the target, the desired velocity will reduce to zero when the robot has reached the target location. Note that only the robots’ acceleration is considered. An extension is to consider the acceleration per omniwheel, but this goes beyond the scope of this work.

D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479


4.4. Applicability to non-holonomic systems The idea of controlling the velocity in both directions, as explained previously, can also be projected onto a differential drive platform, which is another non-holonomic system. However, for such a platform, the heading angle of the robot has to be controlled towards the target instead of controlling the perpendicular velocity to zero. For a car-like platform, the heading angle of the robot towards the target could also be controlled to zero; however, convergence is not guaranteed due to the limited steering angle and due to the fact that the platform cannot change the angle when the forward velocity is equal to zero. 5. Extensions to increase robustness To increase the robustness of the proposed algorithm, several additional adjustments can be made. Especially, if the algorithm is to operate in an environment with partial and/or noisy measurements of the surroundings, or within a rapidly changing environment. The determination of a new subtarget can be implemented with every new measurement of the environment. The extensions, as proposed in this section, can be integrated in the main procedure and therefore they are also evaluated at every step. As a first extension to the main procedure, a safety margin around the obstacles can be incorporated. The entire region of the obstacle plus a safety margin is then regarded as the obstacle to be avoided, and the subtarget is located outside this region. With this extension, the chance of colliding with another object decreases with an increasing safety margin. This feature is also a robustness margin for noisy position measurements. The safety margin can be used as a parameter to trade off movement through tight areas with a high collision probability. Another extension is the use of a sort of software airbag. If the path accidentally happens to enter the safety margin region, the subtarget is instantaneously relocated outside the safety margin, and the path is sort of “repelled” by the obstacle, similar to a potential field. This will reduce the speed of a collision or even prevent a collision. An additional extension is to use the velocity information of the robot and the obstacles. The safety margin is resized as a function of the velocity, such that the path passes fast-moving obstacles at a larger distance. For this purpose, position sensors have to be accurate enough to estimate the velocity, as any noise on the position estimation deteriorates the velocity estimation dramatically. Another extension is to alter the relation between the distance to the subtarget and the desired velocity (19). By decreasing the desired velocity faster, the robot can make sharp turns further away from the object, resulting in a higher robustness. This extension balances the speed and collision probability. All these extensions will affect the aggressiveness of the robot. The closer and faster it passes objects, the more risk is taken regarding collisions with objects. To plan a collisionfree path is impossible for arbitrary situations. Suppose that

Fig. 6. Simulation with 9 objects where one group encircles the robot.

an obstacle on a bounded field can move with equal speed and its intention is to hit the robot instead of avoiding it, then it will always be able to hit the robot assuming similar maneuverability, not to mention if there are more obstacles are on the field. Planning a path with a high probability of no collision is possible by increasing all the margins presented earlier. However, this will result in a very conservative path that involves going around all robots instead of the possible paths in between the objects. For the application of robot soccer, this means that our robots will move out of the way while the other team is scoring goals. 6. Simulations 6.1. Static simulation First, we consider a static situation with 9 obstacles at known positions. The robot and obstacles have a radius of 0.3 m. The maximum velocity, acceleration and jerk of the robot are 2 m/s, 2.5 m/s2 and 625 m/s3 respectively. The update rate of the subtarget is 30 Hz and the trajectory is generated at a rate of 1000 Hz. The dots have an interval of 0.2 s, hence they indicate the velocity of the robot. In Fig. 6, the result of the simulation is shown. Clearly, if the robot comes close to the obstacles, it decreases its velocity such that it is able to change direction fast enough. If the velocity didn’t decrease, a large overshoot of the trajectory would occur, possibly resulting in a collision. In Fig. 7, a potential field algorithm and a wavefront algorithm are used to generate a path for the same situation as in Fig. 6. The potential field algorithm does not converge for the given situation, because it is stuck in a local minimum resulting from the cup shaped obstacle group. The wavefront algorithm approximates the optimal path in the sense of shortest distance. However, it is not yet a smooth path satisfying the physical limitations of the robot. So, an additional algorithm is required to make a feasible path regarding the physical limitations of the robot. This will further increase the computation time and furthermore, if the field of interest is larger while keeping the same resolution of the wavefront algorithm, the computation time will increase proportionally to the surface area.


D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

Fig. 7. The solution of a potential field algorithm (left figure) and wavefront algorithm (right figure) for the same situation as shown in Fig. 6. The dot is the robot position and the cross is the target position.

Fig. 8. Simulation of the proposed algorithm subjected to a rapidly changing environment.

A case where our algorithm does not come up with a converging trajectory is when a group of objects encircles the target position. For example, if the initial position and target position in Fig. 6 are swapped, the robot will wait close to the target position with the objects in between until the group splits up and/or the target position is not encircled anymore. A possible way to overcome this problem is to place the target

outside the group in a collision-free straight line. Actually, this is equal to using the proposed algorithm the other way around, so we try to move the target to the robot. Fortunately, for our application, RoboCup, such a situation is very rare. And even if it occurs, other robots of the same soccer team will probably be able to reach the target position.

D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479


Fig. 9. Simulation of the potential field algorithm subjected to a rapidly changing environment.

6.2. Dynamic simulation The proposed subtarget algorithm will be compared to the potential field algorithm in a more realistic environment. The comparison with the wavefront algorithm is omitted because it is too computationally expensive to run the vision at the desired frequency. Both algorithms will be subjected to an environment with obstacles of different sizes, which randomly move at a maximal 1 m/s. The random movement is obtained by injecting noise on the acceleration with a maximum of 1.5 m/s2 . To simulate measurement noise, e.g. due to the vision system, noise is added to the position of the obstacles with a standard deviation of 0.03 m. Parameters for the proposed algorithm are similar as used in the static simulation. The result of a trial is shown in Fig. 8. The value of the potential field J at the robot position oEr is X 2 e−2kEoi −Eor k2 . (20) JoEr = ktE − oEr k2 +

Fig. 10. Arrival times of 100 trials for the proposed algorithm (grey) and the potential field algorithm (black).


The gradient of J at oEr is calculated by using an Euler approximation with a step size of 0.01 m. The gradient direction and amplitude determines the acceleration vector limited by the maximum of 2.5 m/s2 . Double integration, with also a saturation on the velocity of 2 m/s, results in the position of the robot. The same random trajectories used in Fig. 8 are used in the simulation using the potential field algorithm. The results are shown in Fig. 9.

Fig. 11. Mapping of the omni-vision image to a top view.

If we compare Figs. 8 and 9, the proposed algorithm converges better. The potential field algorithm is held up by the cup shaped obstacle group. Due to the randomly moving


D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

Fig. 12. Experiment where the robot has to move across the field while avoiding the obstacles. Images on the robots’ path are captured and shown above. For each image, the following marks are also shown: target (square), subtarget (large cross), detected obstacles (circles), detected line points (small crosses).

obstacles, the cup breaks up at a certain time such that the robot manages to find a way through. Because the result is trial dependent due to the random walks of the obstacles, 100 trials are performed for a better comparison between the proposed algorithm and the potential field algorithm. The algorithms are subjected to the same situation each trial. Each trial has an equal initial state, however, the seed of the noise is chosen randomly per simulation/trial such that obstacles move differently during each trial. The arrival times and other statistics of the 100 trials are shown in Fig. 10 and Table 1 respectively. For this given situation, the proposed algorithm outperforms the potential field algorithm with an averaged arrival time of ±40% less. The proposed algorithm is able to deal with obstacles close to the robot as well as far away from the robot. The potential field can only make a trade off between the two. If the contribution of an object to the potential field is more global, the path will become very conservative, and if the contribution is too local, the path is only changed close to the object, resulting in a higher collision probability. 7. Experiments The path planning algorithm is also implemented on the robots of Tech United. The robot is placed in front of the yellow goal, and the objective is to drive towards the blue goal,

Table 1 Results of 100 trials with equal initial conditions but different obstacle movement

First to arrive Average arrival time Average hits per trial

Proposed algorithm

Potential field algorithm

91% 10.0 s 0.65

9% 13.9 s 1.19

with obstacles placed in between. For illustration purposes, the omni-vision image is transformed to a top view via the pixeldistance relation rmeters = p1 tan( p2rpixels )


where rpixels is the distance towards the robot center in pixels, p1 and p2 are calibrated parameters, and rmeters is the distance towards the robot center in meters. This mapping can be computed for a grid of points, resulting in a top view as shown in Fig. 11. At several points of the robots’ path, images are captured together with the target, estimated obstacle locations, and computed subtarget for that position. The result is shown in Fig. 12. Similar results are observed as in the simulations. Although the obstacles are not moving in this experiment, uncertainties are present in the detection of the obstacles. Obstacles appear and disappear due to the presence of other

D. Bruijnen et al. / Robotics and Autonomous Systems 55 (2007) 470–479

obstacles. The location of an obstacle also varies, due to the color segmentation based obstacle detection. Regardless of these uncertainties, the robot avoids the obstacles, without any hesitation about which side to choose. Furthermore, the detected line points are shown in Fig. 12, which are used for self localization. The presence of a lot of obstacles complicates the detection of these line points and thus the self localization. The algorithm used has similarities with [18], and will not be explained further in this work. 8. Conclusions The proposed algorithm has been shown to generate suboptimal smooth paths in a rapidly changing environment. It outperforms potential field algorithms with respect to global convergence to the target. Although convergence is not guaranteed for arbitrary situations, it converges in most practical situations. Furthermore, it is able to adapt fast in a changing environment due to the low computational cost, which enables trajectory updates at the rate of the image acquisition of the vision system, which in our case operates at approximately 30 Hz. The proposed algorithm has been tested in simulations and experiments and shows promising results. References [1] J. Lee, R. Huang, A. Vaughn, X. Xiao, J. Hedrick, M. Zennaro, R. Sengupta, Strategies of path-planning for a UAV to track a ground vehicle, in: AINS 2003, Menlo Park, CA, USA, 2003. [2] D. Rathbun, S. Kragelund, A. Pongpunwattana, B. Capozzi, An evolution based path planning algorithm for autonomous motion of a UAV through uncertain environments, in: Digital Avionics Systems Conference, 2002. [3] DoD, Unmanned aerial vehicles roadmap 2002–2027, Tech. Rep., Washington, DC, USA, December 2002. [4] M. Eichhorn, A reactive obstacle avoidance system for an autonomous underwater vehicle, in: IFAC World Congress, IFAC, Prague, Czech Republic, 2005, p. CDrom. [5] C. Behring, M. Brancho, M. Castro, J.A. Moreno, An algorithm for robot path planning with cellular automata, in: Proc. Conf. Cellular Automata for Research and Industry, 2000. [6] P. Vallejos, J.R. del Solar, A. Duvost, Cooperative strategy using dynamic role assignment and potential fields path planning, in: Proc. IEEE Latin Amer. Robot. Symp., Mexico City, Mexico, 2004, pp. 48–53. [7] Z. Shiller, Y.-R. Gwo, Dynamic motion planning of autonomous vehicles, IEEE Transactions on Robotics and Automation 7 (1991) 241–249. [8] A. Scheuer, T. Fraichard, Planning continuous-curvature paths for car-like robots, in: Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robots and Systems, 1996, pp. 1304–1311. [9] A. Stentz, The focussed D ∗ algorithm for realtime replanning, in: Proc. 1995 International Joint Conference on Artificial Intelligence, 1995, pp. 1652–1659. [10] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, S. Thrun, Anytime dynamic A∗ : An anytime, replanning algorithm, in: Proc. of the International Conference on Automated Planning and Scheduling, ICAPS, 2005.


[11] A. Bhatia, E. Frazzoli, Incremental search methods for reachability analysis of continous and hybrid systems, in: Hybrid Systems: Computation and Control, in: Lecture Notes in Computer Science, vol. 2993, 2004, pp. 142–156. [12] E. Frazzoli, M.A. Dahleh, E. Feron, Real-time motion planning for agile autonomous vehicles, in: AIAA Conf. on Guidance, 2000. [13] D. Kang, H. Hashimoto, F. Harashima, Path generation for mobile robot navigation using genetic algorithm, in: Int. Conf. Indust. Electr. Control and Instrumentation, Orlando, FL, USA, 1995, pp. 167–172. [14] D. Lebedev, J. Steil, H. Ritter, A neural network model that calculates dynamic distance transform for path planning and exploration in a changing environment, in: Proc. Int. Conf. Robotics and Automation, Taipei, Taiwan, 2003, pp. 4209–4214. [15] H. Sfeir, A neural-network-based path generation technique for mobile robots, in: Proc. Int. Conf. Mech., 2004, pp. 176–181. [16] M. Ashmore, N. Barnes, Omni-drive robot motion on curved paths: The fastest path between two points is not a straight-line, in: AI’02: Proceedings of the 15th Australian Joint Conference on Artificial Intelligence, Springer-Verlag, London, UK, 2002, pp. 225–236. [17] H. Fang, Z. Lin, Stability analysis for linear systems under state constraints, in: American Control Conference, 2004, pp. 441–446. [18] M. Lauer, S. Lange, M. Riedmiller, Calculating the perfect match: An efficient and accurate approach for robot self-localization., in: RoboCup, 2005, pp. 142–153. Dennis Bruijnen was born on March 9th, 1980 in Tegelen, The Netherlands. He received his M.Sc. Degree in Mechanical Engineering from the Technische Universiteit Eindhoven, Eindhoven, The Netherlands, in 2003. Upon completion of his degree, he started as a Ph.D. student in the same group on the subject of mechatronic design exploration for wide format printing systems in association with Oc´e Technology BV. In 2005, he joined the Tech United Robocup team, which participates in the middle size Robocup-league. Jeroen van Helvoort was born on January 15, 1980 in Vlijmen, The Netherlands. He received his M.Sc. Degree (Cum Laude) in Mechanical Engineering from the Technische Universiteit Eindhoven, Eindhoven, The Netherlands in January 2004. Since January 2004, Jeroen is a Ph.D. student in the Control Systems Technology group at the department of Mechanical Engineering of the Technische Universiteit Eindhoven. His research focuses on data-based and disturbancebased control, with a special interest in Unfalsified Control. Jeroen has been with the Tech United Robocup team since its formation in 2005. Within the team, Jeroen focuses on motion planning and sensor fusion. Ren´e van de Molengraft received the M.Sc. degree (cum laude) in Mechanical Engineering from the Technische Universiteit Eindhoven, Eindhoven, The Netherlands, in 1986. From 1986 until 1990 he was a research assistant at the Technische Universiteit Eindhoven. In 1990 he received the Ph.D. degree from the Technische Universiteit Eindhoven on the subject of Identification of Mechanical Systems for Control. In 1991 he fulfilled his military service. Since 1992 he is an assistant professor in the Control Systems Technology group of the Mechanical Engineering Department of the Technische Universiteit Eindhoven.