thomas hellström and ola ringdahl* 7(1-2).pdf58 t. hellström and o. ringdahl with look-ahead...

56 Int. J. Vehicle Autonomous Systems, Vol. 7, Nos. 1/2, 2009

Real-time path planning using

a simulator-in-the-loop

Thomas Hellström and Ola Ringdahl*

Department of Computing Science,Umeå University,SE-901 87 Umeå, SwedenE-mail: [email protected]: [email protected]*Corresponding author

Abstract: This paper describes the development of a real-time pathplanner for off-road vehicles using a simulator. The general idea withthe presented system is to extend a standard path-tracking algorithmwith a simulator that, in real-time, tries to predict collisions in a windowforward in time. If a collision is predicted, the vehicle is stopped and apath-search phase is initiated. Variants of the original path are generatedand simulated until a feasible path is found. The real vehicle thencontinues, now tracking the replanned path.

Keywords: path planning; simulator in the loop; obstacle avoidance.

Reference to this paper should be made as follows: Hellström, T.and Ringdahl, O. (2009) ‘Real-time path planning using asimulator-in-the-loop’, Int. J. Vehicle Autonomous Systems, Vol. 7,Nos. 1/2, pp.56–72.

Biographical notes: Thomas Hellström is currently working as anAssociate Professor at the Department of Computing Science at UmeåUniversity and has received his PhD from the same department in 2001.His research interests are in learning robots, field robotics and machinelearning.

Ola Ringdahl works as a PhD student at the Department of ComputingScience at Umeå University. He received his Licentiate in Engineeringfrom the same department in 2007. His research interests are inautonomous vehicles, path tracking, obstacle avoidance and systemintegration.

1 Introduction

Many path-planning algorithms make simplifying assumptions regarding therobot’s geometry or kinematic behaviour. Typically the robot is approximated byeither a point or a circle, and is assumed to be able to move in a predictable

Copyright © 2009 Inderscience Enterprises Ltd.

Real-time path planning using a simulator-in-the-loop 57

way given by simple kinematics equations. This usually works fine for regularindoor robots but is less suitable for large outdoor robots and autonomous vehicles.In our research on autonomous forest machines (Hellström et al., 2006; Hellströmand Ringdahl, 2006; Ringdahl and Hellström, 2008), the principal differencesbetween avoiding obstacles while following a path with a small indoor robot anda 10-metre-long vehicle with articulated steering have become evident. First, theformer can indeed often be treated as circular, while the latter has a much morecomplicated shape. The articulated joint between the two vehicle parts serves assteering mechanism but also causes the physical shape of the vehicle to changedepending on the steering angle φ. As a consequence of this, the front and rearpart of the vehicle move considerably to the sides when changing steering angle,even when not moving forward. For our specific vehicle Valmet 830, φ may changefrom −43 degrees to +43 degrees. If not taken into account, this may cause thevehicle to hit obstacles on the side when trying to avoid obstacles in front. Second,the kinematics for most indoor robots is well known and mostly accurate for flatsurfaces. For an off-road vehicle, ground slip may be significant and cause partlyunpredictable changes in motion as described further on.

For these reasons, a system, Simulator-in-the-Loop for Path Planning(SIMLOPP), has been developed and is described in this paper. The generalidea with SIMLOPP is to extend a standard path-tracking algorithm with asimulator that, in real-time, tries to predict collisions in a window forward intime. This simulation is based on current sensor data giving information about theenvironment around the vehicle. If a collision is predicted, the vehicle is stoppedand a path-search phase is initiated. Variants of the original path are generated andsimulated until a feasible path is found. The real vehicle may then continue, nowtracking the replanned path.

The paper is organised as follows: Section 2 contains a brief overview of relatedwork. In Section 3, the developed system is described in detail. Results from tests ina simulator environment are presented in Section 4. Conclusions and future workare discussed in Section 5. Appendix A describes the kinematic equations used inthe simulator to model a vehicle with articulated steering. Appendix B contains amore detailed description of the developed algorithms.

2 Related work

This work was initiated by a need for an obstacle-avoidance and path-planningsystem in our work with autonomous forest machines. In our previous work(Hellström et al., 2006; Ringdahl and Hellström, 2008) we implemented the wellknown algorithm VFH+ (Ulrich and Borenstein, 1998) which works well for regularindoor robots. For a forest machine it performs reasonably well at avoidingobstacles in open areas, but has difficulties with narrow passages between obstaclesclose to both sides of the vehicle. This is due to the fact that the algorithm does notdistinguish between obstacles in front and to the sides of the vehicle. Furthermore,all obstacles are enlarged with the length of the front part of the vehicle (whichis larger than the width). Due to this, an obstacle close to the side of the vehiclemay be regarded as being partly in the way of the vehicle. Ulrich and Borenstein,the authors of VFH+, have developed an enhanced obstacle-avoidance algorithm

58 T. Hellström and O. Ringdahl

with look-ahead verification, called VFH∗ (Ulrich and Borenstein, 2000). VFH+calculates a number of suitable directions to travel, assigns a cost value to each ofthem, and selects the direction with the lowest cost as the new direction of travel.In contrast, VFH∗ analyses the further consequences of going in each of thesedirections before making a final decision. This is done by computing where thevehicle would be in the next time step if it should go in each target direction.VFH+ is then applied for the vehicle’s new poses and the cost of going in each newdirection is calculated. By repeating this process, a search tree is created, and the A∗

search algorithm is applied to select the best direction to move. This helps to avoidlocal trap situations that the original VFH+ algorithm would not detect. Still, theimproved algorithm has the same strategy for prediction of collisions. It treats thevehicle as a point and expands the obstacles to the size of the vehicle. Furthermore,the algorithm does not take into account that the rear part of a large vehicle mayhit an obstacle even after the front part has passed it when the steering angle ischanged.

Thrun et al. (2006) use an online path planner that calculates possibletrajectories to avoid obstacles. This is done by adding a lateral offset to a referencepath, and also a rate at which the vehicle will attempt to adjust to this newtrajectory. The planner is implemented as a search algorithm that minimises alinear combination of continuous cost functions. The vehicle model includes severalkinematic and dynamic constraints, such as maximum steering angle, maximumsteering rate and maximum deceleration. The cost functions penalises running overobstacles and leaving the specified corridor the vehicle is allowed to be within.

Noguchi and Terao (1997) have developed a simulator for an agricultural robotusing a neural network. They apply a Genetic Algorithm (GA) to plan an optimalpath given the required initial and final states of the vehicle. To check if the vehiclecollides with an obstacle the distance between the vehicle position and an enlargedobstacle is computed. The GA generates steering angles and changes in steeringangles. These are then sent to the neural network which simulates each generatedpath.

Brian (2001) uses evolution-based path planners to find optimal andcollision-free paths through environments containing both static and dynamicobstacles and target locations. A GA is used with a simulator to guide air vehiclesto their destination without colliding with obstacles. To detect collision withobstacles, minimally enclosing rectangles are used. Bounding rectangles are firstfitted around the vehicle and each obstacle. The overlap between the vehiclerectangle and each obstacle rectangle is then computed. Capozzi concludes that thecomputation time mainly is spent at collision detection, and that faster methodsshould be found. The author also suggests that the method to generate a new pathby moving the original one a distance to the side can be generalised to a sequenceof manoeuvres. Such manoeuvres could be, for example, turn left slowly and turnright quickly.

In both Noguchi and Terao (1997) and Brian (2001), no explicit path trackeris included. The optimised control commands are simply applied until the nextoptimisation takes place. In our approach, the optimisation generates a setpath, and the path-tracking algorithm takes care of fine adjustments at a muchhigher rate (5–10 times per second). Furthermore, in our approach an advancedphysics-based simulator could be included. In this way ground interaction such asslipping and sliding could be taken in to account in path optimisation.


3 System description

In this section, the simulator-driven path-planning system is described in detail.Pseudo-code for the top-level routine is given in Algorithm 1, Appendix B.The main loop performs regular path tracking (Line 10) with the wanted path p.Every metre, the path is evaluated in a simulated path-tracking 5m forward topredict the risk for collision (Line 3). To track the path, the simulator usesthe path-tracking algorithm Follow the Past (Hellström and Ringdahl, 2006).Other path-tracking algorithms may be used as well, provided they are based onthe perpendicular distance to the path, and do not aim at the path further ahead(i.e., the Pure Pursuit algorithm (Coulter, 1992) does not work well in this context).The simulation is performed by the SimCheck function described in Algorithm 3.If no collision is predicted, path tracking along p continues until the goal is reached.If a collision is predicted, a new path is constructed (Line 5) by modifying p. Thisis done by the PathSearch function described in Algorithm 2. If a feasible pathcan be constructed, it replaces the reference path and is used by the path trackerto guide the real vehicle around the obstacles. Note the collision detection runscontinuously during path tracking. Hence, as soon as a new collision is predicteddue to previously unseen obstacles, a new path-search will take place. A graphicaloverview of the system is given in Figure 1. The main components are described indetail below.

Figure 1 System description of the simulator-driven path planner

3.1 Path searching

The PathSearch function in Algorithm 2 attempts to find a candidate path, similarto the original path that a simulated vehicle can track for 10m without hittingany obstacle or straying too far away from the original path. This distance givesboth a reasonably long simulation horizon and a reasonably short computation


time. Choosing an even longer distance would mean increased computation timeand also larger uncertainties in obstacle sensor readings. The path evaluation isdone by simulated path tracking along the candidate path and is performed by theSimCheck function (see Algorithm 3) that returns a fitness value for the candidatepath. The fitness concept is further described in Section 3.2.

The search problem formulated in Algorithm 2 aims at finding a vector xopt

such that the candidate path generated by the function p(xopt) has a high enoughfitness value. The p(x) function works as follows. Ten metres of the original pathin front of the vehicle is extracted and equally divided into five segments. Theelements of the x vector represent lateral offsets for the segments, such that theyare shifted perpendicular to the original path, thus creating a modified candidatepath, as illustrated in Figure 2. The search algorithm tries to find an acceptablecombination of these five offsets. Initially, a sequence of offsets to the steeringcommands from the path-tracking algorithm were used instead of modifying thereference path, as suggested by Brian (2001). This is hard to combine with pathtracking that tries to keep the vehicle on the path since an offset steering commandcauses the vehicle to deviate from the path. The path tracker’s response is tocommand the vehicle to turn back towards the path to compensate, therebycounteracting the offset. Using the offset to modify the reference path avoids thisproblem.

Figure 2 Ten metres of the original path in front of the vehicle are divided intofive segments. By offsetting each segment perpendicular to the original path thesearch function tries to find a combination of offsets that allow the vehicle toavoid the obstacles (see online version for colours)

Figure 3 illustrates how an offset segment makes it possible for the vehicle to avoidobstacles. The upper panel shows how the simulation (grey area) predicts a collisionwith an obstacle. In the lower panel the path is replanned such that the obstaclesare avoided. In this paper we have implemented and evaluated three different searchalgorithms that are described in more detail in Section 3.3.

3.2 Evaluating paths

As described above, paths are evaluated at two places in the system: In the mainloop to decide if the vehicle can go on tracking the path without colliding, and


in the PathSearch function, used to generate alternative paths. In both cases, thefunction SimCheck performs the evaluation as described in Algorithm 3. SimChecksimulates path tracking for a fixed distance D ahead on the path. For each metre,performance is computed such that the performance for the path as a whole canbe returned as the minimum of all momentary performance values. The kinematicsof a forest machine with an articulated joint is more complex than for manyother vehicles and robots. This is, as described above, part of the motivationfor the development of the simulator-based path planner, and is also a necessarycomponent of the real-time simulator. Appendix A describes the kinematics for thesimulated articulated forest machine in more detail. The used simulator also modelsother aspects of the forest machine, e.g., the actuators responsible for changingsteering angle.

Figure 3 If the simulation predicts a collision with an obstacle (upper panel) the path isreplanned such that the obstacles are avoided (lower panel) (see online versionfor colours)

A momentary performance is computed by the fitness function comprising twoparts. One part is related to the distance between the vehicle and the obstacles, andthe other is related to the distance between the vehicle and the original referencepath p0. In many off-road environments it is not safe to stray too far away fromthe reference path because of obstacles that can not be detected by the vehicle’ssensors, e.g., mire ground or deep trenches. Therefore, in addition to avoidingobstacles, the vehicle must also stay reasonably close to the reference path. In ourparticular implementation, the fitness takes values between 0 and 10, where 10 isbest, and 0 means either that the vehicle is closer than a safety margin to an obstacleor strays too far from the path. The fitness function is illustrated graphically inFigure 4. The minimum accepted fitness value is 5, which is the value of theconstant minfitness referred to at many places in the listed algorithms. Fitness5 corresponds to a minimum distance of 0.5 m between any part of the vehicle and


an obstacle, and a maximum distance of 2.5m between the vehicle and the path.Depending on the accuracy of the obstacle sensors, different fitness functions maybe used.

Figure 4 The fitness value is a function of the vehicle’s shortest distance to any obstacleand its distance to the original reference path. If the fitness value is 5 or above,the path is regarded as traversable at this particular point

It is important to distinguish between the momentary fitness for a single point,computed by the fitness function, and the fitness for a whole path, which iscomputed as the minimum of the fitness values for all vehicle positions whensimulating path-tracking along the path. This path fitness is computed by thefunction SimCheck in Algorithm 3.

3.2.1 Distance between vehicle and obstacles

At each simulated time step the shortest distance between the vehicle and allobstacles is calculated (Algorithm 3, line 3). This is a key operation in the systemand has to be both fast and accurate. The sensor data provides an updated viewof the environment around the vehicle. This is used by the simulator together witha geometrical model of the vehicle to detect collisions with obstacles. The vehicleis modelled by eight lines, four for the front part and four for the rear part,connecting the vehicle’s corners. The distance between the vehicle and an obstacle isdefined as the shortest distance between the obstacle, defined as a point, and each ofthese lines. To speed up the computation, two things are done: first, only obstacleswithin a radius of 5.3m around the vehicle’s articulation joint are considered. Thisvalue is the length of the front part of the vehicle plus a safety margin of onemetre; second, line-obstacle distances are only computed for six of the eight lines,as illustrated in Figure 5. This is based on the assumption that the two lines closestto the articulation link can never be closest to an obstacle, unless a collision wasdetected in a previous time step, due to the the vehicle’s geometry.


Figure 5 To speed up the computation of the smallest distance between obstacles and thevehicle, only the continuous lines in the figure are used. Furthermore, onlyobstacles within the bounding circle drawn around the vehicle are considered inthe computation (see online version for colours)

3.3 Search algorithms

Three different algorithms have been evaluated for the informed search taskdescribed in Section 3.1; DIRECT (Jones et al., 1993) which is a numericaloptimisation algorithm using no derivatives, a GA (Chipperfield and Fleming,1995) and, as a benchmark, a random search algorithm. Each algorithm generatesa population P consisting of N individuals Ii, 1 ≤ i ≤ N , each one representinga candidate path identified by the offset values xi(1), . . . , xi(5). The principaldifference between the algorithms is the way in which new individuals aregenerated/selected based on the previous population (i.e., the p function inAlgorithm 2). Random search generates random individuals. This method is usedas a benchmark for the other two algorithms.

The GA is implemented using the Genetic Algorithm Toolbox (Chipperfield andFleming, 1995) for Matlab. The basic steps in the GA are described in Algorithm 4.It is inspired by nature’s way of improving a population over several generationsby combining genes from successful parents. To increase the chances of findinga global maximum, the mutation operation is introduced. It selects one or morechromosomes xi(j) in an individual i and randomly changes them. This randomprocess is often seen as a (theoretical) assurance that the probability of findingany given individual is never zero (Goldberg, 1989). To avoid testing the samesolution more than once in each generation, an individual is not considered if anequal one has already been generated in the same generation. Two individuals I1and I2 are considered equal if maxj |x1(j) − x2(j)| ≤ 0.01, i.e., if all correspondingchromosomes in the two individuals differ by less than 1 centimetre.

We use a Matlab implementation gblSolve (Björkman and Holmström, 1999)of the global optimisation algorithm DIRECT (Jones et al., 1993). The firststep in the algorithm is to transform the search space to a unit hypercube.The fitness function is then sampled at the centre-point of this cube. Thealgorithm identifies a set of potentially optimal rectangles in each iteration.All potentially optimal rectangles are further divided into smaller rectangles, whosecentre-points are sampled. Conventional optimisation algorithms like DIRECT aimat minimising an object function, as opposed to GAs, which aim at maximisation.The search condition SimCheck(p(xopt), 10, p0) ≥ minfitness in Algorithm 2 istherefore changed to SimCheck(p(xopt), 10, p0) ≤ −minfitness when using theDIRECT algorithm. To speed up execution, DIRECT has been modified to returnimmediately if an object function value < −minfitness is found.


4 Results

To evaluate the developed system and the three different search algorithms anadditional vehicle simulator is used. The simulator tracks each of the five paths inFigure 6 100 times with the three algorithms.1 To test the algorithms’ ability toplan collision-free paths in difficult situations, the obstacles are placed such thatthe original paths cause collision and need careful modifications to be feasible.However, in most real situations, the vehicle has much more space to manoeuvre.

As described in Section 3, the path-tracking simulator is called every metre andthe search module is called if a collision is predicted (or rather if the predicted pathfitness is too low). Depending on the path and the obstacles in the environmenteach search either finds a solution or fails within the allowed maximum number ofiterations. Figure 7 shows the result of replanning a path with DIRECT. Note thatthe vehicle drives along a smooth trajectory, even if the replanned path is notsmooth. Tables 1–5 summarise the results for the three search algorithms forthe five paths. GA denotes the Genetic Algorithm, DIR denotes the DIRECTalgorithm, and Rand denotes the random search. The results presented in the tablesare:

Successful searches: Number of solved search problems.Average search time: Computed for all successful search problems.Average no. of fitness evals: Computed for all successful search problems.Average time to halt: Computed for all failed search problems

(maximum allowed iterations was reached).No. of search operations: Number of calls to the algorithm (total for 100 runs).

In general, the genetic and DIRECT algorithms have roughly the same performancewhile the random search usually does not solve as many situations and also takeslonger. This is to be expected, as random search just tries random solutions, whilethe other two algorithms try to solve the search problem in a systematic way.In almost all test cases, the DIRECT and GAs manage to safely navigate the vehiclealong the whole path, but the performance for path 2 is not as good as for theother paths. This path contains particularly difficult situations, making it hard tofind a traversable path. The reasons for this may be that a path correction in thebeginning of the path leads the vehicle into an impossible situation further on.This is confirmed by the good results for the random algorithm for the same path,and also by the relatively higher performance for the GA which contains a largerelement of randomness than does the DIRECT algorithm. The problem is related tothe limited 10m prediction window. Traversable paths may be possible to generateif the size of this window is increased, of course at the price of longer computationtimes.

Table 1 Results for path 1

Algorithm GA DIR Rand

Successful searches [%] 96 100 44Average search time [s] 12 17 24Average no. of fitness evals 109 152 625Average time to halt [s] 90 – 78No. of search operations 581 900 170




Successful searches [%] 80 67 81Average search time [s] 4 2 3Average no. of fitness evals 186 111 194Average time to halt [s] 98 33 64No. of search operations 500 300 406



Successful searches [%] 100 100 99Average search time [s] 4 2 2Average no. of fitness evals 52 19 24Average time to halt [s] – – 66No. of search operations 354 500 370



Successful searches [%] 99 100 87Average search time [s] 6 8 9Average no. of fitness evals 45 47 184Average time to halt [s] 64 – 85No. of search operations 421 300 362



Successful searches [%] 100 100 98Average search time [s] 4 4 8Average no. of fitness evals 25 29 81Average time to halt [s] – – 95No. of search operations 400 400 416

The time it takes to solve a search problem depends on the necessary number ofcalls to the fitness function. It also depends on how far along the path the simulatorhas to run for each candidate path to find out if the vehicle will collide with anobstacle or stray too far from the reference path, i.e., if the fitness becomes zero.In our tests, the average time to find a solution varies between 2 s and 24 s. Anotherimportant performance measure is the time it takes before giving up on a problemif no solution is found. This time depends mainly on the number of individualsevaluated, but as explained above also on how far the simulator has to run for eachindividual. In our tests, this time varies between 33 s and 98 s. It should be notedthat the maps in Figures 6 and 7 are not accurate a priori maps, but the result ofcollecting sensor information along the route.


Figure 6 The five paths used to evaluate the algorithms. The vehicles are shown in theirstarting positions. The obstacles shown are the result of collecting sensor dataalong the entire route (see online version for colours)

Figure 7 The result of replanning a path. The figure shows the original path as well asthe replanned one, and the vehicle’s position as it tracks the new path.The vehicle is shown in its final position (see online version for colours)

Figure 8 shows the improvement of fitness over several generations in the GA forten different problems solved by the GA. The final individuals in this example allhave acceptable fitness values (at least five), but depending on the difficulty of thesituation, it takes between 3 and 7 generations to get there.

In addition to avoiding obstacles on a predefined path, the developed SIMLOPPsystem can be used to plan a completely new path, where only start and goal


points are initially given together with an accurate map that includes all knownobstacles in the environment. This map could for instance be generated byairborne laser scanning (Wallerman and Holmgren, 2007). The planning is doneoff-line in advance, with an additional simulator replacing the real vehicle. Thesimulated vehicle moves along a path generated by repeated calls to the path-searchmechanism that generates feasible 10m long path segments when necessary. In thisscenario the vehicle should be given more freedom to deviate from the original path,since it only reflects the general preferred direction of motion and not necessarilya reasonable path alternative. In our tests, the vehicle was allowed to move up to10m away from a straight line between start and goal. Figure 9 shows the results,where a 200m straight path between start and goal points is specified, and theenvironment contains a random set of obstacles. The system manages to generate a

Figure 8 Maximum path fitness in a population as a function of generation, for tendifferent problems solved by the Genetic Algorithm. When the fitness valuebecomes at least 5, the path is considered traversable and search is terminated(see online version for colours)

Figure 9 A completely new path can be planned off-line in advance, given only the startand goal points. Combined with online use of the system, obstacles not presenton the map can be detected and avoided in real-time (see online versionfor colours)


collision-free path from start to goal. This way of performing off-line path planningcan also be combined with the online use of the system. In this way, obstacles thatwere not present on the map can be taken into account as soon as they are seen bythe vehicle’s sensors.

5 Conclusions and future work

The proposed SIMLOPP system can be used in various scenarios for autonomousvehicles and robots. Referring to the forest machine application for which thesystem is developed, the most obvious use is for improved obstacle avoidance,where a reference path is first demonstrated by a human driver (Hellström andRingdahl, 2006). To autonomously track this learnt path, the vehicle wouldsimulate 5 m ahead, and stop if a collision is predicted. Alternative modificationsof the reference path will then be simulated until a feasible modified track has beengenerated. If no such path can be found, the vehicle would have to take otheractions such as calling for human assistance. Of course the entire operation wouldtake place in real-time and obstacles would be provided by the vehicle’s sensors.This scenario is illustrated in Figure 7. Our tests show that using a simulator topredict and avoid collisions works well. The system is able to safely navigate alarge forest vehicle around obstacles on and close to the path. The main advantagecompared to VFH+ is the handling of narrow passages and the fact that the wholevehicle is taken into consideration, not only the front part. Another scenario, alsoenvisioned in a forest environment, is off line path planning of a longer route,based on map information. An approximate path given by a straight line from startto goal is then modified in the same way as described above. An example of thisscenario can be seen in Figure 9. The two scenarios may also be combined suchthat a rough plan is first generated off-line. While tracking this path, replanningmay occur if new obstacles appear in the visible neighbourhood of the vehicle.

The presented work uses a simulator instead of the physical vehicle for theevaluation of the developed system. The next step is to move the system to areal forest machine to validate the results. Initial tests have been conducted andverifies the soundness of the presented approach. The simulator used for pathevaluation in the path-search routine can be replaced by a physics-based simulator.We have initiated development of such a simulator for forest machines, basedon the Colosseum3D framework (Backman, 2005). This 3D-simulator includesrealistic vehicle kinematics and dynamics and can improve the quality of thepath simulations, for example by simulating ground slip. However, the current2D-simulator runs over 100 times faster than real time, which may be hard to matchwith a more advanced physics-based simulator.

The efficiency of the implemented algorithms and the simulator could probablybe increased if the implementation was moved from Matlab to a program languagemore suited for real-time applications, e.g., Java or C/C++. This also means that theaverage search times reported in this paper would decrease or allow the algorithmsto evaluate additional possible solutions, or predict longer distances ahead, in thesame amount of time. This would also mean that the vehicle most often do notreally have to stop to perform path planning. A new feasible path may be generatedin a fraction of a second such that the vehicle can move on without interruption.


References

Backman, A. (2005) ‘Colosseum3d – authoring framework for virtual environments’,Proceedings of 11th EGVE Workshop, Aalborg University, Denmark, pp.225–226.

Baker, J.E. (1987) ‘Reducing bias and inefficiency in the selection algorithm’, Proceedingsof the Second International Conference on Genetic Algorithms and their Application(ICGA2), Lawrence Erlbaum Associates, Hillsdale, New Jersey, USA, pp.14–21.

Björkman, M. and Holmström, K. (1999) ‘Global optimization using the direct algorithmin MATLAB’, Advanced Modeling and Optimization, Vol. 1, No. 2, pp.17–37.

Brian, J.C. (2001) Evolution-Based Path Planning and Management for AutonomousVehicles, PhD Thesis, University of Washington, Seattle, Washington, USA.

Chipperfield, A.J. and Fleming, P.J. (1995) ‘The matlab genetic algorithm toolbox’,IEE Colloqium on Applied Control Techniques Using MATLAB, Digest, (1995/014),pp.10/1–10/4.

Coulter, R.C. (1992) Implementation of the Pure Pursuit Path Tracking Algorithm,Technical Report CMU-RI-TR-92-01, Robotics Institute, Carnegie Mellon University,January, Pittsburgh, PA.

Goldberg, D.E. (1989) Genetic Algorithms in Search, Optimization and Machine Learning,Addison Wesley Publishing Company, Boston, MA, USA, January.

Hellström, T., Johansson, T. and Ringdahl O. (2006) Development of an Autonomous ForestMachine for Path Tracking, Volume 25 of Springer Tracts in Advanced Robotics, Fieldand Service Robotics: Results of the 5th International Conference Edition, Springer,FSR 2005, 29–31 July, 2005, Port Douglas, QLD, Australia, pp.603–614.

Hellström, T. and Ringdahl, O. (2006) ‘Follow the past – a path tracking algorithmfor autonomous vehicles’, Int. J. Vehicle Autonomous Systems, Vol. 4, Nos. 2–4,pp.216–224.

Jones, D.R., Perttunen, C.D. and Stuckman B.E. (1993) ‘Lipschitzian optimization withoutthe lipschitz constant’, Journal of Optimization Theory and Applications, Vol. 79,No. 1, October, pp.157–181.

Mühlenbein, H. and Schlierkamp-Voosen, D. (1993) ‘Predictive models for the breedergenetic algorithm’, Predictive Models for the Breeder Genetic Algorithm, Vol. 1, No. 1,pp.25–49.

Noguchi, N. and Terao, H. (1997) ‘Path planning of an agricultural mobile robot by neuralnetwork and genetic algorithm’, Computers and Electronics in Agriculture, Vol. 18,pp.187–204.

Ringdahl, O. and Hellström, T. (2008) Autonomous Forest Machines – Techniquesand Algorithms for Unmanned Vehicles, VDM Verlag Dr. Müller, 2008. ISBN978-3-639-04343-3.

Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D., Aron, A., Diebel, J., Fong, P.,Gale, J., Halpenny, M., Hoffmann, G., Lau, K., Oakley, C., Palatucci, M., Pratt, V.,Stang, P., Strohband, S., Dupont, C., Jendrossek, L-E., Koelen, C., Markey, C.,Rummel, C., van Niekerk, J., Jensen, E., Alessandrini, P., Bradski, G., Davies, B.,Ettinger, S., Kaehler, A., Nefian, A. and Mahoney, P. (2006) ‘Stanley, the robot thatwon the darpa grand challenge’, Journal of Field Robotic, Vol. 23, No. 9, pp.661–692.

Ulrich, I. and Borenstein, J. (1998) ‘VFH+: reliable obstacle avoidance for fast mobilerobots’, IEEE Int. Conf. on Robotics and Automation, May, Leuven, Belgium,pp.1572–1577.

Ulrich, I. and Borenstein, J. (2000) ‘VFH*: local obstacle avoidance with look-aheadverification’, IEEE International Conference on Robotics and Automation, April,San Francisco, CA, USA, pp.2505–2511.


Wallerman, J. and Holmgren, J. (2007) ‘Estimating field-plot data of forest stands usingairborne laser scanning and spot HRG data’, Remote Sensing of Environment,Vol. 110, No. 4, October, pp.501–508.

Note

1DIRECT is a deterministic algorithm, i.e., the result for each of the 100 runs will beexactly the same. However, for a uniformed presentation of results, 100 runs are presentedalso for the DIRECT algorithm.

Appendix A: Kinematic equations

A simplified mechanical layout is illustrated in Figure 10. The centre of each wheelaxle rotates along a circle with radius r at an angular velocity ω = v/r, where v isthe speed of the vehicle and r depends on the geometry of the vehicle including thesteering angle φ. The front and rear parts (with lengths a and b respectively) of thevehicle rotate along circles with different radii. In the following, only the front partis considered. For the front axis, the radius r is given by

r =a +

b

cos φ

tanφ. (1)

Figure 10 Definition of steering angle φ, position (x, y), heading θ, and turning radius rfor an articulated forest machine. The global coordinate system for the pose(x, y, θ) can be seen in the upper left part of the figure

To avoid modelling slip, r is calculated for a virtual axle located in between thereal axles of the front part. Given a vehicle position (x, y) and heading θ, measuredat the middle of the front axle at time t, the position (x′, y′) and heading θ′ at


time t + ∆t is given by

x′ = cos(ω∆t)r sin(θ) − sin(ω∆t)r cos(θ) − r sin(θ) + x

y′ = sin(ω∆t)r sin(θ) − cos(ω∆t)r cos(θ) + r cos(θ) + y (2)

θ′ = ω∆t + θ + c∆φ

where the constant c ∈ [0, 1] describes how the change in steering angle φ affectsthe vehicle’s heading θ. c is largely a function of slip, which in turn depends on theweight distribution for the two vehicle parts: the front part moves more if the rearpart is heavily loaded. Other important factors influencing slip are tyres, surfaceconditions and minor obstacles which may influence the turning motion. In thiswork c = 0.2 was used, corresponding to a relatively heavy front part that movesless than the rear part of the vehicle.

Appendix B: Algorithms

thomas hellström and ola ringdahl* 7(1-2).pdf58 t. hellström and o. ringdahl with look-ahead...

Documents