608 T. Hellstr¨om, T. Johansson, and O. Ringdahl Modified Valmet 830 Contr ol comm ands : Turn(3.1) Speed(0.36) Halt Sensor data: GPS: 63.53.341N 20.19.247E Compass: 213.5 deg. Radar: 32 de g. 2. 17 me ter 276 deg. 0.45 meter Simulator Modified Pioneer AT2 Switchboard Operator program: • Path recording •Path tracking •Tele operation Smart Modified Pioneer AT2 ttFv ∂∝ ∫ )( ttv ∂= ∫ )( α α Fig.2. The work has been conducted on four differenttarget machines, eachwith increased complexity. This approachgreatly simplifies the researchand development of both hardware and software. 3I nfrastructure for Soft wa re Dev elopmen t As part of the developmentwork, ageneral framework for developmentof software forrobots andautonomous vehicleshas been constructed. Amodular structurewas designed with aset of requirements in mind: • Not having to change anycodeinthe system whenreplacing one sensor (e.g. aspeed sensor that gets data from the wheel encoders) by another sensor(e.g. aspeed sensor that gets its data from aGPS receiver). • Not having to change anycodewhen using different vehicles. • Operating the system on one or more computers, connected by network. • The analysis, design, and implementationshould be object-oriented, to suit the modular design. The system is made up of anumberofsoftware modules, some of whichalso representreal hardware devices.Eachmodule canbeindependently loaded in to the running system, and be logically connected to other mo dules. There arecurrentlysix major categories of modules: Development of an Autonomous Forest Machine for Path Tracking 609 Sensors (represent hardware units that deliver sensor data, e.g. speed, heading, position). Actuators - represent hardware units that control external equipment, e.g. throttle, steering angle, and brakes. Vehicle (several imple- mentations of real and virtual vehicles. See Fig. 2). Control Panels (present data on the screen, or allow the vehicle to be manually controlled). Control- lers (process sensor data and compute control signals for actuators). Proxies and Servers (facilitate transfer of sensor data and control commands over a network). Each module is implemented as one or more classes. The modules are connected primarily by an event-driven system. There is no central control loop running. Instead the system reacts to changes in its environment. For example, a sensor signal arrives and sets up a series of method calls that ultimately leads to a change in the state of the system. Some sensor signals might just update an internal map of the surroundings, while others might stop the vehicle immediately. Other events are timed events, i.e. some action is performed repetitiously. Operator input for pure tele-operation of the vehicle is handled in the same way: a push of a button in the GUI (Graphical User Interface) or a real joystick sets up a chain of events. The Matlab program responsible for path tracking runs on another computer, and is not part of this event-driven system. Instead it polls the sensor system, in a traditional fashion, in its main control loop. A module usually executes in a separate thread, i.e. all the modules run in parallel. Most sensing, control, user interface, and behavior are defined in modules. They communicate with other modules through connections, either by sending commands directly to another module or by listening on other modules. They also have other properties that depend on their actual type, for instance update interval for sensors, and network addresses for servers and proxies. Every module has an associated configuration file with properties that control the module’s behavior. For a sensor, this file may contain e.g. pose, network address, and filter constants. Most of these properties are set once and for all, while others are changed either by the user or by the module itself. The module can also save the changes so they take effect upon the next time the system is run. One example would be an experiment to find the most appropriate filter constant for a specific sensor; when a suitable value is found the sensor can store it in its configuration file. The next time the sensor is run, it automatically uses the saved value. The forest machine system contains more than 60 different initialization files, and to facilitate changes, as well as for providing an overview, a graphical configuration manager has been developed. The configuration manager gives the user a graphical picture of how every module is connected to other modules, and can also be used to modify individual properties for a module. 610 T. Hellstr¨om, T. Johansson, and O. Ringdahl 4 Path Tracking and Obstacle Avoidance To navigate safely through the forest, a new path-tracking algorithm named Follow-the-Past has been developed. Traditional algorithms like Follow-the- Carrot [1] and Pure-Pursuit [2] use position information only, and sometimes run into problems that can be avoided by taking into account additional recor- ded information from a human driver. If the vehicle deviates from the recorded path, for example as a result of avoiding an obstacle, or because of noise in the positioning sensors, the Follow-the-Past algorithm steers like the driver, plus an additional angle, based on the distance to the path. The algorithm is described in the following section. More details and test results are found in [6] and [7]. 4.1 Follow-the-Past Algorithm While manually driving along the path, the orientation and steering angles are recorded together with the position at every moment. The recorded position ( x ,y ), the recorded orientation θ and the recorded steering angle φ are used by three independent behaviors: φ β : Turn towards the recorded orientation θ φ γ : Mimic the recorded steering angle φ φ α : Move towards the path Each behavior suggests a steering angle and is reactive, i.e. operates on the current input values; orientation, steering angle, and shortest distance to the path. φ α uses recorded positions ( x ,y ) and actual position ( x, y ) as inputs. φ β uses recorded orientation θ and actual orientation θ as inputs. φ γ uses the recorded steering angle φ as input. The three behaviors are fused into one action, the commanded steering angle φ t , as shown in Fig. 3. The three independent behaviors φ α , φ β , and φ γ operate in the following fashion: φ β : Turn towards the recorded orientation The angle θ is defined as the recorded orientation at the closest point on the recorded path. This point is called the path point. φ β is computed as the difference between the current orientation θ and the recorded orientation θ : φ β = θ − θ. (1) φ γ : Mimic the recorded steering angle This behavior simply returns the recorded steering angle φ at the path point: φ γ = φ . (2) By using the recorded steering angle, the curvature of the path is auto- matically included in the final steering command. This is a great advantage compared to methods like Pure-Pursuit [2] and Follow-the-Carrot [1]. Development of an Autonomous Forest Machine for Path Tracking 611 Move towards path Turn towards the recorded orientation θ ’ Mimic the recorded steering Φ ’ + (x, y) θ GPS/INS θ ’ (x’ , y ’ ) Φ ’ Φ t w 1 w 2 w 3 Φ α Φ β Φ γ Recorded path Safe speed v ’ v t Φ α Fig.3. Path tracking with reactivecontrol of steering angle φ t . φ α :Move towards the path This behavior is responsible forbringing the vehicle back to the path, if the vehicle deviatesfor somereasonfromthe path. Suchadeviation can be caused by noiseinthe position signal, or by the obstacle-avoidance system. φ α can be computed in manyways, e.g. by the following algorithm (refer to Fig. 4: 1. Determinethe closest pointonthe recorded path (denotedPath Point). 2. Compute aL oo kA headP oin ta taL oo kA headD istance fromt he Pa th Point, in adirection δ, definedasthe sum of therecorded orientation θ andthe recorded steering angle φ at thePath Point, i.e.:δ = φ + θ . 3. CalculateaLookAheadAngle ψ ,defined as the polar angular coordinate for the vector between the vehicle’s current coordinates and the Look Ahead Point. 4. Compute φ α as thedifference between ψ andthe angle δ ., i.e.: φ α = ψ − δ. An alternativemethodtocompute φ α can be found in [6]. Command Fusion The three behaviors φ α , φ β ,and φ γ all return asuggestedsteeringangle, aiminga tfulfilling the goals of therespectivebehaviors. These three values are fused into one value φ t by aweighted addition as shown in Fig. 3. In our tests, all weights have been set to 1. I.e.: φ t = φ β + φ γ + φ α . (3) 612 T. Hellstr¨om, T. Johansson, and O. Ringdahl Path Look Ahead Point Path Point α φ δ Ψ δ l Fig.4. Calculation of φ α ,whichisresponsible for keeping the vehicle on the track. φ t = ψ − δ + φ β + φ γ = ψ − ( φ + θ )+(θ − θ )+φ (4) = ψ − θ. Te sting and Results The dev elop ed algorithm has be en tested bo th in as im ulator for forest ma- chines [11]and on aPioneer robot. In this report we presentonly atest done with the Pioneer rob ot and compare the results to an implemen tationo ft he Pure-Pursuit[2] method.Inthis test,aLook-Ahead Distance =1.2 meter is used.F igure 5(a)s ho ws resultsf or path track ing with the Fo llo w-the-P ast method. The vehicle (thickline) is capable of following arecorded path (thin line) with almost no deviation from the path. As areference, Fig. 5(b) shows howthe vehicle behaveswhen using the Pure-Pursuitmethodunder the same conditions. Thispath hassome sharpturns, which makes it difficult for both Follow-the-Carrot and Pure-Pursuit, as they tend to “cut corners” instead of following ahighly curvedpath.Under normalconditions, thisproblemis avoided by the Follow-the-Past algorithm. To functioninthe forestmachine application,the path-tracking behavior hasbeen combined with VFH+ [13]for obstacleavoidance. The HIMM grid used in the original VFH+ algorithm is replaced by an oc cupancy grid with Bayesian updating. The expression for the fused φ t is: Development of an Autonomous Forest Machine for Path Tracking 613 45 46 47 48 49 1248 1249 1250 1251 1252 1253 [m] [m] (a) Follow-the-Past 45 46 47 48 49 1248 1249 1250 1251 1252 1253 [m] [m] (b) Pure-Pursuit Fig.5. Comparison between a) the Follow-the-Past and b) the Pure-Pursuit path trac king algorithms. Fo llo w-the-P ast is able to follo wt he path almost pe rfectly ,w hile Pure-Pursuit tends to ”cut corners”. The examples above are from testswith the Pioneer rob ot. 5S tatus, Exp eriences andF utureW ork The system has been successfully implemented on the simulator and on the Pioneer robot. The developed algorithm for path tracking performs very well, and the general tools for robot software architectures have been shown to be both powerful andflexible. Sensor tests and system adjustments for the forest machineare in progressand planned to be completed during2005. Thecontinuation of theprojectwill deal with the sensingproblems specific forforestenvironments. Techniques to distinguishobstacles fromground in uneven andhilly environmenthave to be developed.The specific problem with detection of human beings is also of the highest priorityfor afuture productification of an unmanned forestmachine.The future work will also involvedevelopmentofwaystoachieveaccurate localization and estimation of heading without expensive GPS equipment. Acknowledgements This wo rk wa sfi nancedb yT he Kemp eF oundations, VINNOV A, Land Sy- stems H¨agglunds, Carl Tryggers stiftelse, LKAB and Komatsu ForestAB. We acknowledge their support gratefully. The authors would alsoliketothank Ur- 614 T. Hellstr¨om, T. Johansson, and O. Ringdahl ban Sandstr¨o m for his implementation of the occupancy grid and Kalle Prorok for his work with the radar sensors. References 1. M. J. Barton. Controller Development andImplementation for Path Planning and Following in an Autonomous Urban Vehicle .Undergraduate thesis, Univer- sityofSydney,Nov.2001. 2. R. C. Coulter. Implementation of the pure pursuit path tracking algorithm. Technical Report CMU-RI-TR-92-01, RoboticsInstitute, Carnegie Mellon Uni- versity, Pittsburgh, PA,January 1992. 3. F. Georgsson, T. Hellstr¨om, T. Johansson, K. Prorok, O. Ringdahl, and U. Sand- str¨om. Developmentofanautonomous path tracking forest machine-astatus report Technical Report UMINF 05.08, DepartmentofComputing Science, Ume˚a University, 2005. 4. U. Hallonborg. F¨ orarl¨osa skogsmaskiner kanbli l¨ onsamma. Skogforsk RESUL- TAT,(9), 2003. 5. T. Hellstr¨om. Autonomous navigation for forest machines. Technical Report UMINF 02.13, DepartmentofComputing Science, Ume˚a University, 2002. 6. T. Hellstr¨om and O. Ringdahl. Followthe past-apath tracking algorithm for autonomous forest ve hicles. Te ch nical Rep ort UMINF 04.11, Departmen to f Computing Science, Ume˚a University, 2004. 7. T. Hellstr¨ om and O. Ringdahl. Autonomous path trac king using recorded orien- tation and steering commands. In Proceedings of Towards Autonomous Robotic Systems 2005,Imperial College London, England, 2005. 8. R. Kalman. Anew approachtolinear filtering and prediction problems. Tran- sactions of the ASME -Journal of Basic Engineering,82:35–45, 1960. 9. H. M¨ ak el¨ aa nd K. Koskinen. Na vigation of outdo or mobile rob ots using dead reckoning and visually detected landmarks. In ICAR’91 Fifth International Confer enc eo nA dvanc ed Ro bo tics ,p ages 1151 –1 156, June 1991. 10. P. Z. Peebles Jr. Radar Principles .John Wiley &Sons, 1998. 11. O. Ringdahl. Path tracking and obstacle avoidance for forest machines. Master’s Thesis UMNAD454/03, DepartmentofComputing Science, Ume˚a University, 2003. 12. A. Rob otics. Rob ots, AG V’s &r ob otic sensing. http://www.activme dia.c om/ , 27 Jan. 2005. 13. I. Ulrichand J. Borenstein. VFH+: Reliable obstacle avoidance for fast mobile robots. IEEEInt. Conf. on Robotics and Automation,pages 1572–1577, May 1998. 14. K. Westlund and T. Hellstr¨ om. Requirements and system design for arobot performing selectivecleaning in young forest stands. to be publishedinJournal of Terramechanics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ure-Pursuit Fig.5. Comparison between a) the Follow-the-Past and b) the Pure-Pursuit path trac king algorithms. Fo llo w-the -P ast is able to follo wt he path almost pe rfectly ,w hile Pure-Pursuit. events. The Matlab program responsible for path tracking runs on another computer, and is not part of this event-driven system. Instead it polls the sensor system, in a traditional fashion, in its. operates on the current input values; orientation, steering angle, and shortest distance to the path. φ α uses recorded positions ( x ,y ) and actual position ( x, y ) as inputs. φ β uses