Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester

Edo Jelavic, Dominic Jud, Pascal Egli, Marco HutterField Robotics 2022 · DOI

TL;DR

This paper presents a fully autonomous system for precision tree harvesting using a legged robot in challenging forest environments. The system integrates novel approaches for mapping, localization, planning, and control, enabling the robot to navigate, identify, and grab trees without human intervention, marking a significant step towards automated forestry.

What Problem Does This Paper Address?

Traditional forestry operations are labor-intensive, dangerous, and often inefficient. While mechanization has improved productivity, human operators remain a bottleneck, especially in complex tasks like selective harvesting or thinning. These operations require navigating dense, uneven terrain, often in GPS-denied environments, and precisely manipulating trees. Existing automation efforts have focused on specific sub-tasks or smaller machines, but a fully autonomous system for large-scale harvesters in realistic forest settings has been a significant challenge. This paper aims to bridge this gap by developing and demonstrating an integrated system for autonomous tree harvesting.

Methods

The core of the system is a legged harvester (HEAP), a Menzi Muck M545, equipped with custom sensors and actuators. The system comprises several key modules:

  • Mapping and Localization: They employ Google Cartographer for SLAM, using LIDAR, visual, and inertial data. This allows the robot to build a 3D map of the environment and localize itself within it, even under dense forest canopies where GPS is unreliable. They also developed a method to convert 3D point clouds into 2.5D elevation maps for planning.
  • Tree Detection: A lightweight, model-based approach using LIDAR point clouds is used to detect tree trunks and estimate their positions. This method focuses on geometric features and is designed for online operation.
  • Planning:
    • Mission Planner: Determines the order of trees to harvest, initially based on human input.
  • Base Pose Planning: This involves two stages: generating candidate approach poses around a target tree and then using an RRT* planner to find a feasible path to one of these poses. Traversability maps, derived from elevation maps, are used to identify obstacles.
  • Arm Grasp Pose and Motion Planning: Computes the desired gripper pose and a three-stage maneuver (retract arm, turn cabin, extend arm) to reach the tree.
  • Control:
    • Chassis Control: Combines a Hip Balancing Controller (HBC) for terrain adaptation and a pure pursuit controller for path tracking, enabling the robot to navigate rough and uneven terrain.
  • Arm Control: An Inverse Kinematics (IK) controller, based on Hierarchical Optimization, tracks the planned arm trajectory.

Datasets

The paper describes experiments conducted in:

  • A testing field with a mix of concrete and gravel surfaces.
  • A natural forest patch.
  • An urban setting (emergency services training ground in Wangen an der Aare, Switzerland) for ground truth evaluation.

The authors also mention using synthetic data for planning evaluations. No specific datasets for training machine learning models are detailed, as the tree detection is model-based and the planning relies on geometric algorithms.

Key Insights and Ideas

  • Integrated System Approach: The paper emphasizes the importance of integrating multiple complex modules (mapping, localization, planning, control, detection) into a cohesive system for achieving full autonomy in a challenging domain.
  • Legged Harvester Capabilities: The use of a legged platform (HEAP) is crucial for navigating the highly unstructured and uneven terrain found in forests, allowing it to overcome obstacles that wheeled vehicles might struggle with.
  • GPS-Denied Navigation: The reliance on LIDAR-based SLAM and visual-inertial odometry for mapping and localization is key to operating effectively under dense forest canopies where GPS signals are often unavailable or unreliable.
  • Lightweight Tree Detection: Developing a computationally efficient tree detection algorithm suitable for online operation on the robot is a significant contribution, enabling real-time decision-making.
  • Approach Pose Planning in Clutter: The method for planning approach poses in confined and cluttered environments, by first generating candidates and then using RRT* for pathfinding, addresses a critical challenge in mobile manipulation.

Key Results

  • Path Tracking: Achieved an average tracking error of 17 cm on flat surfaces and 0.461 m to 0.684 m on inclined and wet terrain.
  • Localization: Translational error of 0.11±0.09 m on the test field, 0.17±0.28 m in the first forest, and 0.25±0.28 m in the second forest when using Cartographer. When using ICP, errors were significantly lower: 0.085±0.038 m in forest 1, 0.078±0.035 m in forest 2, and 0.13±0.11 m on the test field.
  • Planning: Achieved success rates of up to 98% in forest scenarios, with planning times generally under 7 seconds for approach pose generation and RRT* planning.
  • Tree Detection: Demonstrated high recall (up to 95%) and precision (up to 98%) in various forest types, with a lightweight algorithm that runs in approximately 300 seconds for a 26 million point cloud.
  • Fully Integrated System: The system successfully performed autonomous harvesting cycles, grabbing wooden logs and simulated trees, demonstrating the viability of the integrated approach in realistic environments.

Limitations and Future Work

  • Localization Accuracy: While good, the end-effector position accuracy of around 30 cm might not be sufficient for high-precision harvesting, necessitating local tree detection.
  • Tree Detection Failures: The tree detection system can fail in the presence of heavy clutter, vegetation, or when branches touch other trees, leading to false negatives. Mistaking branches for trees or detecting the same tree twice (false positives) are also noted issues.
  • Manual Correction: The traversability estimation requires manual correction of false negatives, which is time-consuming.
  • Wet Conditions: The system struggled in wet conditions where the machine could get stuck in mud, though this is less of an issue in typical forestry operations conducted during dry seasons.
  • Future Work:
    • Improve approach pose planning by incorporating footprint adaptation and adjusting candidate pose generation based on the environment.
  • Benchmark different planners and parallelize approach pose generation.
  • Enhance the arm planner to include tree geometry and estimate tree trunk weight for better control.
  • Develop more intelligent mission planners that optimize objectives (e.g., minimal time) and potentially coordinate multiple harvesters.
  • Develop more rugged sensors for harsh environments and explore redundant sensor modalities (e.g., radar) for robustness against clutter and occlusions.
  • Investigate behavior trees for more compact state machines with integrated recovery behaviors.
  • Explore using ICP for localization as an alternative to Cartographer for potentially better performance and lower CPU usage.
  • Integrate visual information for traversability estimation and tree detection.

Summary updated

BibTeX
@article{jelavic2022robotic,
  author = {Edo Jelavic and Dominic Jud and Pascal Egli and Marco Hutter},
  title = {{Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester}},
  journal = {Field Robotics},
  year = {2022},
  doi = {10.55417/fr.2022046},
}
Figure