Sensor Fusion in Forestry

By Jürgen Rossmann, Petra Krahwinkler, and Markus Emde

Modern machines such as wood harvesters can automatically cut trees and remove branches, but an expert is still needed to plan a thinning and to mark the trees to be felled. The process can be accelerated if the forest ranger can virtually mark trees to be cut, using geographic coordinates instead of colored crosses sprayed on the stems. This requires the robotic wood harvester to be able to locate itself accurately to enable automatic navigation to the next tree for cutting.

Absorption of the GPS signal in the forest canopy leads to poor results, however, with errors up to 50 meters and more. Furthermore, the canopy may cause interruptions and signal loss for several seconds. The performance can be even worse on a moving vehicle, where the signal may even get lost until the vehicle reaches an open area or stops.

Other approaches use differential GPS (DGPS) sensors as their main source of position information. However, our experiments using a high-precision DGPS sensor showed that its accuracy is not even close to sufficient for navigating to a single tree. As the DGPS suffers from the same canopy-related disturbances and shielding, it cannot benefit from its theoretical advantages. In pratice, the DGPS system did not update its position at all when signal reception became too weak.

A different approach was needed. We found it in the framework of the Virtual Forest, more precisely in the semantic modelling of forests, where techniques are being developed to delineate single trees from remote sensing data, such as airborne laser scanner data. Along with the trees and their geo-coordinates, the height and the diameter at breast-height are determined. This data can be used to generate a tree map, which can be used for navigation. The map has a mean error between 0.5 and 1.5 meters, which is still below the mean tree distance of about 2.5 meters.

Visual GPS. The idea of Visual GPS is to bring current developments in the field of robotics into the forest and combine them with information on forest inventory so that the result outperforms other navigation approaches. A matching algorithm is run based on a tree map, generated from remote sensing data, and the tree group, which was detected by one or more laser scanners.

We then implemented a particle filter algorithm, as it enables considering different kinds of distributions. Particles are also called random state samples, and each particle is a hypothesis as to what the true world state might be.

In the initialization, particles are distributed uniformly. An importance weight wt is calculated for each particle, incorporating the measurements as described below. A sampling step rejects particles with a low importance weight and replaces them with new particles, which are distributed according to the previous map. This process is repeated until the particle distribution concentrates at one point, and the particle with the highest weight is returned as the result (see Figure 1).

Figure 1. Particle concentration after resampling; wood harvester at center.

A single tree as a landmark cannot be associated with its corresponding tree in the map. However, patterns of tree positions can be matched. We chose a square area to guarantee even particle distribution and short calculation time. Each particle represents a hypothesis for the position of the vehicle and is tested for its probability to represent that position.

To make the approach more robust against faulty tree maps, we implemented a rotation variant approach, determining vehicle heading along with its position. This enhanced the probability measure used in the propagation step. Instead of embracing only the distances of the trees to the reference point, their relative position is used, considering the heading wt of the current particle:

This approach directly calculates vehicle heading, but the sensitivity towards rotation, which results from the new probability measure, leads to a higher number of particles that must be used during the initialization step.

Global Search. Experiments on a test area with about 22,700 trees proved that the algorithm worked reliably for tree groups containing 20 or more trees, and for position errors of the magnitude of the mean tree distance. Similar tree groups could not be found within the forest. However, the calculation time was too long to be used for navigation.

Local Search. To overcome the high calculation time, we reduced the number of particles. The initial position is estimated with an ordinary GPS sensor. Although the GPS measurement is faulty in the forest, it can limit the search to a restricted area. Machines most often start at the edge of a forest stand, at a forest road, or a canopy opening. At these spots the canopy usually is transparent, and GPS sensors work with higher precision. Therefore, they provide a good initialization for the algorithm.

Robotic wood harvester.

In the following steps, the previous position can be used instead of the output of the GPS sensor for determining the search area. The previous position provides a better initial pose estimation than the GPS sensor and therefore gives the opportunity to further decrease the search area.

To reduce the number of trees for which the distance has to be calculated, trees with a distance from the initial pose estimation smaller than the sum of the estimation of the maximal position error and the maximal distance of the trees in the scanned tree group from the reference position are extracted from the tree map.

Another way to reduce the search area is to estimate vehicle orientation. This is difficult for machines such as wood harvester, which moves slowly and stops frequently when cutting trees. Therefore, small lateral position differences result in large orientation deviances, as the difference vector does not directly point into the direction of the movement any more. Another approach is to use sensor fusion and mount a compass onto the vehicle. During particle initialization, the angle can be restricted to the domain of uncertainty around the compass orientation. However, mounting a compass onto a wood harvester proved to be a serious problem, as the harvester’s massive metal body disturbs the compass measurement.

Figure 2 shows the workflow of the complete system.

Figure 2. Navigation system components.

Results

The simple criterion presented here proved to be reliable in the vast majority of cases. Problems can occur when the tree group contains trees that are not part of the tree map (false positive). This can happen due to missing trees in the tree map or faulty tree cognition in the local laser scanner measurement. In the first case, the understory might not have been detected in the airborne laser scanner data. In the second case, other objects like the harvester’s aggregate might have been mistaken for a tree.

The case of trees not detected in the local laser scanner measurements but contained in the tree map (false negative) does not create problems in the pose estimation step. The algorithm searches for a corresponding tree for each unit in the tree group. For a false positive, no corresponding tree can be found, whereas a false negative is simply not considered. However, if the size of the tree group is too small, the estimation errors grow. The minimum number of trees depends on the search area radius. A size of 20 trees proved to generate reliable pose estimations even during the global search. Dropping below 15 trees, the number of faulty position increases rapidly as more similar patterns can be found.

Single faulty positions can be filtered with respect to the movement constraints of a harvester. The velocity is very low, and the orientation cannot jump. In the experiments, cycle times of about 0.5 seconds were reached on a standard PC. As forest machines do not demand very short calculation time, the algorithm proved to run fast enough to allow identification of single felled trees onboard real machines. One application of the algorithm was to support a navigation assistant to the next tree, similar to navigation systems in cars.

To evaluate system accuracy on a real wood harvester, a surveyor’s office was instructed to measure the vehicle’s position at seven distinct locations. At each position, the sensor input data was written to file for several seconds. This data was evaluated, and for each location more than 45 pose estimations were calculated. The mean value of the position error amounted to approximately 0.55 meters.

Future Work

Reliability can be enhanced by using a detailed digital ground model and the cabin tilt in order to detect the area where the laser beams hit the ground, and therefore avoid the detection of false positives. Similarly, the position of the aggregate, which can be measured by integrating sensors in the hydraulic cylinders of the crane, can be cut from the laser scanner measurements and ignored during tree detection, further reducing the amount of false positives in the tree group. With the integration of an outlier rejection step for false positives in the detected tree groups that ignores trees for which no corresponding candidate tree can be found, a more accurate importance factor can be calculated.

Another task is the integration of the algorithm with a Kalman filter to allow real-time performance of the algorithm. Therefore, the Kalman filter is initialized with the pose estimation of the particle filter algorithm, which is also used for continuous checks of the current position estimate, thereby combining two algorithms with different advantages. The Kalman filter allows real-time execution and therefore speeds up the overall navigation algorithm. The particle filter algorithm can periodically check the position estimated by the Kalman filter and correct it. Furthermore, it provides a strong method to cope with two main problems in mobile robotics: the data association problem and the kidnapped robot problem.

Simultaneously, a mapping and map-correction algorithm could be integrated into the system so that understory trees, which cannot be detected using remote sensing data, and deciduous trees, which are more difficult to delineate in airborne laser scanner data, can be added to the tree map.

Jürgen Rossmann is head of the Institute of Man-Machine Interaction at the RWTH Aachen University, where Petra Krahwinkler and Markus Emde are research scientists.