Techniques and Algorithms for Autonomous Vehicles in ... - CiteSeerX

Techniques and Algorithms for Autonomous Vehicles in Forest Environment Licentiate Thesis

Ola Ringdahl [email protected] Department of Computing Science Umeå University, Sweden ISSN-0348-0542 ISBN-978-91-7264-373-4 UMINF-07.17 October 17, 2007

ii

Abstract This thesis describes an ongoing project of which the purpose is designing and developing techniques and algorithms for autonomous off-road vehicles. The focus is on some of the components necessary to accomplish autonomous navigation, which involves sensing and moving safely along a user-defined path in a dynamic forest environment. The work is part of a long-term vision in the forest industry of developing an unmanned shuttle that transports timber from the felling area to the main roads for further transportation. A new path-tracking algorithm is introduced and demonstrated as superior to standard algorithms, such as Follow the Carrot and Pure Pursuit. This is accomplished by using recorded data from a path-learning phase. By using the recorded steering angle, the curvature of the path is automatically included in the final steering command. Localization is accomplished by a neural network that fuses data from a Real-Time Kinematic Differential GPS/GLONASS, a gyro, and wheel odometry. Test results are presented for path tracking and localization accuracy from runs conducted on a full-sized forest machine. A large part of the work has been design and implementation of a general software platform for research in autonomous vehicles. The developed algorithms and software have been implemented and tested on a full-size forest machine supplied by our industrial partner Komatsu Forest AB. Results from successful field tests with autonomous path tracking, including obstacle avoidance, are presented.

iii

iv

Preface The work presented in this thesis is in large parts based on the following publications: 1. Thomas Hellström, Pär Lärkeryd, Thomas Nordfjell and Ola Ringdahl. Hel- eller halvautonoma skogsfordon - historia, vision och nuläge. [Fully or semi autonomous forest vehicles - history, vision, and present situation]. Manuscript, 2007. 2. Thomas Hellström, Thomas Johansson, and Ola Ringdahl. A Software Framework for Control and Sensing in Mobile Robotics. Technical Report UMINF 07.05, Department of Computing Science, Umeå University, April 2007. 3. Thomas Hellström, Thomas Johansson, and Ola Ringdahl. Development of an Autonomous Forest Machine for Path Tracking, volume 25 of Springer Tracts in Advanced Robotics, pages 603-614. Springer, field and service robotics: results of the 5th international conference edition, 2006. 4. Thomas Hellström and Ola Ringdahl. Follow the past - a path tracking algorithm for autonomous vehicles. Int. J. Vehicle Autonomous Systems, Vol. 4, Nos. 2-4, pp.216-224, 2005. 5. Thomas Hellström and Ola Ringdahl. Autonomous path tracking using recorded orientation and steering commands. In proceedings from Towards Autonomous Robotic Systems (TAROS05), pages 81-87, London, UK, 2005. 6. Ola Ringdahl. Path tracking and obstacle avoidance for autonomous forest machines. Master’s Thesis UMNAD 454/03, Department of Computing Science, Umeå University, April 2003. In addition to these publications, minor additional contributions to the thesis can be found in [23, 33, 34, 36, 68]. The development of the system described here has involved a number of people. Thomas Hellström has been the project leader. The software framework NAV2000 [31] was primarily developed by Thomas Johansson. The occupancy grid Map2D was developed by Urban Sandström as part of his Master thesis v

project [69]. An early version of the laser-based localization was developed by Arsalan Siddiqui as part of his Master thesis project [71]. The Kalman filter and some initial tests with the gyro were made by Fredrik Georgsson [23]. Kalle Prorok conducted experiments with the Tyco radar as described in [23]. Financial support was granted by Kempestiftelserna, VINNOVA, Komatsu Forest AB, Carl Tryggers stiftelse, LKAB, and BAE Systems Hägglunds AB.

vi

Acknowledgments First and foremost I would like to thank my supervisor Thomas Hellström for his valued help and support over the years. Without him, I would never have had the opportunity to work with this interesting project. Thanks for persuading me to write a Licentiate thesis. I would also like to thank Thomas Johansson for his continued support of the NAV2000 system and for proofreading a part of my thesis; Jürgen Börstler for reading through my thesis and making valuable suggestions on how to improve it; Fredrik Georgsson for his help on neural networks and for proofreading a part of my thesis; Roger Oscarsson, Erik Billing, and Niclas Börlin for proofreading parts of the thesis. Our industrial partner Komatsu Forest AB, especially the guys at the prototype workshop and Dennis and Fredrik, for helping us out when we were performing tests with the forest machine. I would like to thank all of my great colleagues at the department, and also take this opportunity to answer your continued question: “är du ute och leker nu igen?”. The answer is no, I was collecting data for this thesis. Last but not least, I would like to thank my family and friends for always being there for me. Umeå, August 2007 Ola Ringdahl

vii

viii

Contents 1 Introduction 1.1 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Autonomous Ground Vehicles 2.1 History . . . . . . . . . . . . . . . . . 2.2 AGVs today . . . . . . . . . . . . . . 2.3 Sensors . . . . . . . . . . . . . . . . . 2.3.1 Pose sensors . . . . . . . . . . . 2.3.2 Satellite navigation systems . . 2.3.3 Obstacle sensors . . . . . . . . 2.3.4 Sensor fusion . . . . . . . . . . 2.4 Propulsion . . . . . . . . . . . . . . . . 2.5 Path planning . . . . . . . . . . . . . 2.6 Path tracking . . . . . . . . . . . . . . 2.6.1 Vector Pursuit . . . . . . . . . 2.6.2 Follow the Carrot . . . . . . . 2.6.3 Pure Pursuit . . . . . . . . . . 2.7 Obstacle avoidance . . . . . . . . . . 2.7.1 Potential Fields . . . . . . . . . 2.7.2 Vector Force Field (VFF) . . . 2.7.3 Vector Field Histogram (VFH) 2.7.4 VFH+ . . . . . . . . . . . . . 3 Development of an Autonomous 3.1 Introduction . . . . . . . . . . . 3.1.1 Relation to earlier work 3.1.2 System requirements . . 3.1.3 System design . . . . . . 3.1.4 Computers and software 3.2 Vehicles . . . . . . . . . . . . . 3.2.1 Simulator . . . . . . . . 3.2.2 Pioneer . . . . . . . . . 3.2.3 Valmet 830 . . . . . . . 3.3 Definitions . . . . . . . . . . . . ix

1 2

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

3 3 4 7 7 9 10 13 14 15 16 17 17 17 21 22 22 23 23

Forest Machine . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

35 35 36 37 38 39 41 41 42 42 43

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

CONTENTS 3.3.1 Steering angle, heading, and orientation . . . 3.3.2 Coordinate system . . . . . . . . . . . . . . . 3.4 Software platform . . . . . . . . . . . . . . . . . . . . 3.4.1 Introduction . . . . . . . . . . . . . . . . . . 3.4.2 Motivation . . . . . . . . . . . . . . . . . . . 3.4.3 Design criteria . . . . . . . . . . . . . . . . . 3.4.4 Modules . . . . . . . . . . . . . . . . . . . . . 3.4.5 Inter-Module Data Flow . . . . . . . . . . . . 3.4.6 Communication . . . . . . . . . . . . . . . . . 3.4.7 Configuration . . . . . . . . . . . . . . . . . 3.4.8 Health Monitoring and other support systems 3.4.9 Results . . . . . . . . . . . . . . . . . . . . . 3.5 Sensors and sensor data processing . . . . . . . . . . 3.5.1 Position sensors . . . . . . . . . . . . . . . . . 3.5.2 Heading sensors . . . . . . . . . . . . . . . . . 3.5.3 Obstacle sensors . . . . . . . . . . . . . . . . 3.5.4 Sensor fusion . . . . . . . . . . . . . . . . . . 3.6 Path tracking . . . . . . . . . . . . . . . . . . . . . 3.6.1 Follow the Past . . . . . . . . . . . . . . . . . 3.6.2 φβ : Turn towards the recorded orientation . . 3.6.3 φγ : Mimic the recorded steering angle . . . . 3.6.4 φα : Move towards the path . . . . . . . . . . 3.6.5 Command fusion . . . . . . . . . . . . . . . . 3.6.6 Finding the nearest point on the path . . . . 3.6.7 Testing procedure . . . . . . . . . . . . . . . 3.6.8 Results . . . . . . . . . . . . . . . . . . . . . 3.7 Obstacle avoidance . . . . . . . . . . . . . . . . . . 3.8 Putting it all together . . . . . . . . . . . . . . . . . 3.9 Summary and conclusions . . . . . . . . . . . . . . . 3.10 Future work . . . . . . . . . . . . . . . . . . . . . . . A Test of different GPS-systems

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

43 43 45 45 48 48 52 55 56 57 58 61 62 63 74 83 87 96 96 96 98 98 101 102 105 105 115 115 118 120 121

x

Chapter 1

Introduction The forest industry has a long-term vision of developing an unmanned shuttle that transports timber from the felling area to the main roads for further transportation [28]. The main advantages with such a solution are lower labor costs and less ground damages and emissions due to the lower weight of an unmanned vehicle (the cabin alone weighs several tons). Unmanned vehicles in off-road use have been for long an active area of research and development. Mostly the research is for specific problems and has resulted in very few commercial applications or complete systems. The presented work is part of the project Autonomous Navigation for Forest Machines within the IFOR research center at Umeå University. The purpose of the project is design and development of techniques and algorithms for off-road vehicles. The focus of this thesis is on some of the components necessary to accomplish autonomous navigation, which involves sensing and moving safely along a user-defined path in a dynamic forest environment. A large part of the work has been design and implementation of a general software platform for research in autonomous vehicles. In addition, methods for this specific application have been developed, such as the path-tracking algorithm Follow the Past. Several techniques for localization in addition to a GPS have been developed, such as laser-based localization, and wheel odometry enhanced with neural networks. The initial design and ideas underlying the project reported in this thesis are described in [35]. The developed hardware and software have been installed on a Valmet 830 forest machine (a forwarder) supplied by our industrial partner Komatsu Forest AB. The resulting system is able to autonomously drive along a previously learnt path, while avoiding any obstacles in the way. We present results from successful autonomous path tracking of a previously learnt path, and also the way obstacle avoidance is integrated. 1

Introduction

1.1

Outline

Chapter 2 contains a survey on work done in the field of Autonomous Ground Vehicles. It also covers some central concepts in this field as well as sensors and algorithms commonly used, with emphasis on applications in forestry. In Chapter 3, the work done in our project is presented in more detail. System and software design, the sensors used as well as evaluation thereof are described. A large part is about techniques for using data from several sensors simultaneously to increase the accuracy of the sensor readings. A new path-tracking algorithm, Follow the Past is also presented in this chapter. Field tests in various environments, conclusions, and future work finalize the thesis. Appendix A contains tests done on the GPS system and also some arguments for having chosen this particular system over others.

2

Chapter 2

Autonomous Ground Vehicles

Work on autonomous vehicles for indoor use has been conducted since the late 1960s (mostly different sorts of robots), but in later years applications for outdoor use, referred to as Autonomous Ground Vehicles (AGV), have been developed. These applications are mostly in military reconnaissance, automated machines for agriculture, and mining, as well as construction and automation equipment for personal cars. Some tasks can be hard to render fully autonomous and may require some kind of supervisory or semi-autonomous control. This can be done in two different ways [54]. In continuous assistance systems, an operator delegates a (sub-) task to the AGV and only has to monitor ensuring that nothing goes wrong. If the vehicle is unable to complete the task, the operator can take over and remote-control it to overcome the difficulties. The other approach to semi-autonomous control is control trading. Much like in continuous assistance, the operator assigns a task to the AGV, but does not have to monitor the vehicle while it performs its task. This enables the operator to control several vehicles simultaneously because he/she only has to give new orders when a task is done and can then leave the AGV to its work. The two major reasons for developing autonomous vehicles found in most works about AGVs are cost efficiency (less labor cost) and safety (e.g. in hazardous environments). Our contribution to the research of AGVs is in the automation of the forest industry, as described further in Chapter 3. The remainder of this chapter contains a survey of previous work done on AGVs.

2.1

History

The history of AGVs is not as old as the general robotic applications. The works most commonly referred to as groundbreaking in this field are ALVINN [64] and Navlab 5 [65], which are described below. 3

Autonomous Ground Vehicles ALVINN In 1993 Carnegie Mellon University [64] developed ALVINN (Autonomous Land Vehicle In a Neural Network), an autonomous road-following vehicle based on neural networks. As input to the neural net, an image array of 30x32 pixels was used. The output was quantized into 30 discrete units, from sharp-right to straight-ahead to sharp-left. To train the neural network, a human driver first took the vehicle along the road. After training, the system was able to follow the road types and conditions it encountered. ALVINN ran successfully up to 145 kilometers without human intervention on highways and has also been successfully used on dirt and paved roads of various types. Navlab 5 In 1995 Carnegie Mellon University used a new vision-based road follower [65] to autonomously drive almost 4800 kilometers. Autonomous visual steering was used for 98.2 percent of the trip from Pittsburgh to San Diego, averaging speeds about 25 kilometers per hour.

2.2

AGVs today

The research in AGVs has come a long way and in recent years, many applications have been presented. In this section, some work on state-of-the-art AGVs is presented. AutoStrad One of the most advanced commercial AGVs to date is the AutoStrad (Automated Straddle Carrier) in Brisbane, Australia [25]. It is developed in cooperation between the machine manufacturer Kalmar Industries and Australia’s largest operator of container terminals, Patrick Corporation. This vehicle is capable of picking up, carrying, and placing shipping containers, allowing movement of containers from vehicles on ground to holding yard, to quay cranes and back see Figure 2.1. The on-board computers control engine and wheel speed, steering angle, and container hosting. The navigation system consists of a precision GPS in parallel with a millimeter-wave radar (MMWR). Paths and container movement tasks are generated remotely by the port central control system. The vehicle pilot system maintains a map of the terminal area to determine which places are passable and to implement traffic management systems to avoid other carriers (there are currently 14 AutoStrads working in the Brisbane port). The control system is able to autonomously control the vehicle in speeds up to 25 km/h. The only thing still done manually is loading and unloading containers on trucks. The reason is the difficulty to determine exactly where to place the container on the vehicle, and that different vehicles have different arrangements to mount and hold containers. The AutoStrad is in full commercial use and not only a research project as many other projects are. An advantage 4

2.2 AGVs today to using autonomous straddle carriers is improved efficiency (an autonomous vehicle can operate 24-hours a day in virtually any conditions). The AutoStrad can also optimize container movements and storage requirements, thereby maximizing productive use of port infrastructure. The system can be controlled by one operator at a control center on site, thereby minimizing labor cost.

Figure 2.1: AutoStrad (Autonomous Straddle Carrier) moving containers in the port of Brisbane, Australia. Photograph courtesy of Kalmar Industries.

Vision-Guided Mechanical Weed-Control The world-wide problem of environmental pollution caused by excessive use of herbicides has motivated the development of an autonomous mechanical weedcontrol vehicle in Sweden [76]. The vehicle uses a color camera to recognize plants in a field and distinguish them from weeds. It is able to follow a row of plants while mechanically weeding in between them. The robot is also planned to be able to recognize the end of the rows, turn, and find the next row autonomously. By replacing chemical weed control by mechanical weeding, the amount of herbicides used for crop protection in agriculture is reduced without having to have workers do the weeding, thereby allowing more cost-efficient growing of ecological crops. ”Besten” - The Beast A work that has gotten much attention in the forest industry lately is a machine called ”Besten” (or The Beast) [8]. The system consists of a forwarder remotecontrolling an unmanned harvester that cuts down the trees and puts them directly on the forwarder, which then unloads at the nearest road. This is 5

Autonomous Ground Vehicles not really a fully autonomous machine, but it could be a step on the way to developing one. Normally two forwarders take turn in controlling the harvester. While one unloads at the nearest road, the other one can harvest trees. The purpose of having an unmanned harvester is to save money, both on labor cost and the machine itself. Autonomous Weeder for Christmas Trees In Denmark, an Autonomous Weeder for Christmas Trees has been developed [30]. They adopted an ordinary lawn mower to weed Christmas tree plantations, where tree plants grow in straight rows approximately 1.2 meters apart from each other. To navigate through the plantation every tree plant’s position was measured with a GPS. This way a map that covers the whole plantation was put together, and a program was used to plan a route for the vehicle. A weed cutter was placed on a swing arm controlled so it should cut between trees, and as close to the tree plants as possible, without damaging them. This was done solely by knowing the position of each plant and the vehicle, being equipped with a GPS receiver. No other sensors were used to control the cutter arm or vehicle. A problem was discovered when the vehicle rolled much by driving on uneven ground. Because the GPS-antenna was mounted on a pole on top of the vehicle, the position of the antenna changed more than the vehicle had actually moved, resulting in a misreading of the vehicle’s position. To remedy this, a gyro was mounted on the vehicle, and the position returned from the GPS was corrected with the readings from the gyro. This resulted in a slightly better performance, but the gyro was too noisy to give an accurate enough reading of the vehicle’s roll. Some initial tests were done with a laser scanner to detect the trees and potential obstacles on the path. It was concluded that the scanner should be suitable for both obstacle detection and the tree-row location, thus making the vehicle navigation more robust. The conclusions of the project were that it is feasible to build a small autonomous machine for weeding Christmas trees but further work is needed to make the application safer and more reliable. DARPA Grand Challenge The Grand Challenge [38] is an off-road robot competition devised by DARPA (Defense Advanced Research Projects Agency) to promote research in the area of autonomous vehicles. The challenge consists of building a robot capable of navigating 280 kilometers through desert terrain in less than 10 hours, with no human intervention. The first competition was held in 2004, but none of the 15 participating robots managed to navigate more than 5% of the entire course. The Challenge was repeated in 2005 and this time 5 of 23 participants managed to complete the whole course. The winner was Stanford’s robot Stanley [79] making it in just under 7 hours. The path was defined by a number of GPS coordinates (lat-long) and corresponding width of the path. This meant that the teams did not have to do any global path planning, as the path was already defined by DARPA. The Stanley robot was a modified four-wheel-drive car 6

2.3 Sensors with many sensors to perceive the environment and estimate the position of the vehicle. To be able to scan the environment in front of the car, they used 5 SICK laser range-finders. The lasers measured cross-sections of the approaching terrain at different ranges up to 25 meters in front of the vehicle. The vehicle was also equipped with a color camera for long-range road perception. For long-range detection of large obstacles, Stanley also held two 24 GHz radar sensors to cover the frontal area up to 200 meter, with a coverage angle in azimuth of about 20 degrees. The Stanford team used an online path planner to decide the optimal path inside the corridor given by DARPA. This was done by applying a search algorithm that minimizes a linear combination of continuous cost functions, subject to a fixed vehicle model. The cost functions penalize running over obstacles, leaving the path corridor, and the lateral offset from the current trajectory to the sensed center of the road surface. The conclusions of Stanford’s team were that, though the DARPA Grand Challenge was a milestone in the quest for self-driving cars, a number of problems still need to be solved to get reliable autonomous vehicles. The most important one is that the race environment was entirely static. To be able to construct autonomous cars, they must be able to perceive and interact with moving traffic. Even when driving in static environments, the Stanley robot can only handle limited types of obstacles. It cannot distinguish between a rock and tall grass, for example.

2.3

Sensors

Perception is a key part of an autonomous system. The most common and relevant sensors for AGVs are presented in this section. The sensors are divided into three different groups; pose (position and heading), satellite navigation, and obstacle sensors. A large part of the survey in this section is taken from the preliminary study of the IFOR-project Autonomous Navigation for Forest Machines [35]. In Section 3.5, the specific sensors used in our project are presented.

2.3.1

Pose sensors

The sensors presented in this section are the ones most commonly used for determining the pose (position and heading) of a vehicle. Compasses measure the orientation of Earth’s magnetic field (most often only the horizontal component) relative to the vehicle, and hence are used to estimate the heading of the vehicle. Typical techniques are: gyro-compasses that use gyroscopes pointing toward the magnetic north pole and flux-gate compasses that use a toroidal magnet suspended in Earth’s magnetic field. This kind of sensor is highly susceptible to local variations in the ambient magnetic field. Calibration is therefore essential. Also, compasses are known to be of limited value close 7

Autonomous Ground Vehicles to large metal objects and machine equipment. An alternative is to use a gyro, which provides a relative measure of the vehicle’s attitude. Inclinometers are used to measure the orientation of the gravity vector relative to the vehicle. In their simplest form they can be implemented as mercury switches. More sophisticated types measure the tilt and skew of the vehicle in degrees. Typical error is less than 0.5 degree. This sensor type is highly essential, since it can avoid serious disasters that would otherwise be unpredictable for the autonomous vehicle. Inertial navigation system (INS) measures derivatives of the robot’s position by gyros and accelerometers. Most equipment contains 3-axis pitch/roll/yaw gyros and 3-axis pitch/roll/yaw accelerometers. In this way attitude and position can be calculated. INS systems are prone to internal drift problems and must be periodically re-calibrated for full accuracy. INS systems are often used as a supplement to other position sensors, such as GPS. Wheel encoders (shaft encoders) sense the rotation of the steering shaft and wheel axles and convert these to change in orientation and position. Because of wheel slippage, the accuracy of this method can be quite poor. Therefore, wheel encoders are often used for odometry as a backup or supplement to other navigational sensors, such as GPS and INS. Examples of combining GPS with odometry can be found in [22]. A vehicle for automated harvesting combines GPS, INS, and odometry in [63]. The normal way to perform this sensor fusion is by Kalman filtering [42], a stochastic method known to compute the optimal linear predictor from two or more signals. In [29] a deterministic approach is combined with Kalman filtering for sensor fusion of odometry and inertial sensors. This approach is reported to improve the position estimates significantly compared to a pure Kalman filter approach. Laser-based localization The general idea for most laser-based localization techniques is to compare two or more laser scans taken from different viewpoints, but covering, at least partly, the same objects in the environment. By comparing the scans, the change in robot pose (position and heading) can be estimated. A number of algorithms for this have been proposed. Bailey and Nebot [3] developed a method based on matching identified landmarks, such as cylinders and corners. The IDC (Iterative Dual Correspondence) [48] is an iterative algorithm consisting of two steps. First it finds corresponding points in the reference scan with points in 8

2.3 Sensors the new scan. The second step is to do a least square minimization of all pointto-point distances with respect to translation and rotation. These two steps are then iterated until a solution is found. Bengtsson and Baervelt [7] developed this method further and created the sector-based variant, IDC-S. This method showed much better performance compared to the original IDC algorithm, especially in environments with large changes. Selkäinaho [70] proposed an efficient pixel-based matching method that works in unstructured outdoor environments. Similar techniques have been used in indoor environments [44, 3]. The techniques for estimated pose changes can be used in two major ways. For relative localization, the scans are generated with a very short time difference, and represent two vehicle positions very close to each other. The change in vehicle pose is estimated by finding the optimal transformation (translation and rotation) that makes the two scans match each other. The technique is often referred to as laser odometry, and, just like ordinary wheel odometry, it suffers from accumulation of errors. For global localization, the vehicle records a database with reference scans during one or more passages along a fixed route. For localization, the current laser scan is compared to the database, to find the best matching one(s). In this way the vehicle’s position is estimated relative to the positions at which the reference scans were recorded. This corresponds to a global localization. There is no accumulation of localization errors in this method, since the recorded reference scans are used in each step of the localization.

2.3.2

Satellite navigation systems

Today, two different satellite navigation systems are available; the American GPS (Global Positioning System) and GLONASS (Global Navigation Satellite System) from Russia. These two systems are quite similar, and many GPS receivers are able to use the GLONASS signals as well, to increase the accuracy of the position estimation. The main difference is that GLONASS have fewer operational satellites, but Russia is committed to fully restore the system by 2011. Currently (May 2007) 11 of 24 satellites are operational [39]. An advantage of GLONASS is better coverage at high latitudes. A new system, Galileo, is under development in Europe but not yet in operation. GPS (Global Positioning System) This is the most accurate position sensor today. Each GPS satellite sends out two L-band carrier waves (L1, L2) with ranging codes modulated on them (Pseudo Random Noise, PRN) [15]. A standard GPS receiver calculates an estimate of the vehicle’s position by measuring the differences in time-of-flight for the PRN-signals from 3 to 12 geostationary satellites. Velocity and heading estimates are also possible to compute, based on Doppler effect of the signals from the satellites. The velocity measurement accuracy is about 0.1 m/s. Basic GPS receivers have a position accuracy of around 15 meters 95% of the time (100 meters if Selective Availability is on). An extended technology is DGPS 9

Autonomous Ground Vehicles (differential GPS) which has around 0.5 meters accuracy [18]. This technique utilizes two receivers; a base station located at a known position, and a rover receiver placed on the vehicle. The base station is able to calculate the position error in the GPS-signal it receives, and can send correction data to the rover receiver [15]. To get an even higher accuracy of the position, Real Time Kinematics, RTK, can be used. This is an extension of DGPS where the two receivers use not only the the ranging codes, but also the carrier waves which, due to the shorter wavelengths, can be measured down to millimeter precision. To calculate the distance to each satellite, the number of full periods of the carrier wave from the satellite to the receiver is required. This is referred to as solving the integer ambiguity, or getting a fix solution. In addition to this number the fraction of the last period, given by the phase of the wave, has to be calculated [85]. A Real Time Kinematic DGPS is capable of delivering positions with errors between 2-20 cm and headings with errors of less than 0.1 degree [57, 66]. Regardless of the GPS type, the technology has limitations that make a GPS system insufficient as the single position sensor for an autonomously moving robot. The most common problems involve obstruction of line-of-sight to satellites, multi-path problems and active jamming from other RF sources [18]. Therefore, a GPS system is often combined with INS or odometry.

Galileo A new satellite navigation system, called Galileo, is under development in Europe. The European Commission (EC) first presented plans for a European Galileo satellite navigation system in 1999. A four-phase development is planned with funding from both public and private sectors. Galileo is designed for both civilian and governmental purposes and the system will be controlled and operated by civil management. Galileo will be composed of a constellation of 30 satellites (compared to 24 for the GPS-system) and will be inter-operable with both GPS and GLONASS enabling a user to measure a position with the same receiver from any of the satellites in any combination. The satellites will have an inclination of 56 degrees (55 for GPS, 64.8 for GLONASS) in three orbital planes, as illustrated in Figure 2.2. By offering dual frequencies as standard, Galileo will deliver real-time positioning accuracy down to a meter range (compared to around 15 meters for an ordinary GPS) according to the European Space Agency, ESA. In late December 2005, the first Galileo satellite was launched into orbit to demonstrate and test key technologies and systems. The system is scheduled to be fully operational by 2008.

2.3.3

Obstacle sensors

To avoid driving into obstacles, we have to determine whether there are any obstacles nearby, and if so, where they are. In this section some of the most common sensors used for obstacle detection are presented. 10

2.3 Sensors

Figure 2.2: Deployment of the European Galileo satellite positioning system. The system is scheduled to be fully operational by 2008. Image courtesy of the European Space Agency, ESA.

Machine Vision utilizes cameras mounted on the vehicle to provide information about the environment. Many reported applications perform classification tasks with the image data, see for example [24]. In harvesting machines it is important to distinguish between crop and soil areas in the images. Camera data is used to detect uncut regions of crop in [59, 60] by color segmentation and classification based on histograms representing the conditional probabilities for cut and uncut pixels.Examples of image analysis for tree identification of forest inventory can be found in [13]. Stereo cameras make it possible to compute distances to objects. However, stereo vision requires carefully calibrated cameras. This is generally a tough procedure that requires investing time and effort. Stereo perception is often used to build 3-dimensional maps of the environment for proposed lunar rovers, e.g. in [47].Batavia and Singh [6] describe an obstacledetection methodology that combines two complementary methods: adaptive color segmentation, and stereo-based color homography. Homography is described as the poor man’s stereo. Although computationally cheap, it does not provide true depth information, but rather information about whether a particular image feature rises above the ground plane. In combination the two methods are claimed to work well, particularly for environments where the terrain is relatively flat and of roughly the same color.Machine vision can also be used for localization of the vehicle. A technique inspired by the view-based approach is described in [50]. Image data is saved in a database during path recording, and new images are matched against the database during the actual path tracking. In this way the position and attitude of the vehicle can be 11

Autonomous Ground Vehicles estimated. Ultrasonic sensors/sonars determine distance to objects by transmitting a short pulse of ultrasound from the sensor. A receiver detects the reflection off objects ahead of the sensor, and the distance can be computed by measuring the time that elapsed between emission and reception. The sensitivity of a sonar sensor is cone shaped (typically 15-30 degrees cone), which means that the exact position of the object is unknown. However, the technique is very effective for detection of free space, and is commonly used for obstacle detection in indoor robotics. Another problem with sonars is specular reflections, which occur when a sonar beam hits a smooth surface at a shallow angle, and is therefore not reflected back to the robot, but outwards. Only when an object further away reflects the beam, is a reading obtained, indicating a larger free space than actually exists [55]. Typical maximum range with full accuracy for this kind of sensor is 5 meters (product information ActivMedia). Infrared reflex sensors are most typically used for distance measurements by transmitting a modulated infrared light pulse and measuring the intensity of the reflection from obstacles nearby. In practice, infrared sensors can only be used for detection of objects, not for range measurements [55]. Our tests show that they are very sensitive to the reflectivity of the objects sensed and the angle to them. Another problem we found was that the sensors interfere with each other when placed close together making the measurements very noisy. Infrared detectors measure the infrared emission from a human body. This kind of detector is often used in intruder alarms and could be used as an indication of human presence near a vehicle. Laser range finders also known as Ladar or Laser scanners. They work by the same principles as ultrasonic sensors with the important difference of using near infrared light instead of ultrasound. Instead of measuring the elapsed time between emission and reception, some laser scanners measure the phase shift of an amplitudemodulated laser signal and use it to compute the distance. The resolution and range are greatly improved compared to ultrasonic sensors and the problem with specular reflections is also much less pronounced. Typical accuracy is ±50mm for a 180-degree scan with a resolution of 0.5 degree. Total range is between 50 and 150 meters (product information for SICK laser scanners). A major limitation with this kind of sensors is the high price. This has restricted their development and effective use in AGV systems [18]. Another problem is the 12

2.3 Sensors sensitivity to dust, rain, and snow, although there exist filters to reduce this sensitivity. Millimeter wave radar promises performance not degraded by environmental conditions such as dust, rain, and snow [72, 41, 19]. It is often considered to be a better alternative than Laser range scanners for outdoor automation. In [16], Clark presents an analysis of how radar signals interact with common objects in typical field robotics locations. This leads to the development of natural feature-extraction techniques, enabling suitable navigation markers to be identified from the radar measurements. The way identified features may be compiled into a map of the operating area is also described. Tactile/bump sensors create a sense of touch, and are often used as collision avoidance sensors of last resort. They can either be treated as normal sensors or be wired directly into the low-level control of the vehicle propulsion so as to cause the vehicle to immediately halt.

2.3.4

Sensor fusion

Sensor fusion is a term used to describe any process that combines information from multiple sensors into a single percept [54]. The major reason for merging data from several sensors is that the readings from one sensor are too noisy to give reliable data. Adding a second sensor can give a ”second opinion” or vote on the percept. There are three different strategies for behavior-based sensor fusion [54] (refer to Figure 2.3): • Sensor fission: Each behavior is connected to its own sensor as in part a) of Figure 2.3. In this system, the vehicle would react directly to the input of a single sensor. The path-following algorithm Follow the Past that we have developed (see Section 3.6) is of this type. In that algorithm three different behaviors react on data from different sensors and based on this suggest one behavior (a steering angle) each. This is then fused into a single behavior, the resulting steering angle for the vehicle. • Action-oriented sensor fusion: Data from several sensors are fused into one single percept that the behavior can then react on, see part b) in Figure 2.3. An occupancy grid for fusing obstacle sensors, as described in Section 3.5.4, belongs to this paradigm, as data from several obstacle sensors are fused into one single percept that the algorithm (behavior) for obstacle avoidance then can react on. • Sensor fashion: In this strategy a sensor selector is used to select the best sensor of several at the current circumstances as in part c) of Figure 13

Autonomous Ground Vehicles 2.3. An example of this is that we use GPS as a primary position sensor and wheel odometry for backup. It is used long as the GPS has a good accuracy. If the GPS accuracy gets too low (for example under a large tree that covers the GPS antenna) we switch to the wheel odometry until the GPS accuracy recovers. This system is described in more detail in Section 3.5.4.2. Of course, these strategies can be combined to create new fusion behaviors.

sensor sensor sensor

percept percept percept

behaviour

action

behaviour

action

behaviour

action

Combination action mechanism

a. sensor sensor sensor

percept percept

percept

fusion

behaviour

action

percept

sensor sensor sensor

percept percept percept

Sequence selector

b.

percept

behaviour

action

c. Figure 2.3: Three strategies for sensor fusion [54]: a.) Sensor fission, b.) Action oriented sensor fusion, c.) Sensor fashion.

2.4

Propulsion

Several different types of propulsion methods are used for AGV:s. They can be divided into four groups [74]: 1. Wheeled vehicles: The most common type of propulsion method, based on the motion of a rolling wheel. Many different types of steering have 14

2.5 Path planning been developed, such as differential steering, skid steering, synchronous drive, tricycle drive, and Ackerman steering (car drive). Differential and skid steering both rely on different velocities of left and right wheels to steer. The difference is that skid steering works with four wheels or more, while differential steering only works with two wheels. Synchronous drive means that both front and rear wheels can be turned for better maneuverability. 2. Tracked vehicles: Vehicles that steer by moving the tracks at different speeds. The advantage of tracked vehicles is the ability to move through rough terrain, but they can have quite a destructive effect on the ground, especially when turning. 3. Limbed vehicles: Limbed propulsion is inspired by the insects’ way of moving. The advantage of this method would be the ability to move through very rough terrain and over obstacles that even tracked vehicles can have a hard time going over. Less ground damage is another advantage of this kind of propulsion. However, it has proved to be very complex to get a legged vehicle to work, due to problems in stability maintenance, limb control and placement. 4. Miscellaneous propulsion methods: This category includes methods that do not fit into the other three categories. One method that has become popular lately is sphere-shaped robots. These robots typically move by shifting the sphere’s center of gravity through moving a weight inside them [77]. A typical field of use for this kind of robots is as guarding robots, for example along the perimeter of a large complex. The reason that wheeled vehicles are the most popular can be ascribed to the fact that many wheeled robots are commercially available by many manufactures. Another common method to build a wheeled AGV is to simply convert an ordinary outdoor vehicle into an AGV.

2.5

Path planning

A purely reactive vehicle is able to move around in the world without colliding into obstacles, but it has no way to know where it should go to reach the goal other than to just wander around till it reaches it. To be able to move a vehicle from start to goal in a safe and efficient way, a planned path is required between these points. Path-planing techniques are usually divided into two categories [54]: • Topological Path Planing A path is planned between several landmarks that the vehicle has to recognize. This method is usually used indoors because it is easier to perceive the landmarks (for example an intersection in a corridor). 15

Autonomous Ground Vehicles • Metric path Planning Instead of using landmarks, this planning technique utilize waypoints, represented by (x,y)-coordinates. These coordinates could be measured using a GPS for example. The vehicle often builds a map of the world by itself, by driving around in the environment while recording sensor data and generating a map from it. In the case of a forest machine, it would be better to get the coordinates for the waypoints from a geographical map. Another source of information could be an aerial photo of the area. The problem with using a geographic map for path planning is that it is difficult to plot a path really traversable for the vehicle. In a forest there could be obstacles such as fallen trees or large boulders that do not show on the map, and the ground could be mire, for example. Irrespective of the source, a representation of the world must be used that enables a search for the optimal path. The most popular representation is a regular grid. Many solutions have been suggested for solving the problem of partially known environments. Lumelsky [40] assumes the environment has no obstacles initially and just plans a path straight between start and goal. If an obstacle appears in front of the vehicle, it moves around it and then continues straight towards the goal again. Stentz [75] developed a more optimal way of planning, called D*. The planner begins with an a priori map (the world map known from the start) and computes the optimal path from every location to the goal. This is implemented by doing in advance an A* search from each possible location to the goal. This means that if the vehicle has to go around an obstacle, it knows the optimal path to the goal from the new position. The algorithm also re-plans continuously as the vehicle moves along in the world, and adapts from changes in the environment not on the a priori map.

2.6

Path tracking

The aim of path tracking is to follow a predefined path, represented by a series of (x,y) coordinates (points) joined by line segments. A variety of more-orless advanced techniques has been developed to solve this problem, mostly for small two-wheeled robots or car-like vehicles. In more complex techniques, all aspects of a vehicle performance (for example, brake, throttle, and steering response, vehicle roll, and tire slippage) is controlled. For low-speed applications simpler control strategies are often used, where simplifying assumptions of the vehicle’s dynamics can be made. A forest environment is very complex, and many things can affect the way the vehicle will behave in a certain situation (for example, ground conditions, the load of the vehicle, and ambient trees). Before developing a path follower for articulated vehicles, it is important to choose the proper navigation point; the point of reference by which the vehicle’s position is determined. According to Mäkelä [49] this choice is critical for controlling an articulated vehicle. Choosing the articulation link as navigation point has been considered improper by many authors. The reason is that if the navigation point 16

2.6 Path tracking is outside the path, a correction will temporarily deviate even further when turning towards the path, which makes it difficult to get a stable controller. Another possible choice is the center of the rear wheel axle, which produces an even less stable controller. In his thesis, Mäkelä concludes that selecting the center of the front wheel axle as navigation point is the best choice when driving forward. Three path-tracking methods are presented in this section: Vector Pursuit, Pure Pursuit and Follow the Carrot. We have implemented the two latter ones and compared them to our new path-tracking method, Follow the Past. The results are presented in Section 3.6.

2.6.1

Vector Pursuit

An example of a more advanced path-tracking technique is the Vector-Pursuit method [84], based on the theory of screws [4]. This theory (introduced by Sir Robert Ball in the year 1900), describes the instantaneous motion of a rigid moving body (such as a car-like robot) relative to a given coordinate system. This means that the Screw theory can be used to represent the motion of a vehicle, (i.e. a rigid body), from its current position and orientation to a target position and orientation in a point on a given path (the look-ahead point). The advantage of this method is that it considers not only the position of the lookahead point, but also ensures that the vehicle arrives there with the correct orientation and steering angle (with regard to the orientation and curvature of the path at the look-ahead point).

2.6.2

Follow the Carrot

In Follow The Carrot [5], a carrot point is selected on a Look-Ahead Distance on the path, and then the vehicle is steered directly toward that point, see Figure 2.4. The analogy is that you hold a carrot in front of the vehicle and try to make it reach the carrot through steering toward it. With the notation in Figure 2.4, the steering angle φ is calculated by: φ = ψ − θ

(2.1)

where: ψ : Look-Ahead angle θ : Current orientation of the vehicle

2.6.3

Pure Pursuit

The Pure-Pursuit method [1, 82] is an improvement of Follow The Carrot. The main difference between them is that instead of steering directly towards the carrot point as in Follow the Carrot, a circular arc is fitted between the vehicle and the carrot point in Pure Pursuit, as illustrated in Figure 2.5. This circle is uniquely defined by adding the condition that the heading of the vehicle should be along the tangent line of the turning circle. Algorithm 1 describes how the steering angle is derived. The Pure-Pursuit method enables a smoother steering 17

Autonomous Ground Vehicles Look Ahead Point

Look Ahead Distance

Path

φε φ 2

ψ

Path Point

θ

Figure 2.4: Follow the Carrot calculates a steering angle directly towards the look-ahead (carrot) point, refer to Equation 2.1. control, and improves the vehicle’s ability to handle curved paths with lesser short-cuts than in Follow the Carrot. The reason is that the vehicle normally moves along circular arcs when turning, rather than along straight lines, hence trying to follow a circular arc is more natural than a straight line. An analogy is a driver of a car who looks at a point further ahead on the road and then tends to steer smoothly toward that point, thereby driving in circular arcs on a curved road. The derivation of the curvature γ and radius r of the arc connecting the carrot point and the current position of the Vehicle Center Point (xv , yv ) follow the principles in [17]: Consider Figure 2.6. The point (xc , yc ) is on a Look-Ahead Distance D from the current position of the vehicle. The following two equations can be easily derived. The first is from the geometry of the right triangle in Figure 2.6, and the second from the summing of line segments on the x-axis. x2c + yc2 = D2 ∆x + d = r.

(2.2) (2.3)

Equation 2.2 describes the circle of radius D about (xv , yv ), that defines all possible carrot points (xc , yc ) for the vehicle. Equation 2.3 describes the relationship between the radius of the arc that joins (xv , yv ) and the carrot point (xc , yc ), and the x-offset of the carrot point from the vehicle, ∆x. This 18

2.6 Path tracking

Look Ahead Point

γ Path

Look Ahead Distance

Path Point

Figure 2.5: Pure Pursuit fits a circular arc (γ) between the vehicle and the look-ahead point. The steering angle is then derived from the curvature of this arc. Algorithm 1 Pure Pursuit 1. Determine the current location of the vehicle. 2. Find the point on the path closest to the vehicle. This is done for calculating where the carrot point is (refer to Section 3.6.6). 3. Find the carrot point. As in Follow The Carrot this point is on a constant Look-Ahead Distance from the path point (refer to Section 3.6.6). 4. Transform the carrot point and the vehicle location to the vehicle’s coordinates. The equations to calculate the curvature are only valid in the vehicle’s coordinates, therefore all angles and coordinates must be expressed with respect to the vehicle’s current position and orientation, see Figure 2.6. 5. Calculate the curvature, γ, of the circular arc, see Equation 2.7. 6. Determine the steering angle, φ. The steering angle is derived from the radius of the circular arc (r = γ1 ), see Equation 2.9.

19

Autonomous Ground Vehicles

Y′

y ( xc , y c )

γ D

θ ( xv , y v )

ψ

r

α

∆x d

x X′ Figure 2.6: Transformation from world coordinates (X 0 , Y 0 ) to vehicle coordinates (x, y) in Pure Pursuit. The new steering angle of the vehicle is calculated by fitting a circular arc (γ) between the vehicle center point (xv , yv ) and the carrot point (xc , yc ) on a distance D, with look-ahead angle ψ. The vehicle has orientation θ.

equation states that the radius of the arc and the x-offset are independent and differ by d. The curvature and radius are derived by:

d = r − ∆x 2

(r − ∆x) + yc2 = r2 r2 − 2r∆x + x2c + yc2 = r2 2r∆x = D2

(2.4) 2

D 2∆x 2∆x . γ= D2 r=

To calculate the curvature, γ, the following geometric relation is used, see 20

2.7 Obstacle avoidance Figure 2.6: π −θ+ψ 2 ∆x = Dcos(α) 2∆x γ=− 2 D D2 1 r= =− γ 2∆x

(2.5)

α=

where ∆x : D: θ: ψ: r:

(2.6) (2.7) (2.8)

x-displacement of the Carrot Point in the vehicle’s coordinate system distance between the Vehicle Center Point and the Carrot Point orientation in the global coordinate system Look-Ahead Angle in the global coordinate system radius of the circular arc.

For a forest machine, the steering angle φ is derived by (refer to Figure 2.7): ` φ = 2sin−1 ( ) r

(2.9)

where ` : is the distance from the Vehicle Center Point to the wheel axis r: is the radius from Equation 2.8. l

φ 2

φ 2

l

r r

Figure 2.7: The relationship between steering angle φ and turning radius r for a forest machine.

2.7

Obstacle avoidance

To successfully develop an autonomous vehicle able to traverse real terrain, obstacle avoidance is a key issue. The most primitive form of obstacle avoidance 21

Autonomous Ground Vehicles stops the vehicle short of an obstacle when detecting it, to avoid collision. In applications for avoiding obstacles in indoor environments (typically long corridors) it is common to use the wall-following method. Here the vehicle follows the wall of the corridor at a certain distance. If an obstacle is detected, the vehicle regards it as just another wall and follows it around at a safe distance until the vehicle resumes its previous course. The drawback with the wall-following method is that it is less versatile and therefore not suitable for outdoor applications. A vehicle in a forest environment demands a completely different level of complexity. Here we must be able to detect the size of and range to all obstacles, steer around (or between) them, and then continue on the correct path again. This means that obstacle avoidance must be combined with path following to know which side of each obstacle it’s best to go (depending on where the path is), and where to go after the obstacle has been safely avoided. In this section, four approaches to obstacle avoidance are presented; Potential Fields [58], Vector Force Field (VFF) [45], Vector Field Histogram (VFH) [11], and VFH+ [80]. Of these four we have implemented the two latter ones, and arrived at the conclusion that VFH+ best suits our needs. The reasons for this are presented in Section 3.7.

2.7.1

Potential Fields

A classical approach to reactive obstacle-avoidance behavior is the Potential Fields method [58]. By applying a grid over the world map and setting each cell to an attractive or repulsive force representing goals and obstacles, the vehicle can safely navigate through the map by reacting to the forces. The advantage of this method is that the vehicle reacts directly on its environment and is not dependent on any global planner (which is often slower). A drawback with Potential Fields is that the vehicle can get trapped in local minimas in the field.

2.7.2

Vector Force Field (VFF)

The idea with VFF [45] is that every obstacle within a certain distance of the vehicle applies a repulsive force vector on the vehicle, with a magnitude inversely proportional to the square of the distance between the obstacle and the vehicle, “pushing” it away from the obstacle. To draw the vehicle towards the goal point, an attractive force is applied to it. To tell where the vehicle should be heading, a resultant vector is produced by taking the vectorial sum of all forces; repulsive from the obstacles and attractive to the goal point. The main advantage over similar methods that use force vectors is the use of a certainty grid in combination with force vectors. In short, a certainty grid is built much like a radar screen, where each obstacle found by a sensor counts up the certainty value at the corresponding coordinates in the certainty grid. This means that a misreading by a sensor results in a low certainty value and is almost completely ignored, while a real obstacle is detected multiple times by the sensors, thereby resulting in a higher certainty value, which in turn results in 22

2.7 Obstacle avoidance a larger repulsive force from those coordinates. The authors identified a couple of problems with the method, including difficulties with narrow passages and trap situations due to local minimas in the field.

2.7.3

Vector Field Histogram (VFH)

The VFH method [11] is based on VFF described above. VFH tries to overcome some of the shortcomings of VFF with a 2-stage data reduction technique rather than a single-step technique used in the VFF method. This way, three levels of data representation exist: 1. The highest level holds detailed description of the vehicle’s environment. A 2-dimensional histogram grid that corresponds to the real environment is used to store obstacle data from the sensors. 2. On the next level, a 1-dimensional polar histogram is constructed around the vehicle. This is done by mapping the histogram grid onto the polar histogram, resulting in a number of sectors with values of the obstacle density in the direction that corresponds to each sector. 3. The lowest level of data is the actual output of the VFH-algorithm; the preferred traveling direction and the steering angle necessary to get there.

2.7.4

VFH+

VFH+ [80] is an enhanced version of VFH. The main difference between the two methods is that VFH+ has a 4-stage data reduction process to select the new direction for traveling. In the first three stages, the 2-dimensional histogram grid (the same as in VFH) is reduced to a 1-dimensional polar histogram. In the 4th stage a cost function is applied to the polar histograms to select the best direction for traveling. This method is used as the primary obstacle avoidance method in our project. 2.7.4.1

The Primary Polar Histogram

A window moves with the vehicle, covering a circular region of cells in the histogram grid, as illustrated in Figure 2.8. We now split the active window into n sectors. The content of each cell is used to create an obstacle vector, with direction β from the cell to the Vehicle Center Point (VCP), and with magnitude m. Direction and magnitude are obtained by: yj − y0 ) xi − x0 = (cvi,j )2 (a − bd2i,j )

βi,j = tan−1 ( mi,j

23

(2.10) (2.11)

Autonomous Ground Vehicles

Histogram grid Obstacles

Active cell (i,j)

dmax

Sector k

VCP

Active window

Figure 2.8: Cells of a circular region in the histogram grid are mapped onto the Primary Polar Histogram, resulting in a number of sectors with values of the obstacle density in the direction corresponding to each sector.

24

2.7 Obstacle avoidance where a, b : cvi,j : di,j : mi,j : x0 , y0 : xj , yj : βi,j :

Positive constants Certainty value of cell (i, j) Distance between cell (i, j) and the VCP Magnitude of the obstacle vector at cell (i, j) Current coordinates of the VCP Coordinates of cell (i, j) Direction from cell (i, j) to the VCP.

The parameters a and b are chosen so that (2.12)

a − bdmax = 1 where dmax

is the distance to the farthest active cell.

A drawback with the original VFH method is that it does not take into account the width of the vehicle. To compensate for this, the VFH+ method enlarges every obstacle cell with the robot radius rr (the distance from the VCP to the furthest point on the vehicle). For further safety, the obstacle cells are enlarged by rr+s = rr + ds (2.13) where ds is the required minimum distance between an obstacle and the vehicle. For each obstacle cell an enlargement angle γi,j is defined by γi,j = arcsin(

rr+s ). di,j

(2.14)

This is the angle between the obstacle vector and the vector between the VCP and the enlarged obstacle cell, see Figure 2.9. For each sector k with angular resolution α, the polar obstacle density Hkp is then calculated by 0

Hkp = (max(mi,j ))hi,j with 0 hi,j = 1 0 hi,j = 0

i, j ∈ k

(2.15)

if kα ∈ [βi,j − γi,j , βi,j + γi,j ] otherwise

This creates a polar histogram that takes into account the width of the 0 vehicle, see Figure 2.10. The h function replaces the smoothing function in VFH and thereby eliminates the difficulties of fine tuning the low-pass filter that is part of the VFH smoothing function. 2.7.4.2

The Binary Polar Histogram

The second stage is to make a binary polar histogram H b from the primary histogram, see Figure 2.11. Instead of having density values, the sectors of H b 25

Autonomous Ground Vehicles Enlarged Obstacle cell Obstacle

rr + s

γ i, j di, j

rr rr + s

Figure 2.9: To compensate for the vehicle width, every obstacle is enlarged with the robot radius rr plus the required minimum distance between an obstacle and the vehicle, ds . are either free (0) or blocked (1). Unlike the fixed threshold from VFH, VFH+ uses two thresholds: τlow and τhigh . Bar k in the binary polar histogram has height Hkb defined by

2.7.4.3

Hkb = 1

if Hkp > τhigh

Hkb = 0

if Hkp < τlow

Hkb

otherwise

=

b Hk−1

(2.16)

The Masked Polar Histogram

The original VFH method does not take into account the limits of the vehicle’s turning radius, but simply assumes the vehicle can travel in any direction at any given time, as in the left part of Figure 2.12. The VFH+ method creates a masked polar histogram that considers the maximum turning radius of the vehicle, as in the right part of Figure 2.12, and determines which sectors are blocked by obstacles. In Figure 2.13, all angles to the left side of the left obstacle are blocked due to the maximum turning radius. To start with we define two circles on each side of the vehicle, see Figure 2.13, representing the left and right trajectories at the maximum steering angle. 26

2.7 Obstacle avoidance

4

18

Primary Polar Histogram

x 10

Magnitude Upper Threshold Lower Threshold Look Ahead Angle Goal Angle Orientaion

16

14

Magnitude

12

10

8

6

4

2

0

0

50

100

150

200 Angle (degrees)

250

300

350

400

Figure 2.10: The Primary Polar Histogram. If a sector (angle) has a magnitude greater than the upper threshold, it is considered to be blocked. If it has a magnitude below the lower threshold, it is considered to be free. If the magnitude is between the thresholds, the sector gets the same value as its neighbor. In this example, all magnitudes are either above the upper threshold or zero.

27

Autonomous Ground Vehicles

Binary Polar Histogram 1.4 Magnitude Look Ahead Angle Goal Angle Orientaion

1.2

Magnitude

1

0.8

0.6

0.4

0.2

0

0

50

100

150

200 Angle (degrees)

250

300

350

400

Figure 2.11: In the Binary Polar Histogram, the values from the Primary Polar Histogram (Figure 2.10) are reduced to either 1 (blocked) or 0 (free).

Without dynamics

With dynamics

Figure 2.12: The original VFH method does not take into account the limits of the vehicle’s turning radius, but simply assumes the vehicle can travel in any direction at any given time. The VFH+ method considers the maximum turning radius of the vehicle.

28

2.7 Obstacle avoidance Enlarged Obstacle cells Obstacles

Blocked

rr + s

Free Blocked

dl

Blocked

rl dr

rr + s

Free

rr

Figure 2.13: The maximum turning radius of the vehicle determines which directions are blocked by obstacles. In this example, the left obstacle blocks all directions to the left of it, while the vehicle can pass to the right of the other obstacle. The positions of the centers of these circles are defined by

where rr , rl : θ:

∆xr = rr sinθ

∆yr = rr cosθ

(2.17)

∆xl = −rl sinθ

∆yl = −rl cosθ

(2.18)

distance to the right and left trajectory centers respectively. the current orientation of the vehicle

The distances between an obstacle in cell (i, j) and the two trajectory centers are obtained by:

where ∆x(i) and ∆y(j):

d2r = (∆xr − ∆x(i))2 + (∆yr − ∆y(j))2

(2.19)

d2l

(2.20)

2

2

= (∆xl − ∆x(i)) + (∆yl − ∆y(j))

the distance between obstacle cell (i, j) and the VCP.

The direction to the right of an obstacle is blocked if d2r < (rr + rr+s )

[condition 1]

(2.21)

and the direction to the left of an obstacle is blocked if d2l < (rl + rr+s )

[condition 2] 29

(2.22)

Autonomous Ground Vehicles where rr+s : rr , rl :

radius of an enlarged obstacle cell from (2.13) radii to the right and left trajectory centers respectively.

By computing these two conditions for every obstacle cell, we get two limit angles ϕl and ϕr which define the maximum steering angle to the left and right of an obstacle. The direction directly backwards from the current orientation θ is defined as ϕb = θ + π. The algorithm for calculating the limit angles is described in Algorithm 2. Algorithm 2 Calculating two limit angles ϕl and ϕr which define the maximum steering angle to the left and right of an obstacle. 1. Determine ϕb = θ + π. Set ϕl and ϕr = ϕb 2. For every obstacle cell Ci,j : (a) If βi,j in (2.10) is to the “right of” θ and to the “left of” ϕr , check condition 1 in (2.21). If satisfied, set ϕr = βi,j . (b) If βi,j is to the left of θ and to the “right of” ϕl , check condition 2 in (2.22). If satisfied, set ϕl = βi,j . where θ: βi,j :

the current orientation of the vehicle the angle between the vehicle and obstacle cell (i, j)

An angle a1 is defined as being to the “right of” angle a2 if mod(a1 +π−a2 , 2π)−π is positive We are now ready to build the Masked Polar Histogram H m with the help of the Binary Polar Histogram, ϕl and ϕr , see Figure 2.14: Hkm = 0

if Hkb = 0 and kα ∈ {[ϕr , θ], [θ, ϕl ]}

Hkm

otherwise

=1

(2.23)

If all sectors are blocked, there is no possible direction of traveling and the vehicle has to halt. Otherwise we only have to choose from all candidate valleys in the Masked Polar Histogram to get the new direction of traveling, which is the 4th and last stage in the VFH+ method. 2.7.4.4

Selection of the steering angle

This last stage is done by a cost function that determines the cost of going in a certain direction. The original VFH method is very goal oriented, because it chooses the direction closest to the look-ahead (goal) direction. The VFH+ method finds all openings in the Masked Polar Histogram and then determines a set of candidate directions c. 30

2.7 Obstacle avoidance

Masked Polar Histogram 1.4 Magnitude Look Ahead Angle Goal Angle Orientaion

1.2

Magnitude

1

0.8

0.6

0.4

0.2

0

0

50

100

150

200 Angle (degrees)

250

300

350

400

Figure 2.14: In the Masked Polar Histogram, the vehicle dynamics is taken into account. Sectors representing directions where the vehicle cannot travel, because of limitations in the vehicle’s turning radius are marked as occupied. In this example, angles greater than the vehicle’s orientation (∼ 240◦ ) are blocked due to this fact. The original Look-Ahead Angle suggested by the path-tracking algorithm (∼ 175◦) and the new Goal Angle selected by VFH+ (∼ 160◦), can also be seen in the figure.

31

Autonomous Ground Vehicles First we determine if a candidate valley is wide or narrow, depending on the number of subsequent sectors free for travel. If the difference between the left and right borders (kl , kr ) is less than smax it is considered to be a narrow valley and a centered direction of traveling is chosen: cn =

kr + kl . 2

(2.24)

For a wide opening we can choose to go at the right or left side of the valley (cr , cl ). If the target sector kt (the sector with the look-ahead point) is within the valley, it is also considered a candidate direction (ct ): smax 2 smax cl = kl − 2 ct = kt

right side of valley

cr = kr +

left side of valley

(2.25)

if kt ∈ [cr , cl ]

So now we have between one and three candidate directions for each valley in the masked polar histogram. To choose the appropriate direction of traveling at timestep i, a cost function g is applied to each candidate direction c: g(c) = µ1 · ∆(c, kt ) + µ2 · ∆(c,

θi ) + µ3 · ∆(c, ki−1 ) α

(2.26)

where µ1 , µ2 , µ3 : cost factors kt : target (look-ahead) sector θi : the current orientation α: angular resolution ki−1 : previously selected sector ∆(c1 , c2 ) is a function returning the number of sectors between sectors c1 and c2 , such that the result is ≤ n/2, where n is the total number of sectors: ∆(c1 , c2 ) = min{|c1 − c2 |, |c1 − c2 − n|, |c1 − c2 + n|}.

(2.27)

The cost function is divided in three parts: 1. The first part represents the cost for going in the candidate direction, given the target direction. The larger the difference between the candidate and target direction, the larger the cost. This term represents the goal-oriented behavior of the vehicle. 2. Next is the cost for going in the candidate direction, given the current orientation of the vehicle. The larger the difference, the more we are required to change direction, hence larger cost. 3. The last term is the cost for changing direction from the previously selected direction of traveling. The larger the difference between the candidate and the previous direction, the larger the cost. 32

2.7 Obstacle avoidance The cost factors, µ1 , µ2 , µ3 , control the importance of the respective parts in the cost function. To get a more goal-oriented behavior, µ1 must be larger than the other terms put together (µ1 > µ2 + µ3 ). To select the best direction of travel, the cost function g is applied to all candidate directions c. The candidate with the lowest g is selected, denoted csel . The corresponding angle ψ is obtained by: ψ = csel α. (2.28) Because the path-tracking method Pure Pursuit [17] lays a circular arc between the vehicle and the carrot point, as described in Section 2.6.3, some additional calculations must be performed when this algorithm is used. If the chosen sector equals the look-ahead sector, i.e. csel = kt , the angle from Pure Pursuit is used and the algorithm is allowed to work as usual. Otherwise the angle ψ from Equation 2.28 is used.

33

Autonomous Ground Vehicles

34

Chapter 3

Development of an Autonomous Forest Machine 3.1

Introduction

In modern forestry, two types of machines are typically used when harvesting trees. A harvester fells the trees, delimbs them, and finally cuts the logs into desired lengths. This is a highly automated process with just a little intervention from the operator to make it as fast as possible. A forwarder then collects the logs left by the harvester on the ground, and transports them to the nearest road for further transportation. In some cases these two machines can be replaced by a combi machine that both harvests and transports the timber. The forest industry is under keen competition, and therefore must constantly develop its products to make them more profitable. Since the modern harvester is able to make use of almost 100% of the trees’ value, the harvesting speed (m3 /h) must be increased to make it more effective. A problem with this is that the operator works under stressful conditions today, and if the harvesting speed rises even further, the operator becomes a bottleneck. To solve this, the operator would have to be replaced by a fully autonomous machine, or at least get help from semi-autonomous systems. An autonomous harvester must be able to accomplish a number of complicated operations and decision-making [32]: • Choosing which tree to fell. This decision is based on economic gain and ecological considerations. • Where to position the harvester. This decision is based on the relative position and size of the tree, an estimation of the machine’s stability, planning of a good place to put the timber, and ecological considerations. • Choosing the direction of felling. The felling must be done without endangering the machine or surrounding objects and people. Furthermore, 35

Development of an Autonomous Forest Machine the trees must be felled so the logs, after the harvesting process, end up in a place suitable for a later transportation out of the forest. This decision is based on any nearby obstacles, wind direction, inclination of the tree, and risk for damage of the remaining trees. These decisions are hard to implement in a fully autonomous machine. The semi-autonomous system ”Besten”, described in Section 2.2, could be one step on the way developing this kind of machine. Another way to increase the efficiency in forestry is to automate the forwarder. For this we “only” have to be able to navigate through the forest. The choice of the transportation path is made with regards to obstacles in the terrain, the nature of the ground (e.g mire or hard), and ecological considerations. The forest industry has a long-term vision of developing unmanned shuttles that do the work of the forwarder but without an operator [28], as illustrated in Figure 3.1. The main advantages with an unmanned shuttle are lower labor cost, and due to a lower weight of the vehicle, less emissions and ground damage. The problem with the operator becoming a bottleneck can be an issue with the forwarder as well if the productivity goes up. With the same demand on productivity, an autonomous machine could actually be allowed to be less efficient but work longer hours to compensate. By allowing slower crane movements, lighter loads and lower speeds, an autonomous forwarder could be equipped with a smaller engine. This would reduce the wear and tear on the vehicle as well as have a positive effect on the environmental damage. The remainder of this chapter describes an ongoing project of developing techniques for an autonomous path-tracking shuttle, as a part of this vision.

3.1.1

Relation to earlier work

Of the applications described in Section 2.2, AutoStrad [25] is the most advanced system, and the only commercial one. Although very sophisticated, the control system relies on knowing exactly where all obstacles and other vehicles are and the exact size of each container. This is done by fixed radars covering the entire area. Though the vehicles operate in a structured and known environment, they have an emergency stop if an unforeseen obstacle should appear in front of the carrier. The weed-controller [76] and Christmas tree weeder [30] also work in structured environments, where the plants are located in straight rows. The vehicles participating in the Grand Challenge 2005 [79] worked in an unstructured environment, although it had limited types of obstacles. In a forest environment the terrain is very rough, there are a lot of trees and other obstacles, making the task of automating a forwarder even harder. A large part of our project has been design and implementation of a general platform for research in autonomous vehicles. This platform supports many different kind of sensors and different target vehicles. It is also a base upon which algorithms for controlling the vehicle (e.g. path tracking) can be implemented, as described in Section 3.4. The resulting system has been used to test different kinds of sensors and evaluate their usefulness in the forest application. We 36

3.1 Introduction

Figure 3.1: A vision of the future of forestry. The harvester that cuts down trees is still operated manually, but the forwarder that brings the logs to the nearest road is now an autonomous shuttle. Illustration by Komatsu Forest AB. have implemented and evaluated a number of state-of-the-art algorithms for path tracking, obstacle avoidance, laser-based localization, and sensor fusion. In addition, new algorithms and techniques tailored for the forest application have been developed as part of the project. Examples of this are the new pathtracking algorithm Follow the Past, and wheel odometry enhanced with neural networks.

3.1.2

System requirements

In the preliminary study of the project [35], a number of requirements for the project are specified. The forest machine (a forwarder) should be able to follow a predefined path in a forest environment while avoiding any obstacle on the way. The reason why a predefined path should be used instead of the vehicle finding its own way to the goal is that it is very hard to do this fully autonomously due to the unstructured conditions in a forest. To avoid obstacles that the sensors cannot see, for example mire ground, a very steep slope or land we are not allowed to drive on (e.g private property), these conditions would have to be specified before beginning the path planning. Furthermore, ecological concerns, such as ground damage must be taken into consideration when planning a path. A more realistic goal is to specify a path that the vehicle should follow. If the vehicle is not able to go around an obstacle, it should stop and call on an operator to either move the vehicle around the obstacle or remove it. Some important safety-issues should also be considered; the vehicle must be able to 37

Development of an Autonomous Forest Machine detect humans inside a safety region around the vehicle and stop if someone comes too close. Ground conditions must be monitored so the vehicle does not risk uncontrolled sliding or turnover. These safety issues have not been further dealt with in the presented work. Two computers are needed; one on the machine to interface with the hardware, and another computer with which the operator can supervise and take control over the vehicle. Path generation As described in Section 2.5, there exist several techniques for planning a path. However, as discussed earlier, it is very hard to let the vehicle plan its own path through the forest. For example, when using a geographic map for path planning, it is difficult to plot a path really traversable with the forest machine. In a forest there could be obstacles such as fallen trees or large boulders that do not show on the map, and the ground could be mire for example. In our case, we have the advantage that a harvester always cuts its own way from the main road to the area of felling. This means that we have a safe road that is traversable by a forest machine and is on land we are allowed to be on. To get the waypoint coordinates on the path, a GPS could be mounted on the harvester and record a path. A problem with this is that the harvester does not necessarily go straight along the correct path, but may diverge from it to cut down trees beside the path for example. Our solution is to teleoperate the forwarder along the selected path while recording not only the position, but also the speed, heading and steering angle of the forwarder. This makes it possible to take the vehicle’s kinematics into consideration as well. In case of obstacles blocking the planned path, the path must be re-planed locally. In our case this is done by the obstacle-avoidance algorithm, described in Section 3.7.

3.1.3

System design

Following the system requirements, we designed the system with two modes of operation: Path Learning, in which the human operator drives or remote controls the vehicle along a selected path back and forth from the area of felling to the transportation road while the vehicle learns the path. In this phase, position, speed, heading, and the operator’s commands are recorded in the vehicle computer. When the vehicle has been loaded with timber (this subtask could also be done autonomously, but is not considered in this project) the operator activates Path Tracking mode, in which the vehicle is able to autonomously track the learnt path back to the transportation road. The vehicle is also able handle unexpected events, such as avoiding any obstacles in the way (refer to Section 3.7) and compensate for irregularities in the terrain or noise in the positioning sensors, as described in Section 3.5.4. If the vehicle ends up on the side of the learnt path for any reason, it is able to autonomously steer back towards the path again. The path-tracking algorithm is described in more detail in Section 3.6. 38

3.1 Introduction Testing algorithms can be done on different levels, from simply debugging the code to full-scale tests. During the early development phase, testing on the full-size forest machine is both impractical and inefficient. Therefore, the work has been conducted on three different target machines, each with increased complexity. The different vehicles are described in more detail in Section 3.2. As shown in Figure 3.2, the same main program can control any of the three target machines through a software switch-board. Likewise, sensor data passes from the target machine to the main program. In this way, high-level routines like path tracking are easily developed and implemented by the use of a simple simulator [67] (see Section 3.2.1). The simulator implements no sophisticated sensor models, and has a simplified kinematics model for propulsion, but serves very well its purpose for debugging and testing the developed algorithms. The user interface is also easily developed using the simulator as target machine. The infrastructure for sensors or actuators (see Section 3.5), and the modules for communication between the two main computers are most conveniently developed on the small-size Pioneer2 AT8 robot. Various types of sensors are also evaluated on this target machine. In the current phase of the project, the system is moved to the real forest machine, and the routines for vehicle control are fine-tuned and tested. Also, reliable sensor tests are only possible using this final target machine.

3.1.4

Computers and software

The system runs on two computers, as shown in Figure 3.3: the one placed on the vehicle is responsible for hardware interfacing and low-level data processing, such as data fusion in an occupancy grid (see Section 3.5.4.4). This computer communicates by a regular Wireless Local Area Network (WLAN) with the operator computer running the high-level path-tracking algorithms and the user interface. The two computers run with Windows XP at present, although it is designed so it can be moved to other operating systems, for instance UNIX or LINUX. This operating system independence comes from the choice of implementation language, Java and Matlab, as programs written in these languages are easily moved between different platforms. However, certain low-level drivers in C++ will have to be converted. In general, low-level processing and communication is implemented in Java, and high-level processing in Matlab. However, during development the Matlab environment has been used also for typical lowlevel operations, such as Kalman filtering. With a top-level cycle time of around 100 ms, it has not presented any problems, and it simplifies the research work considerably. In a final productified version most processing should be implemented on the mobile computer, with the operator computer in charge of the user interface only. 39

Development of an Autonomous Forest Machine

Operator program: • Path recording • Path tracking • Tele operation

Control commands: Turn(3.1) Speed(0.36) Halt

Sensor data: GPS: 63.53.341N 20.19.247E Compass: 213.5 deg. Radar: 32 deg. 2.17 meter 276 deg. 0.45 meter

Switchboard

Simulator

Modified Pioneer2 AT8

Modified Valmet 830

Figure 3.2: The work has been conducted on three different target machines, each with increased complexity. This approach greatly simplifies the research and development of both hardware and software.

40

3.2 Vehicles Mobile computer Win XP Java - Hardware interface to sensors and actuators - Low-level control loop - Low-level data analysis and occupancy grid - Communication

WLAN communication

DGPS base station

Sensors For localization: GPS/GLONASS Gyro For obstacle detection: 24 GHz radar Laser scanner Engine rpm Steering angle

Remote computer Win XP Matlab - User interface - Path learning - High-level control loop for path tracking - Data analysis routines - Communication

Actuators Steering force Throttle pedal Brake

Figure 3.3: Overview of the architecture of the developed system. The highand low-level parts are split between two computers. The forest machine shown is a vision of what a future autonomous shuttle could look like.

3.2

Vehicles

As described earlier, the system has been developed for three different vehicles, each with increasing complexity. Below is a more detailed description of the three platforms.

3.2.1

Simulator

To simulate a forwarder, a simulator environment was developed as part of the master thesis project [67]. The advantage of using a simulator is that algorithms are easier to develop and test in this environment than on a real forest machine. The main steps in the simulator are: • Determine the turning radius for a given steering angle • Calculate the position and orientation where the vehicle will be after a time ∆t • Plot the vehicle at the new coordinates (x, y), with an orientation θ and steering angle φ. Two different coordinate systems exist in the simulator environment; the global and the vehicle coordinate systems. The global system is defined with x-axis to the east and y-axis to the north, while the vehicle system has its x-axis in the direction of the vehicle’s orientation and y-axis to the left. The definitions of 41

Development of an Autonomous Forest Machine steering angle, orientation, and heading in Section 3.3.1 are used in the simulator environment. To calculate the movement of the simulated forest machine, Equations 3.12 to 3.19 in Section 3.5.1.4 are used. These are the same equations used in the wheel odometry for the real forest machine.

3.2.2

Pioneer

The Pioneer2 AT8 is a small robot (50 x 49 x 24 cm) from ActivMedia Robotics powered by up to three lead accumulators. It weighs 14kg and is able to carry a payload of 40kg. The AT8-model uses skid steering on its four wheels to turn, and it is able to run in rough terrain. The Pioneer has a built-in microcontroller with the ActivMedia Robotics Operating System (AROS) included, which manages low-level commands, and data from wheel encoders and the built-in sensors. This provides an easy interface to the vehicle, so low-level control commands do not have to be implemented. In addition to the GPS and radars that can be seen in Figure 3.2, the Pioneer is equipped with a built-in ultrasonic sonar array. It consists of eight sonars in the front and eight in the rear that can sense obstacles from 15cm to 7m. Together, the forward and rear sonars provide a 360 degree coverage for obstacle sensing. This platform was used as an intermediate step between the simulator and forest machine to test sensors and algorithms. Due to its small size it is easy to handle and can be used both indoors and outdoors.

3.2.3

Valmet 830

This is our final target machine, where final tests of obstacle avoidance and path following are carried out. The forest machine is entirely controlled via an industrial communications, CAN, bus. Widely used in automotive systems, it requires only a single connection from our hardware to the machine control computer (actually several computers, connected by the CAN bus). Various hardware components enable the system to control engine throttle, turning rate, gear selection, etc. Some sensor data is also received through the bus, e.g. engine rpm., current turning angle, and actual throttle level. The forest machine, a Valmet 830 as shown in Figure 3.2, is equipped with an articulated joint for steering. We have implemented a simple proportional integrating (PI) controller that takes care of controlling the steering angle by controlling the joint. The forward kinematics equations, which describe how the vehicle moves in response to changes in steering angle and wheel speed, have a known and simple form (refer to Section 3.5.1.4) but may vary depending on varying load and ground conditions. E.g.: a forest machine with no load tends to move the light rear part more than the heavy front part when turning. 42

3.3 Definitions

3.3

Definitions

In this section, some central definitions about the different coordinate systems are introduced.

3.3.1

Steering angle, heading, and orientation

Since the forest machine uses articulated steering, the definition of heading, orientation, and steering angle may not be obvious to the reader. In this thesis the following definitions are used. The steering angle φ is defined as the angle between the front and rear sections of the vehicle, as shown in the left part of Figure 3.4. φ 2 φ η θ

Figure 3.4: Definition of the steering angle φ, orientation θ, and heading η for a forest machine with articulated steering. Heading η is defined as the direction of the front part of the vehicle, defined in the global coordinate system described in Section 3.3.2. The orientation θ of the vehicle, defined in the same global coordinate system as the heading, is the direction in which the vehicle would travel if the steering angle was zero and the baseline maintained, as the dashed vehicle in the right part of Figure 3.4. It can be computed as the heading of the vehicle minus half the steering angle, i.e. θ = η − φ2 .

3.3.2

Coordinate system

To use several sensors, a uniform coordinate system must be defined for all sensors and the vehicle. The global coordinate system is the one that the position and heading sensors work in. This system is fixed on the ground and defined with x-axis to the east and y-axis to the north. In addition to the global coordinate system, there is a local one fixed to the vehicle. All sensor poses (how the sensors are mounted) are defined relative to this system. We want all sensor readings in the local pose to be expressed in the global coordinate system, 43

Development of an Autonomous Forest Machine refer to Figure 3.5. The transformation between these coordinate systems is described in Section 3.5.4.3. Obstacle sensors work in a different coordinate system, as they give a range and direction to an obstacle relative to the sensor. These readings are then converted either to the local or global coordinate system depending on what application is using them, see section 3.5.4.3. For the sake of completeness and clarity, the properties of the different coordinate systems as well as related definitions are listed below. Global coordinate system • The (x, y, z) coordinate system is right-handed • The origin is arbitrarily defined and is different for different sensors. We use the GPS coordinate system as reference. • The (x, y) plane is defined as horizontal (i.e. perpendicular to the gravitational field) • The x axis points eastward • The y axis points towards the geographic north pole • The z axis points upwards from the (x, y) plane Local coordinate system • The origin is an arbitrarily chosen point in the vehicle (in our case, the middle of the front part of the vehicle, level with the ground) • The (x0 , y 0 , z 0 ) coordinate system is right-handed • The (x0 , y 0 ) plane is defined as horizontal on flat ground • The x0 axis points forward along the vehicle • The y 0 axis points to the left • The z 0 axis points upwards The local pose expressed in the global coordinate system • Position = (x, y, z) • Attitude = (roll, pitch, yaw) = (φ, θ, ψ) • Pose = position and attitude = (x, y, z, φ, θ, ψ) 44

3.4 Software platform Important angles The signs of these angles are determined by the right-hand rule. Refer to Figure 3.5. • Roll (a.k.a. bank angle) φ : the angle between y 0 and the (x, y) plane. (−π < φ ≤ π) • Pitch (a.k.a. elevation) θ : the angle between x0 and the (x, y) plane. (−π < θ ≤ π) • Yaw (a.k.a. heading or azimuth) ψ : the angle between x and the projection of x0 on the (x, y) plane. (−π < ψ ≤ π)

3.4

Software platform

Many of the existing mobile-robot software packages do not include handling of sensors and actuators in a sufficiently systematic and uniform way, as described later in this section. The software framework proposed here, denoted NAV2000, addresses the specific need for interchangeability of components in robotics. At the lowest level, sensors, and sometimes also actuators, often have to be replaced by similar, yet not identical, components. At a higher level, the target vehicle often changes during the work process as described earlier. The presented software provides a framework that supports these replacements and allows configurations of sensors, actuators, and target machines to be specified and manipulated in an efficient manner. The system can be run on several different computers if some software modules require more computing power. To accomplish sufficient monitoring of the system’s health, a dedicated system keeps track of all software modules loaded onto the local computer, and also communicates with health monitors in all other computers running the system. The overall health of every module as well as a more detailed description of possible problems is presented graphically. In addition to this, the system uses logfiles to enable convenient debugging and performance analysis of hardware and software modules. The information in this section has previously been published in our technical report [31].

3.4.1

Introduction

Various tools for mobile robot software development have been proposed over the last two decades. These tools exist at many levels of abstraction, and are designed to support the development in different ways: • At the highest level, the hierarchical, reactive, and hybrid robotics paradigms are represented by a number of architectures that implement the general ideas within the respective paradigm. In the hierarchical paradigm, the robot senses the world, plans the next action, and then acts accordingly. This paradigm focuses much on planning. The Nested Hierarchical Controller (NHC) [51] is a popular architecture for hierarchical systems. In 45

Development of an Autonomous Forest Machine

Z (up)

Y (north) X (east)

(a) Global coordinate system (x,y,z) y’

x’

Top view z’

z’ x’

y’

Left view

Rear view

(b) Local coordinate system (x’,y’,z’)

y’

y x’ yaw ψ x

Top view z

z z’

z’

x’

pitch θ

y roll φ

x y’ Rear view

Left view

(c) Local pose expressed in the global coordinate system

Figure 3.5: Definition of the coordinate system.

46

3.4 Software platform the reactive paradigm, the robot reacts directly on sensor data, without any planning. The subsumption architecture [12] was one of the very first architectures put forth for building reactive systems. The hybrid paradigm is a mix of the other two and is the most common paradigm used today. Here the robot first plans how to get to the goal, then the underlying reactive system takes over and acts on this information. The system can re-plan when new sensor data arrives, or when the goal has been reached. One of the first architectures for hybrid systems was the Autonomous Robot Architecture (AuRA) [2]. • At the intermediate level, there are a number of robot middleware systems, e.g. OSCAR [43], MARIE [14], CARMEN [52], Orca [61], and MIRO [81]. These are software frameworks to support different robotics paradigms and are sometimes called Robotic Development Environments (RDE) [46]. They provide data structures, functions, and communication protocols as a platform to build software architectures on. They often contain highlevel functions, such as path-tracking and obstacle avoidance, in addition to the basic robot interface. • At a lower level, networking middleware such as CORBA [26], Microsoft’s .NET Framework, and MIRPA-X [20], enables software to run on several computers with different operating systems, programming languages, and networks. They are not deigned specifically for robotics applications, but are general programming tools for distributed computing. Many of the above-mentioned robot middleware tools overlook important issues regarding target machines, actuators, and sensors. Some of the intermediatelevel systems do include high-level handling of sensors and actuators, but not in a sufficiently systematic and uniform way. The work presented in this section attempts to fill this gap, and provide a link between physical sensors/actuators (or rather their software counterparts) and the overall control program. The system proposed here, denoted NAV2000, covers the lower level and a significant part of the intermediate level. Instead of a networking middleware, Java is used for (among other things) transparent network communication. It should be emphasized that NAV2000 does not aim at taking the same role as some of the robot middleware systems mentioned above. Many of these systems provide support for a wide range of different sensors, robot platforms and algorithms for obstacles avoidance and path-tracking for example. While we have implemented such algorithms, they are not part of the NAV2000 system. The proposed software infrastructure for control and sensing should be seen as support, or a framework basis, for a complete development environment. NAV2000 is programmed in Java because of its suitability for rapid testing and evaluation, and its support of an Object-Oriented system. Java has built-in multi-tasking (threading) and also supports networked systems, which makes it easier to deploy the system on several computers without using networking middleware. 47

Development of an Autonomous Forest Machine

3.4.2

Motivation

The work has been driven by an identified need for interchangeability of hardware components in the development and use of robotics systems. This need surfaces at many levels in the systems, and at many stages in the development process: At the lowest level, sensors, and sometimes also actuators, often have to be replaced by similar yet not identical components. In a complex system, this may very well mean that a sensor of one type has to be replaced by one of another type, which may be connected to another computer. This kind of replacement is often a major part of the development and research process, where different kinds of algorithms, sensors, and setups have to be evaluated and compared. Sometimes another computer is involved, if some modules require more computing power. Furthermore, interchangeability is also often needed in a running robot. For example, a satellite navigator may have to be replaced by odometry if the satellite signals are occluded, or a laser scanner used for obstacle detection may have to be replaced by a radar sensor due to weather changes. At a higher level, the target vehicle for the developed system is often changed during the work process. As discussed earlier, this is a practical and efficient approach, especially when developing systems for large autonomous vehicles. Furthermore, support for this level of interchangeability will become more and more important as generic robotics systems are developed for many types of tasks and platforms. Interchangeability can be implemented in robotics software in many ways, also without any special tools supporting it. However, as robotics systems become more and more complex, such tools become invaluable.

3.4.3

Design criteria

The aim of the presented work is to provide a flexible and generic link between physical sensors, actuators, and similar components, and higher-level control softwares (e.g path-tracking). It has been driven primarily by three basic requirements: • Interchangeability - Similar hardware components and their corresponding driver routines should have a common interface (”look and feel”) to enable interchanging them at an appropriate level, without the need for modifications of the rest of the system. In fact, the system is designed so higher-level routines would have limited knowledge of the actual implementation. • Virtual modules - Instead of rewriting a sensor, e.g to implement sensor fusion, a new module responsible only for this function is developed, then used in place of the old sensor, and also uses the old sensor for input data. In the same way we can have a virtual vehicle that extends the functionality of a real vehicle. For example a small robot could mimic the 48

3.4 Software platform behavior of a larger vehicle (e.g. slower steering response). Several such virtual sensors or vehicles could be strung together. • Distributed processing - The system should be adaptable to both differing demands of computing power and different configurations during test and “production”. During in-office testing, all modules could be loaded on to a single more powerful computer, but in field trials two (or more) mobile computers may share the processing load. Vital for understanding the difference between virtual modules and interchangeability is that the latter deals with replacing one sort of software module with another, similar module - two different speed sensors for instance. A virtual speed sensor, on the other hand, is not really a piece of software directly communicating with a sensor; it can be a filter or a network proxy that serves in the sensor’s place while adding some functionality. From these requirements a set of more detailed goals can be specified to facilitate the design of the system: • Modularity: A software module is the basic building block of the system. Modules exist in a type hierarchy, with subtypes being, among others, sensors, vehicles, and actuators. At the top level in the hierarchy all modules have a common interface, i.e. they can perform a common set of operations, such as close, open, and return status. This property of modules is used by the system to load, start, stop, and interrogate modules on a high level, without knowing the detailed function of the particular module. A common loader for all types, which processes an initialization file with module names and actual types, loads the modules. On a lower level, all modules of a certain subtype, such as all speed sensors, can be interchanged and still handled similarly by the sensor users. • Extensibility and flexibility: New modules and module types should be easy to add to the system without any changes to the existing software. An important feature of the system is the low coupling between modules, i.e. they are effectively isolated from one another regarding internal representation of data and functions. Only the exposed external interface is shown, and if a new module of a certain type is added, it can be treated as any other module of this type. An example would be a speed sensor type, which only has the function getSpeed. This leaves it up to the implementers to design the module in any way they want, as long as the module delivers data via its getSpeed function. In a practical system there may exist several speed sensors, which take data from the machine itself or from a GPS receiver, but the rest of the system does not know and does not need to know, the actual sensor used at any given time. If a new type of speed sensor is installed, its corresponding speed sensor module can be added to the system without any changes to existing code. Which sensor to use can be configured at start-up or dynamically changed at runtime. 49

Development of an Autonomous Forest Machine • Cohesion: The modules are designed for high cohesion, i.e. a module does just one thing, but does it well. An example would be a sensor that requires some form of filtering of its data. Instead of incorporating filter code in the sensor module (and thus in all sensor modules that require it), a special filter module is developed. The filter module would have the same type as the sensor, and in effect would be a virtual sensor, which is used in the normal sensor’s place. The filter module then uses the actual sensor as its data source. This virtual sensor will for all intents and purposes look exactly like a “real” sensor to the users of the sensor data. In addition, the modules have to have low coupling, i.e. a low dependency on other modules. • Multithreading: Every module should have its own execution thread, and thus run independently of other modules. Polling loops are discouraged; instead, an event-driven system is used with multiple independently executing threads. The threads normally sleep and only wake up if a message arrives from another module, from the network, or a user. • Network Communication: Modules can be located on several host systems, and there must be a way they can communicate transparently, regardless of the actual configuration, i.e. modules should not be aware of whether they are located on the same computer as the modules they communicate with, or a different one. • Object Orientation: The system should be written in an Object-Oriented language. Object-Oriented Analysis and Design (OOAD) is a development paradigm that focuses on similarity with the real world and the actual system to be automated. A key point is the concept of objects, and what they represent. A similar concept is that of software components [78]. The idea behind this is that software should be developed by gluing prefabricated components together, much like integrated circuits (IC). Each component must be stand-alone with a well defined interface and not be dependent on other components to work. An example would be a component for each speed sensor that can be interchanged without needing to change any code. The older imperative paradigm is much more function-oriented and focused on how something should be done. In OOAD the data that makes up an object, together with functions that operate on that data, is central, whereas function-oriented systems usually focus on function call hierarchies, and a global “blob” of data. 3.4.3.1

Important characteristics of object orientation

The following characteristics of object-oriented systems are particularly important and useful to meet the requirements listed above [10]: • Classes - are data abstractions with an interface of named operations (methods) and a hidden local state (attributes). Classes are templates 50

3.4 Software platform for one or more objects in a running system. An object is thus a data structure in memory specified by its type, its class. • Inheritance - There must be provisions for subtypes, or subclasses, which extend base classes and make them more specialized. A subclass can use or replace some or all of the attributes and methods of its base class, but cannot remove anything. Walking ”upwards” in the type tree leads towards a more general model, while downwards leads towards more specialization. • Polymorphism and Dynamic Binding. Polymorphism is a powerful concept in which a reference of the type “reference to class A” can be used to also reference subclasses of A. An example would be a GpsSpeedSensor that inherits from the superclass SpeedSensor. A part of the system can use a reference to SpeedSensor to refer to any of the speed sensors in the system, e.g. GpsSpeedSensor, without actually knowing which concrete sensor is used. The actual type of the sensor can be decided at the start of the system or dynamically during a run without any code changes. Also new SpeedSensors can be added later on without affecting any other part of the system. There are ways to get the real name of the actual sensor supplying the data, but using this information makes it more difficult to extend the system, since code must be rewritten every time a new sensor is added. Dynamic binding is related to polymorphism and makes sure that the correct method is called, even if a method in a superclass is redefined in a subclass. Regardless of the type of reference it is always the referred object’s method that is called. Static binding would look at the type of the reference instead. • Encapsulation makes sure that only an object’s own methods can manipulate its attributes. The data is effectively hidden from outside view, which is in contrast to imperative systems where data is usually global and easily accessed for all functions. Encapsulation helps to isolate the internals of an object, and thus allows the internal representation to change without affecting other parts of the system. An example would be a speed value, which could be stored to and retrieved from an object in miles per hour but internally kept as meters per second. If the internal representation would change to millimeters per second the external interface would look the same, and no user of the object would have to alter code or even notice the change. Speed could actually not be present as a value at all but be computed every time it is requested, from, for instance, elapsed time and distance traveled. Other examples are more complex data structures such as lists, where different implementations can be substituted depending on performance issues for instance. Object-Oriented program development is often divided into at least three stages: 1. Analysis, where a class model is built from the ”real world”, with classes and interactions modeled after the actual problem to be solved, the problem domain. In this phase there is much interaction with the domain 51

Development of an Autonomous Forest Machine experts, i.e. the users of the system. Names for classes and methods are normally chosen so the users can follow the development. The same class names persist through the whole process, and since they are from the problem domain, discussions between users and designers are made easier. 2. Design, where a class hierarchy is built that represents the system to be constructed. Not all of the classes present in the analysis must be used, and new classes can be added here. The important thing is to remember the distinction between the world to be modeled and the system to be written. At the end of the design phase, all class attributes and methods are defined. 3. Implementation, where a suitable programming language ideally would be decided, and the code written. Usually different demands and constraints on the system cause the choice of programming environment to take place at an earlier time. In the Object-Oriented development paradigm, Iterative System Development is often used, where these three stages are iterated, so the experience from the design and implementation phase is fed back into the analysis phase. This has two benefits; the first design arrives at the implementation stage as soon as possible, which is important if time-to-market is an issue. After a while the better and more refined analysis can replace the first, cruder, one.

3.4.4

Modules

NAV2000 uses Java Interfaces for different sub-trees in the architecture: The top root is a Module, under which there are a Sensor tree, a Vehicle tree, a Proxy tree, and so on. In parallel to the interface hierarchy there are also abstract base classes that supply common functionality to the subclasses. One example is BasicModule that implements the interface Module. This abstract class contains a lot of the internal workings that a module might need; loading of parameters, status reporting, and logging initialization. The class BasicSensor likewise implements Sensor and inherits BasicModule. It contains sensor-specific functionality such as data encoding and decoding functions, conversion between little/big-endianness, and support for message notification (observer-subjectpattern). The classes implemented in the NAV2000 system broadly represent different types of components in a robotics system: • Sensors - represent hardware units that deliver sensor data, e.g. speed, heading, and position. • Actuators - represent hardware units that control external equipment, e.g. throttle, steering angle, and brakes. • Vehicles - several implementations of real and virtual vehicles. 52

3.4 Software platform • Controllers - process sensor data and compute control signals for actuators. • Proxies and servers - facilitate transfer of sensor data and control commands over a network (Ethernet or WLAN). These modules hide the actual structures needed to use the network, so a module has the same look and feel whether it is used over a network or not. All modules have a init finish open close getName getStatus setParameter getParameter

few methods and data elements (attributes) in common: : initialize the module : general housekeeping at shutdown : activate the module : deactivate the module : return the name of the module : return the status (e.g. errors) : set any parameter (e.g. calibration) : return the value of a parameter

Parameters are special persistent data elements, i.e. they are kept on permanent storage between runs. An example would be calibration parameters that, once set, are reloaded at every start of the system. A Sensor has only one more method than the Module base class, the getPose method. It returns the actual position and attitude of a sensor relative to the vehicle it is mounted on. Further down the hierarchy there are special versions of Sensors, for example the RangeArraySensor, an array of range sensors, such as sonars. This class specifies a few more methods: getRanges : return the range to each obstacle as measured by the range sensors. getPoses : return the mounting pose of each range sensor. getTimeStamps : return the time when the last measurement was made for each range sensor. A PositionSensor contains both the mounting pose returned by the getPose method, and the position measured by the sensor, returned by getPosition. Timestamp is also incorporated into the data returned by all sensor classes, to facilitate correct time stamping of data. In general, all data, be it speed, steering angle, or gyro temperature, are timestamped, since it is important for higher-level routines and fusion systems to be able to match data taken by different sensors that are not always synchronized. Also, this timestamping is used for health checks and as a means for estimating the performance of the system, in particular the network communication. At the bottom of the class hierarchy the actual implementation classes exist, i.e. classes that can be instantiated. An example would be a HTUSpeedSensor, which in the forest machine is the Hydraulic Transmission Unit Speed Sensor. This sensor reads system data via the vehicle’s own control and data bus, a so-called CAN bus, common in vehicle systems. A class diagram for parts of the forest machine system is shown in Figure 3.6. 53

Development of an Autonomous Forest Machine

GpsPosition PositionSensor +getPosition() +getTimeStamp()

Odometry

Proxy +getPose() +getTimeStamp()

SpeedSensor

HTUSpeedSensor

+getSpeed() +getTimeStamp() GpsSpeed

Server Module +init() +finish() +open() +close() +getName() +getStatus() +setParameter() +getParameter()

Actuator

AngleSensor

VehicleAngle

+getSteeringAngle() +getTimeStamp() LaserScanner

Sensor +getPose() RangeArraySensor Vehicle +setSpeed() +setAngle() +setThrottle() +setTurnRate()

+getRanges() +getPoses() +getTimeStamps()

Radar

Sonar AttitudeSensor

Controller +setEnabled()

+getRoll() +getPitch() +getYaw() +getTimeStamp()

HeadingSensor

Gyro

GpsHeading

+getHeading() +getTimeStamp() Odometry

Figure 3.6: Class diagram for parts of the forest machine system. Only the sensor subclasses are fully described in this diagram. All “real” sensors are instances of one of the implementation classes shown to the far right. All other classes implement general functionality inherited by their subclasses. Some of the methods that must be implemented are shown in the respective class.

54

3.4 Software platform

3.4.5

Inter-Module Data Flow

One of the central tasks of the system is the swift delivery of data from sensor to user, be it a control loop or a module for remote-controlling (teleoperate) the vehicle. In many systems the data-flow is based on “polling”, where interested modules must ask for sensor data, without knowing if there are any new data available. The data flow in our system is event-driven, meaning that when a module has new data to deliver, it signals other interested modules. Since all modules are autonomous, this allows them to deliver data at their own pace. For this to function, there are two requirements: “user-modules” must be able to find the data sources, and have to be able to register interest in the data. The modules can find each other with the help of the Registry, described later in this section. To set up and remove a module’s interests in data, the two methods addObserver and deleteObserver are used. An observer, also called listener, is a special property that can be assigned to any class, and a subject is something an observer observes [21]. Observers and subjects are part of the event-driven data flow, which is the dominant and preferred way to move data through the system. Its basic mechanism consists of subjects, for instance sensors that notify its observers whenever a new measurement value is available. The listeners, which in turn may be subjects, process this data and then notify their observers, and so on. The data finally reaches the end user, normally a control loop in the system or an external system, e.g. a Matlab program. Matlab is not easily amendable to the event-driven paradigm, so the last step in the chain is a standard polling of data. A subject can have any number of listeners, and an observer can observe any number of subjects. An example would be a display that presents some combined and interdependent data, for instance a map that listens to both a position sensor and a heading sensor, and plots the vehicle’s position and direction. A set of support classes is available for all modules, and the two most important ones are Loader and Registry. The Loader is the class responsible for loading, opening, closing, and finishing all modules, and can deliver a list of all loaded modules within a specified class, for instance all loaded speed sensors. The Health Monitoring system (further described in Section 3.4.8) uses this to keep track of all modules and periodically query them for their status. Since all modules must have a getStatus-method, the Health Monitor can do this knowing only that it is dealing with a subclass to Module. The Registry keeps track of the names given to all loaded modules, so an actual module can be found by its name. This functionality is used by most modules to identify and find their subjects, i.e. the other modules it wants to observe. The Registry can also hold arbitrary data, like default network addresses for the local and remote systems, default timeouts, what module is the current TimeSensor (used by the logging facility), and what level of debugging should be used. Debugging can be customized for every module, from none to very detailed, and also on a top level, enabling a user to turn on and off all debugging from a central point. 55

Development of an Autonomous Forest Machine

3.4.6

Communication

The modules in the system may reside on different computers. The communication routines in NAV2000 take care of the data routing, and make the actual location of each module transparent to other modules. Every module of the type SpeedSensor, PositionSensor, etc., has a virtual companion used in place of the real sensor when it is located on another computer. This virtual sensor is of the type Proxy, so there are SpeedProxies, PositionProxies, etc. Every Proxy communicates with a Server, which is its counterpart on the other side of the network. A simple connection between a user of data, e.g. a Matlab program, and a sensor, will be extended by a Proxy-Server-pair as illustrated in Figure 3.7. In this example, a speed-measurement is sent from the GpsSpeedSensor (which listens to a physical sensor) to the SpeedServer. The server sends the package over a network connection, where a matching proxy receives it. Since all servers broadcast their data on the network, proxies on several different computers can pick it up. A Matlab control program asks the SpeedProxy for the current speed, determines a new set speed, and sends it to the VehicleProxy. Here the data is again sent over the network, received by the VehicleServer, which in turn passes it on to the Vehicle object. This object is responsible for adjusting the speed on the physical vehicle. Mobile computer GpsSpeedSensor

Vehicle

Current speed

New set speed

SpeedServer

VehicleServer

WLAN

Stationary computer SpeedProxy

VehicleProxy New set speed

Current speed

Matlab control loop

Figure 3.7: With Server-Proxy-pairs, the system can be used with two or more computers, connected via a network. The Proxy acts as a virtual module, so the user does not have to know whether a network is used or not. Since a SpeedProxy also inherits all methods from the SpeedSensor class, it can be regarded as a sensor in its own right, although it is only a virtual sensor. This is one of the fundamental characteristics of this proposed architecture: 56

3.4 Software platform modules that are not sensors can act as if they were. The concept of virtual sensors makes it possible to locate sensors and users of data on different computers without the user of the data ever having to know this. This places special demands on the Server-Proxy-pair, since it has to deliver that data as swiftly as possible, and also implement the routines for getting and setting parameters over the network. Reorganizing a system from a single-computer operation to several computers involves loading proxies instead of sensors on the ”user-computer”, and loading servers instead of users on the ”sensor-computer”. No changes have to be made to either user or sensor code; all is accomplished by modifying configuration files as described in Section 3.4.7. Communication between separate computers is done by Ethernet network, either directly through a cable or via a Wireless Local Area Network (WLAN). For this, the standard network equipment for wireless PC:s and laptops with speeds from 11 up to 54 MBps (IEEE 802.11g) is used. The WLAN is used for controlling the vehicle, but since the communication handling is transparent to the system, debug and in-office tests can be done by either a cable or direct communication within the computer. The network communication uses datagrams (by the Internet UDP protocol), i.e. small packets of data transmitted with no control over their arrival, and therefore no acknowledgment of received packets is obtained. The alternative would be TCP streams, which guarantee the order, integrity, and completeness of the data. The reason for this choice is threefold: 1. Datagrams can be broadcast to more than one receiver; so several computers can “listen-in” on data from the sensors. 2. Datagrams delineate the data on the network. The data is conveniently seen as small units, as opposed to a data stream, where the software would have to find the beginning and end of each data packet. 3. With datagrams, lost packets are quickly replaced by new data. A streaming model would resend lost packets, but would also impose a variable delay, and with high data rates there would be new data available by the time the original one arrives. TCP also uses bigger packets. Small packets have a greater chance of getting through the network, and the loss of a few packets is normally tolerable. Up-to-date data is often more important than a complete data stream. For protection against a complete loss of control should the network fail, there are several timeouts built into the system.

3.4.7

Configuration

The software drivers for sensors, actuators, and target vehicles have parameters describing their function. For a sensor, this may include sampling rate, amplification level, network address, and pose. Furthermore, the integration of a target vehicle, sensors, and actuators into a complete system has to be handled 57

Development of an Autonomous Forest Machine in a flexible way. To facilitate this, every module has an associated initialization file with properties that control the module’s behavior. 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 gain for a specific sensor; when a suitable value is found, the sensor can store it in its initialization file. The next time the sensor is run, it automatically uses the saved value. The files are stored in separate file folders, one per configuration. Together with the initialization files there is a configuration file that describes the system (choice of target vehicles, sensors, actuators, etc.) and which modules to load. Usually this startup configuration file is stored together with the initialization files for the modules to load in that particular configuration. To start the system, a small boot-loader program reads the configuration from a file, and proceeds to load the appropriate modules. The user can select the configuration to load from a menu, or its name could be hard-coded into the boot loader. In this way, different versions (choices of sensors, filters, etc.) can be easily available during development. The forest machine system contains more than 100 different modules, with initialization files. To facilitate changes and provide an overview a graphical configuration manager has been developed. The configuration manager gives the user an overview of how every module is connected to other modules. It is possible to reroute connections, add new modules, duplicate existing ones, or remove them. One example showing a small part of the modules in the forest machine project is shown in Figure 3.8. The configuration manager can also be used to modify individual properties for the module (i.e initialization files), as shown in Figure 3.9.

3.4.8

Health Monitoring and other support systems

The system has a powerful set of support systems. Among the more important are the Health Monitor and the Logging System. The latter uses a class hierarchy for the output of log data, with Logger as the base class. The most common output channel is a log file (accomplished by the FileLogger), but by substituting other classes the logging can be rerouted to a database, or a memory buffer if time tests demand a low time loss impact from the logging. Logging can be enabled and disabled at a global level and also at the module level, by using instructions in the initialization files. A log record has a number of columns that can be turned on or off. They contain actual time, time elapsed since the start of the log, class, method, and line number of the logging instruction in the code. The columns can also contain a comment supplied in the argument to the log command, and a list of the enabling flags in effect. These flags make it possible to turn on or off a special type of logging event, e.g. UPDATE for data broadcasting, NETWORK for network events, EXCEPTION for error reporting, and LOWLEVEL for detailed logging of, for instance, character-by-character processing in a communications module. The actual logging command takes such 58

3.4 Software platform

Figure 3.8: A graphical configuration manager facilitates changes and provides an overview of all loaded modules in the system. Each module can have several attributes that can refer to other modules. In this example, the PositionSensorSelector, named positionsensor, is able to select one out of four sensors to listen to. The PositionServer listens to the selected sensor via positionsensor, and transmits the data over a network to a corresponding proxy.

Figure 3.9: A module’s properties, in this case the GPS position sensor, can be viewed and edited within the configuration manager. The mounting pose of the sensor, update frequency, and the level of debugging are some of the available properties.

59

Development of an Autonomous Forest Machine a flag as an argument and only contributes to the output if the corresponding flag is enabled. Reliable procedures for checking the status of both sensors and actuators become increasingly important as the complexity of robot systems grows. The NAV2000 system will in itself increase the need for such procedures, since it introduces new levels of abstraction between the hardware and the user program, and also since it offers flexible and configurable setups. The use of timeouts, ”watch-dogs”, and ”heart-beats” addresses the same need, but not sufficiently for a complex system with numerous asynchronous communication channels and interconnected subsystems. To accomplish sufficient health monitoring, a dedicated system keeps track of all modules loaded onto the local computer, and also communicates with health monitors in all other computers running the system. The overall health of every module as well as a more detailed description of possible problems is available, as seen in Figure 3.10. Error types fall into several groups, of which hardware timeouts (lost communication with sensors), configuration errors, hardware errors, and network problems are the most prominent. From the data collected by this system it is also possible to get statistics on the performance of the system, mainly the time delay from a measurement until the data arrives at its final destination. The Health Monitor is essential for both development and usage of a complex robotics system. During development, important information regarding missing sensors or incorrect configurations can be retrieved. During the usage of a ready-build robot, the Health Monitor can be used to detect malfunctioning sensors, actuators, and other equipment.

Figure 3.10: The Health Monitor system keeps track of all loaded modules in the system, and warns if any error occurs. A filled icon in front of a module’s name means that it functions properly. If the icon is striped, the module has some problem. Details about the cause of the problem are displayed when clicking on the module’s name.

60

3.4 Software platform

3.4.9

Results

The NAV2000 system is in daily use in the development and research work in the forest machine project. As described in Section 3.1.2, three different target machines are interchangeably used and a number of varying configurations of sensors and actuators are applied for testing and system integration. Currently the system consists of over 300 different classes. Figure 3.11 shows the average time delay over a network. Because the two computers involved do not have the same time (even if we try to synchronize them), the time it takes to send a package over the network and back again was measured. Half this time is the delay over the network. For about 70% of the tested packages, the delay was less than 2 milliseconds and it never exceeded 24 milliseconds in our tests. 70

Number of occurences [%]

60

50

40

30

20

10

0 0

5

10

15

20

25

Delay [ms]

Figure 3.11: The average time delay over a network measured by sending a package over the network and back again. For about 70% of the tested packages, the delay is less than 2 milliseconds. The maximum delay of the 135 packages sent over the network in this test was 24 milliseconds. Another important performance issue is the number of packages lost over the network. This is measured by analyzing packages containing a serial number. Each time new data arrives the serial number is increased by one. As Figure 3.12 shows, it is fairly uncommon that a package is lost over the network (over 97% of the packages arrive safely). To lose two consecutive packages or more is even less likely (0.2% of all packages in this test). The proposed software structure has proved to be a consistent and powerful tool for the research work. It will be further extended and improved, and will serve as a general basis for our future work with autonomous vehicles and robots. 61

Development of an Autonomous Forest Machine 100 90 Number of occurences [%]

80 70 60 50 40 30 20 10 0

0

1 Packages lost

2

Figure 3.12: It is fairly uncommon that a package is lost over the network (over 97% of the packages arrive safely). To lose two consecutive packages or more is even less likely.

3.5

Sensors and sensor data processing

For an autonomous vehicle to be of any use it must be able to move through the terrain without hitting any obstacles and without requiring any human intervention. For this to work, the vehicle must know where it is and where it is going. To avoid any nearby obstacles, it must obviously be able to detect them first. This means that the vehicle must have sensors that can detect its pose (position and heading), as well as objects in the surrounding terrain. Often several different sensors are used to increase the accuracy and reliability of the sensor reading. Several position sensors may, for example, be combined to get a more accurate estimation of the vehicle’s position. Another use for several sensors is as backup. If one sensor’s accuracy becomes too low, another sensor’s reading (or a combination of readings from several senors) may be used. Algorithms fusing data from several sensors becomes necessary to interpret the data correctly and decide which sensor’s readings to use. This section describes the senors used in the project and some algorithms for fusing data from several sensors. The most important sensors used are: • Satellite navigation (GPS) - used for position and heading estimation. • Wheel odometry - position and heading. • Gyro - gives heading, roll and pitch. 62

3.5 Sensors and sensor data processing • Laser scanner - used both to provide position and heading and detect obstacles. • Doppler Radar - detects a very close by obstacle. Can be used as emergency stop. • Sequential Lobing Radar - able to simultaneously track 10 obstacles. The advantages, disadvantages, and some explanations of the reasons to have chosen these sensors are presented in this section. Virtual sensors and techniques for fusing data from different sensors developed in the project are: • Wheel odometry enhanced with neural networks - improves performance of the wheel odometry. • Laser based localization - a localization technique using the laser scanner mentioned above. • Kalman filter - estimates heading and position by fusing several different sensors. • Sensor switch - switches to another sensor when the quality of the GPS readings deteriorates. • Pose transformation - transforms sensor readings between different coordinate systems. • Occupancy grid - fuses data from several different obstacle sensors.

3.5.1

Position sensors

In addition to a very accurate GPS receiver, wheel odometry and laser-based localization are used as secondary sensors to determine the position of the vehicle. We have also implemented a neural network to increase the accuracy of wheel odometry. 3.5.1.1

Sensor evaluation procedure

As mentioned above, a number of position sensors has been tested and evaluated. To test the sensors, the vehicle was driven along a path while recording all sensor data, including GPS position. As long as the GPS has a fix solution (the ambiguities in the phase signal have been resolved as described in Section 2.3.2), it can be regarded as “ground truth” for estimation of the error in the other position sensors. To estimate the error we could expect in the various sensors, results based on the following equations are presented. The values plotted at time T , errT , are averages of all drifts for periods of length T , i.e the drift we can expect after a time T following a GPS failure, refer to Equations 3.1 to 3.4. 63

Development of an Autonomous Forest Machine ∆GP St,T = GP St+T − GP St ∆Odot,T = Odot+T − Odot drif tt,T = ||∆GP St,T − ∆Odot,T ||

errT = where GP St : Odot : errT : NT : 3.5.1.2

NT X

(3.1) (3.2) (3.3)

drif tt,T

t=1

NT

(3.4)

GPS-position at time t (x, y) Odometry-position at time t (x, y) Average of all drifts for periods of length T Total number computed values of drif tt,T . Satellite Navigation

The main position sensor is a Real-Time Kinematics Differential GPS (RTK DGPS) from Javad. RTK means that the receiver measures distances to the satellites with carrier phase, compared to measuring on the code phase as in ordinary GPS applications. DGPS means that a stationary GPS receiver is connected by a radio link to a mobile GPS receiver, see Figure 3.3. Correction signals for timing, ionospheric, and tropospheric errors are transmitted by radio from the stationary to the mobile GPS receiver, resulting in a centimeter accuracy under ideal conditions. The Javad receiver is capable of receiving signals from both the American GPS system and the Russian GLONASS system. While providing lower accuracy than GPS, GLONASS provides important backup, especially at the high latitudes (64 degrees north) where the work has been conducted, thanks to the inclination angle of 64.8 degrees. In Appendix A, test results with the GPS receiver, as well as some arguments why the Javad system was chosen, are presented. 3.5.1.3

Laser-based localization

As described in Section 2.3.1, a laser scanner can be used for localization. We have implemented a relative laser-based localization, or laser odometry based on an algorithm by Selkäinaho [70] with some modifications. The vehicle’s pose is denoted by (xr , yr , θr ), where θr is the heading and xr, yr is the position. A laser scan comprises M rays emitted at different angles. Each ray is characterized by (βi , ri ), where βi is the angle of emission, and ri is the distance to the closest object in this direction, which is the value returned by the laser. Hence, a scan s can be represented by a vector {rjs , j = 1, M }. The global Cartesian coordinates for the object corresponding to each rj are given by xsj =rjs ∗ cos(βj − θr ) + xr yjs =rjs ∗ sin(βj − θr ) + yr . 64

(3.5)

3.5 Sensors and sensor data processing Given two scans {rj1 , j = 1, M } and {rj2 , j = 1, M }, measured at two nearby positions, the following method can be used to estimate the change in pose between the two positions: Find the pose change (∆x, ∆y, ∆θ) that, when applied to scan 2, maximizes N, defined as the number of objects identified at the same or almost same coordinates, in both scan 1 and the transformed scan 2: (∆x, ∆y, ∆θ) = arg max N (∆x, ∆y, ∆θ) N (∆x, ∆y, ∆θ) =

M P

nk . k=1

(3.6) (3.7)

nk denotes the number of pairs of rays in the two scans that point at roughly the same global Cartesian coordinates, and is defined as nk = 1 if ∃j, (|x1k − x∗j | + |yk1 − yj∗ |) < dk nk = 0 othervise.

(3.8)

The points {(x∗j yj∗ ), j = 1, M } correspond to scan 2 translated and rotated by the pose change (∆x, ∆y, ∆θ). I.e., (x∗j yj∗ ) can be computed by x∗j = r2j ∗ cos(βj − θr − ∆θ) + xr + ∆x yj∗ = r2j ∗ sin(βj − θr − ∆θ) + yr + ∆y.

(3.9)

dk is the tolerance for matching points and is computed as dk = c1 + c2 ∗ rk1

(3.10)

where c1 , c2 are constants. Typical values for these are c1 = 3 and c2 = 0.03. Equation 3.10 depends on the measured distance rk1 to reflect how the laser sensor resolution depends on distance. To solve the maximization problem in Equation 3.6, different techniques can be used. In the implemented algorithm, a simple coordinate search along the three dimensions x, y, and θ is employed. Once optimal values for ∆x, ∆y and ∆θ have been computed by solving Equation 3.10, the new pose (xn , yn , θn ) of the robot can be estimated as xn yn θn

= xr + ∆x = yr + ∆y = θr + ∆θ

(3.11)

An algorithm to calculate a pose can be formed as described in Algorithm 3. The accuracy of the laser odometry decreases over time as illustrated in Figure 3.13. The (a)-figure shows the positions for GPS and laser odometry 65

Development of an Autonomous Forest Machine Algorithm 3 Pixel based localization 1. Determine the start and end boundaries for ∆x, ∆y, and ∆θ. 2. Set the search step resolution for position ∆s and heading ∆b for the search operation. 3. In steps of ∆b, search for the value of ∆θ that gives the maximum number of matching points N in Equation 3.6. 4. Use the found value for ∆θ. In steps of ∆s, search for the value of ∆x that gives the maximum number of points N in Equation 3.6. 5. Use the found values for ∆θ and ∆x. In steps of ∆s, search for the value of ∆y that gives the maximum number of points N in Equation 3.6. 6. Iterate step 1-5 with smaller search space and smaller step resolutions until the best possible match has been found. 7. Update the vehicle pose according to Equation 3.11.

respectively. The (b)-figure shows how the difference (error) increases over time, refer to Equations 3.1 to 3.4. Initial tests on the forest machine show that the laser odometry performs slightly worse than the wheel odometry in 7 different tests, as shown in Figure 3.14. We have found that the laser odometry is sensitive to the large movement of the forest machine’s front end, occurring when the steering angle is changed. This could be part of the reason why the laser odometry does not perform as well as could be expected. The tests done so far were conducted in an environment with brushwood forest, which may not be representative for a normal forest environment with larger trees. Whether or not the laser odometry performs better in a real forest environment remains to be seen. 3.5.1.4

Wheel odometry

The forest machine delivers velocity and steering angle, but for the path tracking we need to know the position and heading of the vehicle. To calculate this, the forward kinematic equations for the vehicle must first be determined: given an initial pose (x, y, θ), a vehicle speed v, and a steering angle φ at time t, determine the pose (x0 , y 0 , θ0 ) at time t + ∆t. For slip-free motion, all wheels roll in full contact with the ground, in a direction perpendicular to its axis of rotation. The distance covered can be computed from the size and rotational speed of the wheel. For many reasons, this is an idealized situation. Slip is often significant and furthermore, hard to model. For this reason, slip-free motion is often assumed when solving the kind of problems stated above. For a multi-wheel vehicle with no slip, the intersection of the continuation 66

3.5 Sensors and sensor data processing

Position error in odometry after a time T

Odometry position compared to GPS 0

GPS Laser odometry

4

−5 3.5 Position error [m]

y (north) [m]

−10 −15 −20 −25

3 2.5 2 1.5 1

−30

0.5

−35 −30

−20 −10 x (east) [m]

0 0

0

20

40 Time [s]

60

80

(a) Position from GPS and laser odometry (b) Mean difference between the position from laser odometry and GPS after a time T.

Figure 3.13: Laser odometry position compared to position from the GPS. The average position error grows with time, and after 80 seconds, the error is about 4.5 meters in this test.

30 Wheel odometry Laser odometry 25

Time [s]

20 15 10 5 0 1

2

3

4 Run number

5

6

7

Figure 3.14: Time before the average position error reaches 1 meter for the laser odometry and wheel odometry in 7 different tests. The average time before the error reaches 1 meter is 9s for laser odometry, compared to 12s for wheel odometry.

67

Development of an Autonomous Forest Machine of the wheel axes is the center point for rotation when the vehicle moves. This point is referred to as ICC (Instantaneous Center of Curvature) or ICR (Instantaneous Center of Rotation). In many cases, a totally slip-free motion is not geometrically possible. The situation for an articulated vehicle with 4-wheel axes is illustrated in Figure 3.15.

t on Fr

(a+ r f=

φ tan φ)/ s o b/c

a

φ φ

b

b tan

sφ co

b/cos φ)/sin φ-

b/

rr =(a+

φ

b tan φ Rear

Figure 3.15: Derivation of radius of rotation for an articulated vehicle with steering angle φ, front length a, and rear length b. The varying steering angle φ makes it impossible to construct the vehicle so the axes intersect at one point. To avoid modeling slip, a common approach is to assume two virtual axes located in between the real axes in the front and rear parts of the vehicle. Another complication is the width of the wheels. The outer part of a wheel is bound to travel a longer distance than the inner part in all curves. Hence, they will slip. Furthermore, the speeds for all wheels have to be controlled so slip-free motion is even approximately possible. The outermost wheels have to rotate faster than the innermost ones, and the rear wheels have to rotate more slowly than the front wheels (assuming a longer rear part as in Figure 3.15). Depending on the mechanical construction and the control system of the vehicle, this is a more or less valid assumption. In any case, the following kinematics equations are derived with all these idealized assumptions. The center of each (virtual) axis rotates around ICC along a circle with radius r. For an articulated vehicle, r is given by the geometry of the vehicle and the steering angle as illustrated in Figure 3.15. For the front axis, the radius rf is given by 68

3.5 Sensors and sensor data processing

rf =

a+

b cos φ

.

(3.12)

− b tan φ.

(3.13)

tan φ

For the rear axis, the radius rr is given by rr =

a+

b cos φ

sin φ

Under the assumptions above, it suffices to study the motion of the front part of the vehicle, since the motion of the rear part is given by the geometry of the vehicle. Given a vehicle pose (x, y, θ) measured at the middle of the virtual front axis at time t, the coordinates (XICC , YICC ) for ICC are given by (3.14)

[XICC , YICC ] = [x − r sin θ, y + r cos θ]

where r for simplicity of notation denotes the radius rf . A motion from pose (x, y, θ) at time t to pose (x,0 y 0 , θ0 ) at time t + ∆t is illustrated in Figure 3.16.

θ’ (x’,y’)

r

(x,y)

θ

180-θ

r

r cos(180-θ) = -r cos(θ)

ω∆t θ-90 (XICC, YICC)

r cos(θ-90) = r sin(θ)

Figure 3.16: Rotation around ICC by an angle ω∆t. The vehicle pose changes from (x, y, θ) to pose (x,0 y 0 , θ0 ) expressed in a global coordinate system (top left). The coordinates of ICC are (x − r sin θ, y + r cos θ). Since the vehicle moves along a circle, it is useful to have an expression for the angular velocity ω defined as 2π/T (with the unit radians/second), where T is the time it would take to complete one full turn around ICC. The known vehicle speed v is assumed to be the speed at which the midpoint of the front axis moves (this is an additional assumption that may be insufficiently valid). It can thus be expressed as 2πr/T , which gives the following expression for ω: ω= 69

v . r

(3.15)

Development of an Autonomous Forest Machine The new position (x0 , y 0 ) at t + ∆t is computed by a 2-D rotation of the point (x, y) by ω∆t radians around the point ICC: x0 = cos(ω∆t)(x − XICC ) − sin(ω∆t)(y − YICC ) + XICC y 0 = sin(ω∆t)(x − XICC ) − cos(ω∆t)(y − YICC ) + YICC .

(3.16)

This can be derived further by substituting XICC : x0 = cos(ω∆t)r sin(θ) − sin(ω∆t)r cos(θ) − r sin(θ) + x y 0 = sin(ω∆t)r sin(θ) − cos(ω∆t)r cos(θ) + r cos(θ) + y.

(3.17)

The new heading θ0 at t + ∆t is given by θ0 = ω∆t + θ + c∆φ

(3.18)

where c is a constant (c ∈ [0, 1]) describing how much the heading changes momentarily when changing steering angle. This constant can actually vary depending on different ground conditions and the load of the vehicle. Preliminary tests show that no single fixed value for c works for all driving conditions. We have not investigated this any further, but the neural network, described in Section 3.5.1.5, include ∆φ as one input and can among other factors compensate for the c-value. In the results presented for the basic wheel odometry, c = 0 has been used. Since the turning radius r from Equation 3.12 is undefined for φ = 0, i.e when the vehicle is driving straight ahead, different equations have to be used to calculate the new pose in this case: x0 = v∆t cos(θ) + x y 0 = v∆t sin(θ) + y θ0 = θ.

(3.19)

These equations can form the basis for wheel odometry given speed estimates v and measured steering angles φ. However, as mentioned above, the derivation makes several dubious assumptions. These can be summarized as: 1. Assuming slip-free motion (ignoring geometrical impossibilities, tires with non-zero width, inconsistent front and rear wheel speed, and slippery ground conditions) 2. The derivation of the equations uses two virtual wheel axes located in between the real wheel axes. 3. The speed value v is assumed to be the speed of the midpoint of the front axis. The available speed value is the estimated vehicle speed, based on the engine rpm and the transmission. This is not necessarily the same as the speed value v used in the kinematic equations. 70

3.5 Sensors and sensor data processing The accuracy of the wheel-odometry position is low as can be seen in Figure 3.17. The (a)-figure compares position for GPS and wheel odometry. The (b)figure shows how the difference (error) increases over time, refer to Equations 3.1 to 3.4. We can see that after 10 seconds, the error is about 1 meter and after 80 seconds, almost 5 meters. The average time before the error was larger than 1 meter in 44 different tests was 3 seconds. The conclusion is that wheel odometry is an unreliable source for position in a forest environment. Position error in odometry after a time T

Odometry position compared to GPS 0

5

GPS Wheel odometry

−5

4.5 4 Position error [m]

y (north) [m]

−10 −15 −20 −25

3.5 3 2.5 2 1.5

−30

1

−35 −40

0.5

−30

−20 −10 x (east) [m]

0 0

0

20

40 Time [s]

60

80

(a) Position from GPS and wheel odometry (b) Mean difference between the position from wheel odometry and GPS after a time T.

Figure 3.17: Wheel odometry position compared to position from the GPS. After 10 seconds, the error is about 1 meter and after 80 seconds, more than 5 meters. This test was performed on a flat surface with minimal slip.

3.5.1.5

Wheel odometry with neural networks

As mentioned above, a number of assumptions had to be made to be able to derive the equations for wheel odometry. Factors, such as wheel slippage and the fact that the vehicle behaves differently depending on ground conditions and the load on the vehicle, make it difficult to calculate the vehicle’s position with enough accuracy. To increase the accuracy of wheel odometry, we replaced the odometry equations described in Section 3.5.1.4 with two neural networks, one for delta heading and one for delta distance (∆θnn , ∆dnn ) as shown in Figure 3.18. The inputs to the networks are the same as to the odometry equations plus heading from the gyro: φ : Steering angle ∆φ : Delta steering angle v : Velocity ∆t : Delta time ∆θgyro : Delta heading from the gyro. The networks were trained with ∆θgps and ∆dgps , i.e the delta heading and 71

Development of an Autonomous Forest Machine ∆φ NN1

φ

∆θ nn

v ∆t

NN2

∆d nn

∆θ gyro

Figure 3.18: By replacing the equations for wheel odometry with two neural networks we are able to increase accuracy. One network calculates delta distance and the other delta heading. Further tests showed that using heading from the gyro was better than the one from the neural network. distance from the GPS. Best results were reached when using a feed-forward backpropagation network with 1 hidden layer consisting of two nodes, as illustrated in Figure 3.19. The hidden layer uses a hyperbolic tangent sigmoid transfer function and the output layer uses a linear transfer function. The network was trained using the Levenberg-Marquardt backpropagation algorithm [27]. The data (collected under about 8.5 minutes, corresponding to 3300 data points) was divided into three parts: the first half for training, third quarter for validation, and last quarter for test. Each network was trained 10 times, and the one with best performance on the test part of the data was selected. The performance of the selected networks was then checked with the validation data to verify the results. Best result was obtained when using a fixed number of epochs (50 for distance and 25 for heading).

∆φ

φ ∆d nn

v ∆t

∆θ gyro Figure 3.19: Best results were reached by using a feed-forward backpropagation network with 1 hidden layer consisting of two nodes. The hidden layer uses a hyperbolic tangent sigmoid transfer function and the output layer uses a linear transfer function. The network is then trained using the Levenberg-Marquardt backpropagation algorithm. The network illustrated in this figure is the one calculating delta distance. Assuming an initial pose (0, 0, 0) at time 0, the pose (x, y, θ) at time t, is 72

3.5 Sensors and sensor data processing given by: t

θ(t) =

X

∆θnn (k)

(3.20)

k=1

∆x(t) = ∆dnn (t) cos(θ(t)) ∆y(t) = ∆dnn (t) sin(θ(t))

(3.21)

t

x(t) =

X

∆x(k)

k=1

(3.22)

t

y(t) =

X

∆y(k).

k=1

Equation 3.21 assumes that the vehicle moves in the direction of its heading, which is an approximation. A more correct direction would be the angular component of the polar coordinates for the vehicle translation. We tried using a neural network to calculate this component, but the position data from the GPS turned out to be too noisy to be useful for training a network properly. The neural networks are able to calculate ∆d and ∆θ more accurately than the odometry equations. However, further tests showed that the resulting position becomes even more accurate by using ∆θgyro instead of ∆θnn in Equation 3.20, i.e using delta heading from the gyro instead of the neural network. To test the trained networks, we applied them on 44 different runs with between 80 and 3300 data points (the same runs used as reference for the other position and heading sensors except the laser odometry). Figure 3.20 shows the time before the average position error reaches 1 meter, when using the standard odometry equations compared to using the neural network combined with gyro heading, in the 44 different runs. On rare occasions (< 1% in our tests) the neural network performs worse than wheel odometry. We have not been able to deduce why the performance is poorer in these particular tests compared to others. Most times however, the neural network performs much better than standard odometry. The average time before the error reached 1 meter was 13 seconds for the neural network, compared to 3 seconds for the uncompensated wheel odometry. For some tests, a 1-meter error was never observed. Therefore, the computed 13 seconds average error value is an underestimation of the real value. The conclusion is that by replacing the equations for wheel odometry with a neural network calculating the distance traveled, combined with the heading from a gyro, the time odometry can be used before the error becomes too large can be increased. A possible source of error is that the training data may not be representative enough for the neural Network to handle all possible situations (e.g. different terrain conditions make the vehicle behave differently). Slightly different mounting position of the GPS antennas between different tests may also have had a negative impact on the results. 73

Development of an Autonomous Forest Machine 45

Neural Network Wheel odometry

40 35

Time [s]

30 25 20 15 10 5 0 0

10

20 Run number

30

40

Figure 3.20: Time before the average position error reaches 1 meter for the neural network and wheel odometry, in several different tests. No value means that the error never reached 1 meter. The average time before the error reaches 1 meter is 13 seconds for the neural network, compared to 3s for the uncompensated wheel odometry. 3.5.1.6

Conclusions

The GPS gives a very accurate position as long as it has a position fix solution. The standard wheel odometry has low accuracy, but can be improved by using a neural network calculating the distance traveled combined with heading from a gyro. Accepting a 1-meter tracking error, the neural network gives sufficiently good position estimates for 10-20 seconds of autonomous operation. Initial tests with the laser odometry show about the same performance as the wheel odometry or even less accurate. Further tests in a real forest environment must be done to verify this result, but it will probably not improve much. This conclusion is based on the fact that the laser odometry assumes a 2-dimensional plane. Due to the ground conditions in off-road terrain, the laser will move up and down considerably between scans. This will result in an incorrect estimation of the range to each obstacle, and thereby an incorrect estimation of the change in the vehicle’s pose. Table 3.1 summarizes the performance of the different localization techniques.

3.5.2

Heading sensors

To navigate safely we have to know not only the position of the vehicle, but also the direction it is moving in. This can be calculated by wheel odometry or measured with different sensors. In our project, GPS, gyro, laser scanner, and wheel odometry have been used to acquire a heading. To estimate the 74

3.5 Sensors and sensor data processing Localization technique RTK DGPS Laser odometry Wheel odometry Neural network

Time to 1 meter error [s] Test 1 Test 2 Inf Inf 9 3 12 13 22

Table 3.1: Performance of the different techniques for localization. The table shows the average time in seconds before the error grows to be larger than 1 meter. Test 1 consists of 44 different runs, but does not include laser odometry. Test 2 consists of only 7 different runs, but includes all techniques. The reason that all techniques performed better in Test 2 is due to the few samples (7 runs) and that the ground was not slippery in that test. best heading possible, sensor fusion can be used to combine data from different sensors, as described in Section 3.5.4. 3.5.2.1

Sensor evaluation procedure

The evaluation procedure for heading sensors is the same as the one described for position sensors in the previous section, but with slightly different equations. The heading value provided by the GPS system, GP St , is regarded as ground truth. The values plotted at time T, errT , are averages of all drifts for periods of length T, i.e the drift we can expect after a time T following a GPS failure, refer to Equations 3.23 to 3.26. ∆GP St,T = GP St+T − GP St ∆Odot,T = Odot+T − Odot drif tt,T = |∆GP St,T − ∆Odot,T |

errT = where GP St : Odot : errT : NT : 3.5.2.2

NT X

(3.23) (3.24) (3.25)

drif tt,T

t=1

NT

(3.26)

GPS-heading at time t Odometry-heading at time t Average of all drifts for periods of length T Total number of computed values of drif tt,T . GPS heading

In addition to position, the GPS can be used to give the vehicle’s heading. Normally, the GPS uses the difference between two consecutive positions to calculate a heading. This method gives quite low accuracy, especially when 75

Development of an Autonomous Forest Machine standing still, as can be seen in Figure 3.21 a). To increase the accuracy, an extra GPS receiver can be used. In this case, the GPS calculates the heading based on simultaneous position estimates from two GPS antennas placed on the vehicle. In Figure 3.21 b), the heading from the GPS using the two-antenna system, with the antennas placed 30 centimeters apart from each other, can be seen. The noise in the heading reading is quite small here (approximately ±1◦ ), and no difference can be seen between the vehicle at a standstill and in movement. The accuracy can be improved further by increasing the distance separating the antennas (approximately ±0.3◦ with 1m separation according to the manufacturer). 400

277

350

276 Heading (deg)

Heading [deg]

300 250 200 150

275 274 273

100 272

50 0 0

5

10

15 Time [s]

20

25

30

(a) One antenna.

271 0

5

10

15 Time (s)

20

25

30

(b) Two antennas.

Figure 3.21: Heading from the GPS while moving straight ahead with the Pioneer robot. The heading is very inaccurate when using only one GPS antenna, as can be seen in (a). When the vehicle stands still, as it does in the beginning and the end in this graph, the heading is more or less random. In the middle of the graph, the vehicle moves forward, but the heading still fluctuates about ±10◦ around the true heading. Using two GPS antennas, placed 30 centimeters apart from each other, results in considerably higher accuracy (approximately ±1◦ ), as shown in (b). The noise in the heading reading is very small and no difference can be seen between the vehicle at a standstill and in movement. The accuracy can be improved further by increasing the distance separating the antennas.

3.5.2.3

Gyro/Compass

To estimate the attitude in general (refer to Section 3.3.2) and heading in particular an electronic compass/gyro from Crossbow Technology is used. The model, AHRS 400CC, is a 9-axis measurement system with linear accelerometers, rotational rate sensors, and magnetometers. The three angular rate sensors consist of vibrating ceramic plates that utilize the Coriolis force to output angular rate independently of acceleration. The three MEMS (Micro ElectroMechanical Sensors) accelerometers are surface micro-machined silicon devices 76

3.5 Sensors and sensor data processing that use differential capacitance to sense acceleration. The magnetic sensors are state-of-the-art miniature fluxgate sensors. Regarding the accelerometers, it is desirable to place the device as close to the center point of the vehicle rotation as possible. For a forest machine this is as close to the ground as possible. For this reason, the AHRS device was initially placed on the floor in the driver’s cabin. After calibration, the magnetic field vectors in x and y directions from the magnetometers appeared as shown in the upper part of Figure 3.22. In an ideal case, the figure should have been a circle centered at the origin. The tilted elliptical shape of the response is due to soft iron effects (i.e. the magnetic field lines are altered by magnetically soft material around the compass). The fact that the response is not centered at origin is due to hard iron effects (i.e. the magnetic field lines are set off by magnets and magnetized metal around the compass). The AHRS device is supposed to eliminate soft and hard iron effects by calibration, but due to the periodical disturbance that shows up as epicycles in the figure, the built-in calibration does not work. The epicyclic disturbance stems from the transmission of the forest machine, and since the AHRS device was placed on the driver cabin’s floor, it was rather close to the rotating transmission. The frequency of the disturbance is consistent with the frequency of the transmission. Since the amount of magnetic metals is several magnitudes higher in a forest machine than in a small-sized airplane, for which the AHRS device was originally constructed, the conclusion is that great care has to be taken in the positioning of the device. After moving the device about 2 meters vertically and placing it on the roof of the forest machine, the field vectors appear as in the lower part of Figure 3.22. The disturbance is still visible but the amplitude has been drastically reduced. After further tests, we determined that the compass was too unreliable, as the heading tends to drift over time. Instead, a method to calculate heading from the gyro’s yaw rate (the heading’s angular velocity) readings was implemented: N P

yawRate(i) i=1 rateSum ∆η = · dt N

rateSum =

where: ∆η : N: dt :

(3.27) (3.28)

Delta heading Number of samples (depending on dt) Sample time (50 ms in the current implementation).

In Figure 3.23 a), the gyro heading is compared to the heading from the GPS while driving on a flat surface with the forest machine (to minimize the effects of vehicle tilt influencing the readings). In the b) part of Figure 3.23, the heading error after a time T can be seen (refer to Equations 3.23 to 3.26). The error does not increase significantly over the approximately 90 seconds the 77

Development of an Autonomous Forest Machine

0.1

Y−magnetic [gauss]

0.05

0

−0.05

−0.1 −0.1

−0.05 0 0.05 X−magnetic [gauss]

0.1

(a) Cabin

0.1 0.08

Y−magnetic [gauss]

0.06 0.04 0.02 0 −0.02 −0.04 −0.06 −0.08 −0.1

−0.05 0 X−magnetic [gauss]

0.05

(b) Roof

Figure 3.22: The measured magnetic field vector a) in the cabin of the forest machine, and b) on the roof of the forest machine (from [23]). Both measurements are taken while completing at least a 360◦ turn.

78

3.5 Sensors and sensor data processing test lasts, so the drift is relatively small, as is the absolute error (< 3.5◦ in this example). In 44 different tests, the average time before the heading error grew larger than 5◦ was 51 seconds, which shows that a gyro can be used as backup for the GPS for almost a minute before the error becomes too large. Heading error in gyro after a time T

Odometry heading compared to GPS 400 GPS Gyro

3

Heading error [deg]

Heading (deg)

350

300

250

2.5 2 1.5 1

200 0.5

150 0

20

40 TimeStamp (s)

60

0 0

80

(a) Heading from gyro and GPS.

20

40 Time [s]

60

80

(b) Mean difference between the heading from gyro and GPS after a time T.

Figure 3.23: Gyro heading compared to heading from the GPS. The difference between the two sensors is relatively small (< 3.5◦ ) and the gyro does not have any large drift over the approximately 90 seconds the test lasts.

3.5.2.4

Laser-based localization

As described in Section 3.5.1.3, a laser scanner can be used to estimate the changes in position and heading. Figure 3.24 shows the accuracy of the heading estimates for the laser odometry, refer to Equations 3.23 to 3.26. Tests on 7 different runs show that the laser odometry has about the same accuracy as the wheel odometry, but is slightly less accurate than the gyro, refer to Figure 3.25. 3.5.2.5

Wheel odometry

If all else fails, we have to rely on wheel odometry to get the vehicle’s heading. For shorter periods, the error on the wheel odometry is relatively small, so with the fusing algorithms described in Section 3.5.4, this heading can be used when the main sensors deliver unreliable data (for example when the GPS loses contact with the satellites). The equations involved in calculating a heading from the steering angle and velocity delivered from the wheel odometry are described in Section 3.5.1.4. In Figure 3.26 the heading error after a time T can be seen (refer to Equations 3.23 to 3.26). The average time (computed from 44 different runs) to reach 1◦ error is 1 second. After 4.6 seconds it becomes larger than 5◦ . 79

Development of an Autonomous Forest Machine

Heading error in odometry after a time T 6

Odometry heading compared to GPS 280 GPS Laser odometry

5 Heading error [deg]

Heading (deg)

260

240

220

200

180 0

4

3

2

1

20

40 TimeStamp (s)

60

80

0 0

20

40 Time [s]

60

80

(a) Heading from laser odometry and GPS. (b) Mean difference between the heading from laser odometry and GPS after a time T.

Figure 3.24: Laser odometry heading compared to heading from the GPS for the same run as in Figure 3.13. After 5 seconds, the error is larger than 3◦ .

6

Wheel odometry Gyro Laser odometry

5

Time [s]

4 3 2 1 0 1

2

3

4 Run number

5

6

7

Figure 3.25: The laser odometry has about the same heading accuracy as the wheel odometry, but is slightly less accurate than the gyro. The figure shows the time before the average error reaches 1◦ for laser odometry, wheel odometry, and the gyro.

80

3.5 Sensors and sensor data processing

Heading error in odometry after a time T

Odometry heading compared to GPS 280

GPS Wheel odometry

20 Heading error [deg]

Heading (deg)

260

240

220

10

5

200

180 0

15

20

40 TimeStamp (s)

60

80

0 0

20

40 Time [s]

60

80

(a) Heading from wheel odometry and(b) Mean difference between the heading GPS. from wheel odometry and GPS after a time T.

Figure 3.26: Wheel odometry heading compared to heading from the GPS The heading error grows very fast and already after 5 seconds, the error is larger than 10◦ . The average time before the error exceeds 1◦ is 1s, and 4.6s before it becomes larger than 5◦ , based on 44 different runs.

3.5.2.6

Wheel odometry with neural networks.

As described in Section 3.5.1.5, an attempt was made to increase the accuracy of wheel odometry with a neural network. The changes in heading are modeled by a neural network as illustrated in Figure 3.18, and the heading at time t is estimated by Equation 3.20. As can be seen in Figure 3.27, the neural network does not improve the heading as much as the gyro does. This is a bit strange, since one of the inputs to the neural network is the gyro data. We have not investigated the reason for this in any detail, but one explanation could be a slight overfit of data to the other input variables. The average time before the error reaches 1◦ (based on 44 different runs) is 1.2 seconds for the neural network, compared to 1.0 second for the standard wheel odometry, and 3.4 seconds for the gyro.

3.5.2.7

Conclusions

The heading from the GPS is very accurate as long as it has a heading-fix solution. Using a gyro results in the best heading estimation if the GPS loses fix solution. With the gyro, the system can run without GPS for approximately 51 seconds before the error exceeds 5◦ . Both wheel odometry and laser-based localization give poor estimation of the heading. Using a neural network gives a very small increase in accuracy compared to wheel odometry and therefore is not used for heading estimation. Table 3.2 summarizes the performance of the different techniques for estimating heading. 81

Development of an Autonomous Forest Machine

25 Neural Network Wheel odometry Gyro

Time [s]

20

15

10

5

0 0

10

20 Run number

30

40

Figure 3.27: Time before the average heading error reaches 1◦ for the neural network, gyro, and wheel odometry in several different tests. No value means that the error never reached 1◦ . The average time before the error reaches 1◦ is 1.2s for the neural network, compared to 1.0s for the standard wheel odometry, and 3.4s for the gyro.

Localization technique RTK DGPS Laser odometry Wheel odometry Neural network Gyro

Time to 1 degree error [s] Test 1 Test 2 Inf Inf 0.7 1.0 0.6 1.2 0.8 3.4 2.6

Table 3.2: Performance of the different techniques for estimating the heading. The table shows the average time in seconds before the heading error grows larger than 1 degree. Test 1 is built up from 44 different runs, but does not include laser odometry. Test 2 is built up from just 7 different runs, but includes all techniques.

82

3.5 Sensors and sensor data processing

3.5.3

Obstacle sensors

Obstacle sensors can be used for two different purposes. The main usage is to detect obstacles around the vehicle so it can avoid them. The other purpose is to build a map of the environment that can be either local (around the vehicle) or global (the whole path). This map can be used for path planning or for localization. In the latter case, local maps can be used as “images” and be compared to the last map to calculate the relative movement from the previous pose. Another way is to save several maps along the recorded path and then compare each new map to the saved ones to calculate an absolute position. This section describes the main obstacle sensors used on the forest machine. 3.5.3.1

Laser scanner

For accurate detection of obstacles in front of the forest machine, an LMS 221 laser scanner (or Ladar) from SICK, shown in Figure 3.28, is used.

Figure 3.28: LMS 221 laser scanner from SICK. The LMS system operates by measuring the time of flight of laser light pulses. A pulsed laser beam is emitted from the scanner and reflected back if it meets an obstacle, and is then registered by the scanner’s receiver. The time between transmission and reception of the pulse is directly proportional to the distance to the obstacle. The pulsed laser beam is deflected by an internal rotation mirror, so a fan-shaped scan is made of the surrounding area. LMS 221 has a field of view of 180◦ and an angular resolution between 1 and 0.25 degrees. The maximum range at which the scanner can detect an obstacle depends on the reflectivity of the object: the more reflective the object, the further away the scanner can detect it. A tree has a reflectivity of about 40% (refer to Table 3.3), which means that the maximum range it can be detected at according to Figure 3.29 is about 60 meters. To make the scanner less sensitive to rain or snow, pixel-oriented evaluation can be used. This means that the scanner takes several consecutive scans in the same direction. By comparing these scans, it is able to determine whether there is a real obstacle in that spot, or it was something temporary like a snow flake. The main advantages with the LMS 83

Development of an Autonomous Forest Machine 221 laser scanner is the high accuracy (±35mm) and its capability of making an entire scan of 180◦ in between 13 ms and 53 ms (depending on the angular resolution).

Figure 3.29: Relationship between reflectivity (refer to Table 3.3) and range with the SICK LMS 221 laser scanner. Courtesy of SICK.

Material Cardboard, matt black Cardboard, gray Wood (raw pine, dirty) PVC, gray Paper, matt white Aluminum, anodized, black Steel, rust Steel, very shiny Reflectors

Reflectivity 10% 20% 40% 50% 80% 110...150% 120...150% 140...200% >2000%

Table 3.3: Reflectivity of different materials (KODAK standards). Courtesy of SICK.

3.5.3.2

Radar

One promising sensor for obstacle detection in a forest environment is the radar shown in Figure 3.30. The advantage of a radar (radio detection and ranging), over sensors based on ultrasound or light, is the ability to view obstacles reliably during bad weather conditions, like snow, fog or rain. One of the radar types used in our project is a pair of the Sequential Lobing Radar C1, produced by Tyco - M/A-COM, USA (primarily for the automotive industry). This radar works with 24 GHz frequency and is based on the monopulse theory 84

3.5 Sensors and sensor data processing [62]. Target range and bearing can be obtained by transmitting and receiving pulses. The range is estimated by matching the received pulse to an internally time-delayed one. An estimation of target-bearing can be obtained by using a receiver antenna with switchable lobe characteristics (Figure 3.31), S∆ (slightly more sensitive diagonally) and SΣ (mainly forward). The bearing is found by calculating the Additive Sensing Ratio, ASR, defined by ASR =

S∆ − SΣ S∆ + SΣ

(3.29)

and using a look-up table combined with phase information to distinguish between the two possible directions.

Figure 3.30: Sequential Lobing Radar C1, produced by Tyco.

Figure 3.31: Switchable antenna characteristics for the sequential lobing radar. The relative strength between the two patterns SΣ (solid line) and S∆ (dashed line) is used to compute the bearing to the target. The transmitted beam width is 8 degrees vertically and ±65 degrees horizontally, and the typical detection range is specified as 0.2-30 meters, with a 15-cm resolution. Maximum range depends on weather conditions, the object’s radar cross-section area, bearing, distance, and reflectivity. Objects containing metal or carbon are easiest to detect. The radar’s physical size is 103x71x32 mm, and the transmitted power is less than 17 dBm EIRP. A Digital Signal Processor (DSP) for simultaneous tracking of up to 10 targets is included in the unit, and the results are presented via a CAN-bus every 30 ms. The target’s relative amplitude is also given. The test results in Figure 3.32 (from [23]) show how a stationary radar, located at the origin, tracks a person walking in a zigzag pattern at various distances, between 1 and 15 meters. The person is detected at up to 9 meters away from the radar, when the bearing angle is within ±30 85

Development of an Autonomous Forest Machine degrees. The results show good reliability and accurate positioning. In the final application, two radars could be mounted to cover the areas slightly to the left and right of the vehicle’s front. However, we have still not done any extensive tests with the radars mounted on the forest machine. 400 300 200 y (cm)

100 0 −100 −200 −300 −400 0

500

x (cm)

1000

1500

Figure 3.32: Detection of a person moving in parallel to the y-axis at distances 1, 2,..., 15 meters away from the radar positioned in the origin, facing along the x-axis (from [23]). The radar detects the person within ±30 degrees between 1 and 9 meters.

3.5.3.3

Doppler Radar

As a “virtual bumper”, or emergency stop, a Doppler microwave radar, initially developed as a backup warning system for vehicles could be used. The principle of the Doppler radar is based on the fact that the Doppler frequency shift in the target echo is proportional to the radial component of target velocity, referred to as the Doppler effect. The radar only reacts to motion, i.e. obstacles approaching or receding the sensor, so it can be mounted even where the field of view is obstructed from parts of the vehicle, for example. The sensor operates at 10.525 GHz and uses frequency modulation of the microwave signal to determine the distance to an obstruction. It is also pulse modulated, so it does not interfere with similar devices, which means multiple sensors may be used on the same vehicle. The radar can sense if an obstacle is less than one meter, less than 2 meters, less than 3 meters, or more than 3 meters away from the sensor. This makes it useful in an emergency stop system, as the vehicle can be stopped if an obstacle comes nearer than one meter for example. It can also be used to slow down the vehicle if an obstacle enters a predefined range, as a security measure. An advantage to using this sensor is the relative low price, which means more of them could be used. In Figure 3.33, a setup with three Doppler radars along with the control unit is shown. 86

3.5 Sensors and sensor data processing

Figure 3.33: Three Doppler radars on top of the control unit.

3.5.4

Sensor fusion

To be able to detect all possible obstacles around the forest machine, several different sensors have to be used. This means that the data from all sensors has to be put together to get a full overview of the environment. To get an accurate estimation of the vehicle’s pose we may have to fuse data from several pose sensors, e.g. gyro and wheel odometry. Furthermore, the mounting pose of the sensors has to be taken into account. All sensor readings must be transformed into the same coordinate system before they can be fused. Fusion of different, simultaneously sampled, sensors typically works by estimating the relative accuracy, and weighting or even completely disregarding sensors that, for the moment, are unreliable. As described in Section 2.3.4, a number of different techniques to fuse sensor data exists. In our project two different strategies for sensor fusion of position and attitude sensors have been implemented; a Kalman filter and a Sensor Switch. In addition to this, an occupancy grid was developed for fusion of obstacle sensors. 3.5.4.1

Kalman filter

The Kalman filter [42] is a set of mathematical equations that provides an efficient computational (recursive) means to estimate the state of a process, in a way that minimizes the mean of the squared error. The filter is very powerful in several aspects: it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown [83]. We implemented a Kalman filter for estimation of the vehicle’s heading and position, based on simultaneously sampled data from GPS, gyro, compass, and the vehicle’s steering angle and speed, as described in [23]. Fusion over time works with multiple readouts from the same sensor (i.e. a time sequence) and results in one piece of information. The Kalman filter computes a weighted sum of varying noise components. One of the assumptions made is that the sensor readings have Gaussian noise, something that is not the case when the GPS switches from “fix” to “non fix” mode (as described in Section 2.3.2). In 87

Development of an Autonomous Forest Machine our implementation, this is handled by setting different noise components for the different run modes (refer to [23]). In “non fixed” mode, the noise is clearly non Gaussian, see e.g. Figure A.2 in Appendix A. Furthermore, the accuracy of the GPS with fix solution is so high that the other sensors have very little extra to offer. Without fix solution, a Kalman filter may increase the accuracy [9]. The developed Kalman filter has not been tested in the field yet, but initial tests show promising results. 3.5.4.2

Sensor Switch

A sensor switch method was developed for fusing position and heading data from the GPS and odometry sensors. As long as the GPS has a fix solution, the GPS data is used. Observe that position and heading data may have fix solutions independently of each other. When the fix solution is lost on either position or heading (or both), the system switches to the default backup position and/or heading sensor (usually wheel odometry and gyro) initialized with the latest correct reading from the GPS, see algorithms 4 and 5. Because the position can become incorrect when the vehicle tilts (due to displacement of the GPS antennas), the fused position is then corrected with respect to vehicle roll and pitch as described in Section 3.5.4.4. The result of fusing GPS with wheel odometry when losing fix solution can be seen in Figure 3.34. In this test the loss of fix has been faked, so the figure shows the real GPS position, but the fused position is the one we would get if the fix solution had been lost for real. As long as at least one of the heading and position solution has a fix, the error is relatively small. If neither heading nor position has a fix solution, the error grows and becomes much larger, as can be seen in the lower part of the figure. Algorithm 4 Fused attitude with the Sensor Switch method ψgps and ψodo are the headings from GPS and odometry respectively. φgyro and θgyro are the roll and pitch from the gyro. Fix solution means that the Realtime Kinematics mode is enabled as described in Section 2.3.2. The vehicle pose (φ, θ, ψ) is computed by the following algorithm. 1. if the GPS has a fix solution for heading (use GPS) (a) ψ = ψgps (b) ψoffset = ψgps − ψodo 2. else (use odometry rotated to the GPS coordinate system) (a) ψ = ψodo + ψoffset 3. φ = φgyro 4. θ = θgyro

88

3.5 Sensors and sensor data processing

Algorithm 5 Fused position with the Sensor Switch method (xgps , ygps , zgps ) is the position data from the GPS. (xodo , yodo) is the position data from the odometry sensor. ψ0 is the difference between odometry and GPS heading at time t = 0 (i.e the difference between the two coordinate systems). The fused position (xnew , ynew , znew ) is calculated by the following algorithm. 1. if the GPS has a fix solution for position (use GPS) (a) (x0 , y 0 , z 0 ) = (xgps , ygps , zgps ) 0 (b) (x0odo , yodo ) = (xodo , yodo )

(c) (xnew , ynew , znew ) = (xgps , ygps , zgps ) 2. else (use odometry transformed to the GPS coordinate system) (a) ∆xodo = xodo − x0odo 0 (b) ∆yodo = yodo − yodo

(c) xnew = x0 + ∆xodo cos(ψ0 ) − ∆yodo sin(ψ0 ) (d) ynew = y 0 + ∆xodo sin(ψ0 ) + ∆yodo cos(ψ0 ) (e) znew = z 0 (f) (x0 , y 0 ) = (xnew , ynew ) 0 ) = (xodo , yodo ) (g) (x0odo , yodo

89

Development of an Autonomous Forest Machine

Real GPS position Pos & Head fix Pos fix only Head fix only No fix

−125 −130 −135

y (north) [m]

−140 −145 −150 −155 −160 −165 −170 −340

−330 −320 x (east) [m]

−310

Figure 3.34: The impact of losing fix solution of the GPS when using sensor switch with wheel odometry. In this test the loss of the fix was faked, so the figure shows the real GPS position, but the fused position is the one we would get if the fix solution had been lost for real. As long as at least one of the heading and position solutions has a fix, the error is relatively small. If neither heading nor position has a fix solution, the error grows and becomes much larger, as can be seen in the lower part of the figure.

90

3.5 Sensors and sensor data processing 3.5.4.3

Pose transformation

The GPS antennas are mounted on the roof of the vehicle several meters above ground. This means that tilt and roll caused by uneven terrain cause the position to vary dynamically. In addition to this, we have a static displacement because the antennas are not mounted at the vehicle’s center point. For our control purposes we need the pose of the local vehicle-fixed coordinate system, expressed in the global coordinate system PGround→V ehicle (refer to Section 3.3.2). The mounting pose of the GPS, PV ehicle→Sensor , describes the relation between the sensor-bound coordinate system and the local one. PGround→V ehicle is computed by multiplying the dynamic pose PGround→Sensor , consisting of the roll and pitch acquired from a gyro and heading from the GPS, with the inverse of the static pose PV ehicle→Sensor [69]. The latter is the mounting pose of the sensor with respect to the vehicle’s center point, see equations 3.30 – 3.33. These equations are general and can be applied to all sensors, not only the GPS.

PGround→V ehicle

P =



R 0

T 1



=

P · P −1 | Ground→Sensor {z } | V ehicle→Sensor {z } Dynamic Static

(3.30)



cos p cos h R =  cos p sin h sin p   x T = y  z

(3.31) − cos r sin h − cos h sin p sin r cos r cos h − sin p sin r sin h cos p sin r



sin r sin h − cos r cos h sin p − cos r sin p sin h − cos h sin r  (3.32) cos p cos r (3.33)

where r: p: h: x, y, z : R: T :

Roll, r ∈ [±π] Pitch, p ∈ [±π/2] Yaw (Heading), h ∈ [±π] Translation vector components Rotational matrix Translational vector.

In Figure 3.35 the result of applying pose transformation on the GPS sensor on the forest machine can be seen. The difference in height between the GPS and compensated position is due to the mounting location of the GPS antennas on the vehicle. Figure 3.36 shows the effect of the roll and pitch on the compensated position. Normally, the compensated position should be about a half meter to the right of the position given by the GPS due to the mounting location of the antennas. The varying differences between the two are due to the varying roll and pitch of the vehicle, seen in Figure 3.37. 91

Development of an Autonomous Forest Machine

z (height) [m]

Sensor switch GPS 2 0 −2 −4 −6 40 30 20

15 10

10

5 0

y (north) [m] 0

−5

x (east) [m]

Figure 3.35: 3D-Position from the GPS and the sensor switch, which is corrected with respect to the vehicle’s pose. The difference in height is due to the mounting location of the GPS antenna (3.6 meters above ground). 45

Sensor switch GPS

40

y (north) [m]

35 30 25 20 15 10 5 0

−20

−10

0 10 x (east) [m]

20

30

Figure 3.36: The result of pose transformation can be clearly seen in this example. The GPS antenna was mounted 3.6 meters above ground and 0.58 meters to the left of the center of the vehicle.The position from the sensor switch is corrected with respect to roll and pitch. Normally the pose transformed position should be about 0.58 meters to the right of the GPS position to compensate for the mounting pose of the antenna. The difference between the two positions is due to the vehicle’s roll and pitch, seen in Figure 3.37. 92

3.5 Sensors and sensor data processing Angles from the gyro 15 Pitch Roll 10

Angle (deg)

5

0

−5

−10

−15 0

50

100 TimeStamp (s)

150

200

Figure 3.37: The corresponding roll and pitch angles from the Gyro to the positions in Figure 3.36. 3.5.4.4

Occupancy Grid

To fuse data from several obstacle sensors, we have to choose a representation of the data. A 2D grid is by far the most common representation of occupied/free space in the environment. It superimposes a 2D Cartesian grid on top of the world space. A grid cell at a location of a significant object will eventually be marked as occupied. To model the forest environment in a better way, a 3D grid would have to be used. The downside to using a 3D grid is that it is more computationally complex. We decided that the less complex 2D grid would be sufficient in a first stage of the project, but that a 3D grid would be a possible extension developed later on. The occupancy grid is 2-dimensional, and the obstacle detection subsystem currently works under a flat-ground assumption. Since it is only targeted at obstacle avoidance by design, and not at global map building, it can be made relatively small (e.g. 100x100 cells) and also move along with the vehicle. Although the system is working under a flat-ground assumption, the vehicle and range sensors are not assumed to be parallel to the flat ground. Their poses are fully 3-dimensional with 6 degrees of freedom (refer to Section 3.3.2), and the subsystem is designed to handle any pose set to a sensor. Although the pose of each range sensor has 6 degrees of freedom, a range sensor is approximated to have a 2-dimensional sensing plane, aligned with the xy-plane of its pose, where the x-axis equals the line-of-sight. The part of the sensing plane within the field of view and maximum range are referred to as the sensor wedge. The implementation of the occupancy grid is inspired by the implementation 93

Development of an Autonomous Forest Machine by Novick [56], where the vehicle is always positioned somewhere inside the mid cell of the map. When the vehicle crosses a borderline to a neighboring cell, the contents of the map is shifted one cell, and the vehicle can remain in the center cell of the grid. The data that gets shifted out of the map is discarded. It is noteworthy that the map is never rotated. Instead, the vehicle rotates and moves within the center cell, and data is always shifted in whole-cell steps. In this way blurring caused by the vehicle’s movement is completely avoided. The sensor fusion is based on a common (and naive) Bayesian approach for updating, as described by Movarec [53], where all individual sensor readings are assumed to be mutually independent between sensors and over time. For each cell, the ratio of the probabilities for being occupied (o) and not being occupied (¬o) is updated after a new sensor reading B according to p(B|o) p(o|A) p(o|A ∧ B) = · p(¬o|A ∧ B) p(B|¬o) p(¬o|A) where A denotes the set of all previous range sensor readings. The usage of this odds form can be further simplified by taking the logarithm: p(o|A ∧ B) p(B|o) p(o|A) log = log + log . p(¬o|A ∧ B) p(B|¬o) p(¬o|A) | {z } | {z } | {z }

Updated cell content

Sensor model

Cell content

In this way, the grid cell updating operation is reduced to simply adding a real number, where a positive value suggests an obstacle, and vice versa. By using binary logarithms it is possible to make highly efficient implementations in integer arithmetic (however, the current implementation uses floating-point arithmetic in Java). In this implementation, each range-sensor software module p(B|o) must supply a Bayesian sensor model that can yield the probability odds p(B|¬o) for a given range value and position on the range-sensor wedge. Movarec also discusses and evaluates a theoretically more correct technique, where previous p(B|o∧A) readings are also considered in the sensor model, p(B|¬o∧A) , but concludes that the cost is significant and the practical benefit seems to be of minor importance. The possible success of Bayesian updating depends to a high degree on the accuracy of the sensor models. When it comes to performance improvement, the primary task is to reduce the number of cells to update. Optimally, only the cells lying directly under each sensor wedge should be updated. In the current implementation, this is accomplished as follows: to determine which cells are directly under the sensor wedge, each sensor model gives a boundary point list to the grid module at system start-up. The boundary point list, which outlines the sensor wedge, is represented in local sensor coordinates. Before each grid update takes place for a given range sensor, the boundary points are transformed into grid coordinates, and a grid scan table is then constructed, which guarantees correctness and efficiency (one important assumption is that the outline of each sensor wedge must be strictly convex). The scan table has a row for each row in the grid, and two columns to define the grid column interval that should be traversed (for each grid row). When a range sensor delivers a new range value, 94

3.5 Sensors and sensor data processing the occupancy grid implementation then traverses the grid cells directly below the sensor wedge, and asks the Bayesian sensor model for the probability odds (for the mid-point of the cell) and subsequently updates each grid cell. The map updating frequency typically falls somewhere between 5 and 50 Hz, depending on the grid and sensor configuration at hand. The grid is configured so the cell size is in the same order of magnitude as the smallest obstacle to be detected. Figure 3.38 shows an example of the occupancy grid used with the laser scanner while driving through a narrow passage. The cell size was 0.1 meter and the map update frequency was 5 Hz.

Figure 3.38: The occupancy grid is able to create a map of the surrounding terrain. In this example, the grid is used with a laser scanner. The cell size was 0.1 meter and the map update frequency was 5 Hz. Sudden change in terrain elevation is a well known problem, where the ground may falsely appear to be an obstacle. That is just one example of problems not easily solved in non-deterministic outdoor environments, when using a 2-dimensional grid. The above mentioned problem is partly solved by adjusting the maximum range of the sensors in such a way that the probability of getting a ground echo (within maximum range) is sufficiently small. Furthermore, some heuristic techniques may be used to determine if a ground echo has been received. Another similar problem is negative obstacles, like ditches and ravines, which is not yet embraced by the obstacle-detection subsystem. Therefore, a 3-dimensional occupancy grid, with embedded terrain analysis, is considered a natural next step. 95

Development of an Autonomous Forest Machine

3.6

Path tracking

In a forest environment it can be hard to determine where it is safe to drive and where it is not by using a geographic map. This makes it hard to implement any of the traditional Path-Planning algorithms described in Section 2.5, to create a safe path to travel along. As mentioned earlier in this thesis, we have solved this by letting a human operator drive the path manually once while recording data from the run. A new path-tracking algorithm, Follow the Past, that makes use of this fact was developed. The information in this section has been published in [37], but more recent results have been added here.

3.6.1

Follow the Past

Traditional algorithms like Follow the Carrot [5] and Pure Pursuit [17] use position information only, and sometimes run into problems that can be avoided by taking into account additional information from the human driver. Follow the Past uses all recorded information (position, heading, steering angle, and speed) to follow a path in the best possible way. 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 algorithm in Follow the Past steers like the driver, plus an additional angle, based on the distance to the path. During the manual driving of the path, the orientation and steering angle are stored together with the position at every moment. All this information is used by Follow the Past, which is composed of 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 closest positions (x0 , y 0 ) and actual position (x, y) as inputs. φβ uses recorded orientation θ0 and actual orientation θ as inputs. φγ uses the recorded steering angle φ0 as inputs. The three behaviors are fused into one action, the commanded steering angle φt , as shown in Figure 3.39. The figure also shows an obstacle-avoidance system and the way it is integrated into the path tracking as well as a system that reduces the speed if the vehicle comes close to an obstacle.

3.6.2

φβ : Turn towards the recorded orientation

The angle θ0 is defined as the recorded orientation at the closest point on the recorded path. This point is referred to as the path point. φβ is computed as the difference between the current orientation θ and the recorded orientation θ0 (refer to Figure 3.40): φβ = θ0 − θ. 96

(3.34)

3.6 Path tracking

v′

Safe speed

φα

vs

vt

s

Occupancy grid Avoid obstacles

φsum

φo

( x′, y ′)

Recorded path

( x, y ) GPS/INS

vo

θ

Move towards φ the learnt path α

Turn towards the recorded orientation θ ′

θ′

φβ

w1 w2

+

w4

s

φt

w3

Mimic the φγ recorded steering ang. φ ′

φ′

Figure 3.39: Path tracking with reactive control of steering angle φt and speed vt

97

Development of an Autonomous Forest Machine Since the behavior φβ is only concerned with the error in orientation, we may imagine the vehicle following a virtual path parallel to the recorded path and passing through the steering joint of the vehicle, as shown in Figure 3.40.

Virtual Path

Path

φβ

Path Point θ

θ′

Figure 3.40: The behavior φβ is the difference between the current orientation, 0 θ, and the recorded one, θ .

3.6.3

φγ : Mimic the recorded steering angle

This behavior simply returns the recorded steering angle φ0 at the the closest point on the recorded path (refer to Figure 3.41). φγ = φ0 .

(3.35)

By using the recorded steering angle, the curvature of the path is automatically included in the final steering command. This is a great advantage compared to methods like Pure Pursuit [17] and Follow the Carrot [5].

3.6.4

φα : Move towards the path

This behavior is responsible for bringing the vehicle back to the path if the vehicle leaves the path for some reason. This can be caused by noise in the position signals, or by the obstacle-avoidance system described in Section 2.7.4. φα can be implemented in many ways. We have implemented two different methods as described below. 98

3.6 Path tracking

Path

φγ Path Point

θ′ δ

Figure 3.41: The behavior φγ is the same as the recorded steering angle. Method 1: directly proportional to the distance from the path The simplest way is to calculate a value directly proportional to the distance between the vehicle and the closest point on the recorded path (refer to Figure 3.42). This is done by multiplying the distance from the path by a constant:

φα = k ∗ d with the condition: |φα | ≤ 90◦ where k: a constant [deg/m] d: the signed perpendicular distance between the vehicle and the path. Note that d is negative or positive depending on the side of the path on which the vehicle is. A typical value for k in the current application is 0.07. To ensure that φα doses not become too large when the distance to the path is large, we need a constraint. The two behaviors φβ and φγ ensure that the vehicle is parallel to the path, and therefore the angle φα must be within [−90◦ , 90◦ ], i.e |φα | ≤ 90◦ . Method 2: use a Look-Ahead Point With Method 1, the calculation of the angle φα is linear. This may result in oscillations about the path when the vehicle is close to it. Reducing the value 99

Development of an Autonomous Forest Machine

Path

Path Point

d

Figure 3.42: Calculation of φα in Method 1: directly proportional to the distance from the path (d). for k results in a slow response for large d, i.e. the vehicle takes longer to reach the path. Method 2 overcomes this problem by using a Look-Ahead Point and calculating the angle between the vehicle and the Look-Ahead Point, see Figure 3.43. The algorithm for computing φα can be found in Algorithm 6. Algorithm 6 Calculation of φα in Method 2 1. Determine the closest point on the recorded path (i.e. the path point). 2. Compute a Look-Ahead Point at a Look-Ahead Distance ` from the path point, in the direction δ in Figure 3.43. This angle is the sum of the recorded orientation θ0 and steering angle φ0 at the path point. 3. Calculate a Look-Ahead Angle ψ, defined as the angle of the vector between the vehicle’s current coordinates and the Look-Ahead Point, in the global coordinate system. 4. Compute φα as the difference between the Look-Ahead Angle and the angle δ. With the notation in Figure 3.43 we get: (3.36)

φα = ψ − δ 0

0

0

δ = φ + θ = φβ + φ + θ where 100

(3.37)

3.6 Path tracking ψ θ φ0

is the Look-Ahead Angle is the current orientation is the recorded steering angle at path point p.

Look Ahead Point

Path

l

φα

δ

Path Point

δ

ψ

Figure 3.43: Calculation of φα in Method 2. Refer to Algorithm 6 and Equation 3.36.

3.6.5

Command fusion

The three behaviors φα , φβ and φγ all return a suggested steering angle. These three values are fused into one angle φt by simple addition as shown in Figure 3.39. I.e.: φt

=

φβ + φγ + φα .

(3.38)

For Method 1 above, this expression is: φt

=

θ0 − θ + φ0 + k ∗ d

where: θ0 is the recorded orientation at the path point p θ is the current orientation φ0 is the recorded steering angle at the path point p k is a constant d is the shortest signed distance between the vehicle and the path point p. 101

(3.39)

Development of an Autonomous Forest Machine The path point p is the closest point from the steering joint of the vehicle to the recorded path, as illustrated in Figure 3.43. For Method 2 above, the expression for the fused φt is (refer to Figure 3.44 and Equations 3.35, 3.36, and 3.37): φt = ψ − δ + φβ + φγ = ψ − (φ0 + θ0 ) + (θ0 − θ) + φ0

(3.40)

= ψ − θ. where ψ is δ is θ0 is θ is φ0 is

the the the the the

Look-Ahead Angle angle from Equation 3.37 recorded orientation at the path point p current orientation recorded steering angle at the path point p. Look-Ahead Point

Path

φt

l

Path Point

δ

θ

ψ

Figure 3.44: Command fusion for Method 2. The resulting steering angle φt is calculated as the difference between the Look-Ahead Angle ψ and the the vehicle’s current orientation θ, refer to Equation 3.40.

3.6.6

Finding the nearest point on the path

To calculate the location of the Look-Ahead Point, the nearest point on the path (denoted “the path point”) must first be determined, see Figure 3.45. This is done by calculating the closest distance to each path segment. Algorithm 7 (from [73]) computes the distance between a point and a line segment. In addition to the distance, the coordinates of the closest point on the segment is also calculated. For the path segment closest to the vehicle, this corresponds to the closest point on the path (i.e. the path point). To speed up the algorithm, 102

3.6 Path tracking P

d (P, Pb )

W

Pb P0

V

P1

Figure 3.45: Distance between the vehicle’s center point P and the closest point on the path, Pb , refer to Algorithm 7.

the search begins a few segments behind the last selected segment and continues up to a few segments in front of it. In this way a “search window” is created around the previous path point. The probability that the closest point on the path is near the point that was closest a moment ago is very high. Normally the new path point should be just in front of the old one, if the vehicle moves forward along the path. If the position given by the GPS contains much noise, this window has to be larger, since the previous path point may be incorrect. But even in this case, the vehicle should still be close to that position and thereby the nearest point on the path is close to the correct one. Since a segment is finite, the vehicle may be outside the perpendicular lines from the endpoints of the segment, as illustrated in Figure 3.46. In this case, the closest point is one of the endpoints, and we must determine which one. One way to do this, however not a very efficient one, is to compute both distances and use the shortest one. We must, however, still determine that the vehicle is actually outside the segment. To combine these two computations, consider −−−→ −−→ −−→ the angles between the segment P0 P1 and the vectors P0 P and P1 P , i.e. from the segment’s endpoints to the vehicle’s center point, see Figure 3.45. If either of these angles is 90◦ , the closest point on the path, Pb is the corresponding endpoint. Otherwise the point lies on either side of the endpoint, depending on whether the angle is acute (< 90◦ ) or obtuse (> 90◦ ). These conditions are easily tested by computing the dot product of the vectors involved. The sign of this product determines if the distance should be computed to one of the points P0 or P1 , or as the perpendicular distance to the segment itself. In a situation such as in Figure 3.46 the algorithm does not find any segment that the vehicle is closest to, and the closest endpoint has to be used instead. 103

Development of an Autonomous Forest Machine

Algorithm 7 Distance between a point and a linear segment [73]. Refer to Figure 3.45. distance( Point P , Segment P0 : P1 ) { v = P1 − P0 w = P − P0 if ((c1 = w • v)