Automated Route Planning from LiDAR Point Clouds for Agricultural Applications ^†

Abstract

This paper develops an algorithm to compute optimal routes for an autonomous compost turner. In commercial composting, the material to be composted is piled up in large heaps called windrows and turned regularly by compost turners. The environment at the composting site is constantly changing, as the locations of the windrows change with each turning procedure. Therefore, we propose a novel method that automatically computes routes on a composting plant from LiDAR data. The LiDAR is mounted on the compost turner together with a dual-antenna GNSS receiver, an IMU, and rotary encoders. An extended Kalman filter is used to obtain the vehicle’s pose. Through direct georeferencing, a global point cloud is obtained. The routing algorithm crops, segments, and filters the point cloud until the points along the ridge of each windrow remain. These points are used to compute the optimal routes along each windrow. Furthermore, a user can select the windrows which need to be turned and the algorithm then computes the most efficient path for the compost turner, which also includes the passages between the windrows. The method was tested within a simulation environment using a 3D model of the composting site. The results show that the algorithm detects the windrows and computes the routes with sufficient accuracy for autonomous compost turning.

1. Introduction

In 2020, the European Commission introduced a circular economy action plan [1] as part of its European Green Deal. Within the Waste Framework Directive [2], it specified the target that by the year 2035, a minimum of 65% of municipal waste should be reused or recycled. As bio-waste amounts to 34% of all municipal waste [3], recycling bio-waste plays a key role in meeting the EU’s recycling targets.

The most common recycling techniques for bio-waste are composting and anaerobic digestion. According to [4], over 90% of the bio-waste that is separately collected in Europe is composted. Therefore, composting can be considered the most important bio-waste recycling technique in Europe.

In composting, the organic material is turned into compost, a biochemically stable product containing microorganisms that can improve soil properties, such as organic matter content, water retention capacity, and pH buffer capacity [3].

The most common method of commercial composting is windrow composting, where the organic material is stacked into long heaps that are turned mechanically by compost turners to promote aerobic microbial activity, moisture release, heat removal, and aeration. However, the job of turning windrows is monotonous and the operator of a compost turner is exposed to high temperatures, water vapor, and gases. Therefore, it is desirable to develop autonomous compost turners.

In recent years, a research team at Graz University of Technology has focused on the development of a navigation module for autonomous compost turners. A concept for an automatic steering system was first presented in [5]. In [6], tests are conducted to select a suitable set of navigation sensors. Two different multi-sensor fusion architectures for fusing GNSS, IMU, wheel odometry, and stereo camera are presented and evaluated in [7]. Ref. [8] presents an odometry model specifically tailored to tracked compost turners that allows for bridging GNSS outages. In [9], a real-time positioning module to estimate the position, velocity, and attitude of a compost turner is developed and tested in an automatic steering system at a composting plant.

In [10], a concept for autonomous compost plant management is proposed. An autonomous plant management not only requires an automatic steering system that steers the compost turner through a single windrow, but requires more complex maneuvers such as turning and driving from one windrow to the next. To achieve these tasks in an automated manner, a complex routing algorithm is required. However, no study has yet focused on routing algorithms for autonomous compost turners. This is exactly where this paper comes in: it develops an algorithm to compute optimal routes from a mobile mapping system mounted on the compost turner.

A major challenge for developing routing algorithms for a composting plant is the constantly changing environment, as the locations of the windrows change with each turning procedure. Therefore, we propose a novel method that automatically computes routes on a composting plant from LiDAR data. In this paper, the concept is presented and tested within a simulated composting plant environment.

Figure 1 shows an overview of the simulated composting plant where several windrows can be seen. The aim of the routing algorithm is to first detect each separate windrow (cyan) in an initialization phase. Afterward, an operator can specify which windrows shall be turned before the compost turner computes an optimal route (orange) through them.

Path planning in environments that are observed with a LiDAR scanner is a common task in mobile robotics. For planning global paths, commonly used algorithms are the A* algorithm, which was first published in [11] and then constantly improved (e.g.: [12,13,14]), and the Dijkstra algorithm [15]. For high-level planning that considers the robot configuration, a local path planner is used. Examples of such local planners are the Dynamic Window Approach algorithm [16] and the Timed Elastic Band (TEB) algorithm [17]. All planners, global and local, use cost maps to define the traversability of different areas within the map.

This paper uses the Dijkstra algorithm as a global and the TEB algorithm as a local planner. However, the main difference to conventional routing applications is that windrows, which would be classified as obstacles by conventional algorithms, are in our case traversable and part of the desired route. This means that each cluster of points within the point cloud that represents such a windrow must not be classified as an obstacle but must be processed to determine the optimal route through it. To the authors’ knowledge, no similar system has yet been developed.

2. Materials and Methods

This chapter describes the steps of the routing algorithm and its validation in the simulation environment. Section 2.1 gives an overview of the developed software and the used software packages. Section 2.2 outlines how the global point cloud of the composting plant is obtained from LiDAR data. Section 2.3 describes how the global point cloud is segmented into individual windrow point cloud clusters. Note that point cloud clusters will be referred to as clusters throughout this paper. Section 2.4 explains how the individual routes through each windrow cluster are calculated and Section 2.5 outlines how the final route for the compost turner is computed using input from an operator. Section 2.6 describes the simulation environment that is used to validate the developed algorithm.

2.1. Software Development

The software for the routing algorithm is developed in C++. For the point cloud processing operations, the openly available Point Cloud Library [18] is used. The Robot Operating System (ROS) [19] provides the appropriate framework for the development of the robotic application. The ROS navigation stack offers the 2D navigation capabilities needed for route planning and following.

2.2. Obtaining the Global Point Cloud from the Windrow Area

In the first step, an up-to-date global point cloud of the composting plant must be generated. This cloud is acquired in an initialization phase where the compost turner is steered manually around the windrows at the composting plant. For this task, a simulated Velodyne Ultra Puck LiDAR scanner was used. Given the scanner’s limited vertical field of view of 40°, it is mounted on top of the compost turner with a forward tilt of 15° to better cover the area in front of the machine.

The full procedure to incrementally build the global point cloud in the initialization phase is shown in Figure 2. Each incoming LiDAR point cloud must first be georeferenced to be in the same coordinate frame as the global point cloud. This is achieved through direct georeferencing using the position and attitude results (Robot Pose) from a previously developed Error State Kalman Filter (ESKF). The filter fuses the odometry data of the compost turner with data from an Inertial Measurement Unit (IMU) and a Real-Time Kinematic (RTK) dual antenna Global Navigation and Satellite System (GNSS) receiver to calculate the state vector of the robot in real-time. For a detailed description of the positioning module, the reader is referred to [9].

To keep the size of the global point cloud as small as possible in order to reduce computational load, several further operations are performed:

• Windrow Area Filter: The windrows are always located within the same area on the composting plant. Therefore, the extent of this area is defined using a confining polygon (Windrow Area Polygon) created from known 2D coordinates. All points outside this polygon are simply filtered out during the initialization.
• Local Box Filter: Points are only added to the global point cloud when they are within a certain vertical range from the ground and within a certain horizontal distance from the LiDAR. The vertical range ensures that ground points are removed. The horizontal distance ensures that points that are further away from the LiDAR—and, therefore, have a lower accuracy—are not included.
• Downsampling: The constant addition of the current LiDAR point cloud to the global cloud leads to areas that have a very high point density. In a downsampling step, the total amount of points can be reduced, especially in these dense areas, by making sure that only one point is kept within a specified Downsampling Radius.

After the initialization phase, the Global Windrow Point Cloud contains all points that belong to windrows and provides the basis for extracting the individual windrow clusters.

2.3. Extraction of the Windrow Clusters

From the Global Windrow Point Cloud, the individual windrow clusters can now be extracted. An overview of the clustering process is given in Figure 3. In the first step, an outlier removal using Random Sample Consensus (RANSAC) is performed, since outliers can negatively influence the clustering result. Therefore, RANSAC with a planar model is used to estimate the best-fitting plane through all points. Points further than a specified Distance Threshold away from this plane, i.e., the outlier points, are removed.

After the outlier removal, the clustering is done using Euclidean Clustering provided by the Point Cloud Library (PCL). The Clustering Distance Tolerance parameter specifies the maximum distance between points to be considered part of the same cluster. Additionally, the cluster size limits (minimum and maximum number of points within a single cluster) can be set. These parameters depend on the density of the point cloud. If the Downsampling Radius (see Section 2.2) is large, a larger Clustering Distance Tolerance and lower cluster size limits are necessary, and vice versa. The resulting clusters each represent one windrow.

2.4. Finding the Optimal Route through each Windrow Cluster

In the next step, each Windrow Cluster is analyzed and processed to find the optimal route for the compost turner through the windrow. The optimal route follows the ridge of each windrow and thus requires the extraction of the ridge points from the Windrow Cluster. A simple line is estimated, as it guarantees a relatively straight windrow after the compost turner finishes moving through the windrow. In Figure 4, the full process of finding this optimal line through each windrow is outlined. The individual steps are explained in the following.

First, a RANSAC line-fitting algorithm is used to obtain the Line Coefficients for the best-fitting line through the cluster points and to further eliminate outliers (Filtered Cluster). Projecting the Windrow Cluster to the RANSAC line (Projected Cluster) assigns every point i a corresponding z-coordinate on the line ($z_{line}^i$). Then, $z_{line}^i$ is subtracted from the original z-coordinate, $z_{original}^i$, of the point:

$$z_{rel}^i=z_{original}^i-z_{line}^i.$$

(1)

This is performed for all n points. The resulting set $Z_{rel}={\left\{z_{rel}^i\right\}}_{i=1}^n=\{z_{rel}^1,z_{rel}^2,\dots ,z_{rel}^n\}$, which represents the z-coordinates of the points with respect to the line, is distributed around zero. From this set, the points with the highest z-coordinates relative to the line can be extracted. The Keep Percentage parameter specifies the percentage of points extracted. These points then represent the ridge of the windrow (Windrow Ridge Cluster).

Finally, a second RANSAC line-fitting algorithm can be applied to obtain the coefficients for the optimal line through the Windrow Ridge Cluster. From the Projected Cluster, the first and last points on the line are extracted. These points are used together with the Line Coefficients from the second RANSAC to obtain the start and endpoints of a line segment—the optimal route through the windrow.

2.5. Route Planning Algorithm for the Compost Turner

After determining the optimal route through each windrow (Windrow Lines), the routing algorithm for the global path at the composting plant can be developed. The aim is to find a route through user-selected windrows, which takes maneuvers on the composting plant into account. Such maneuvers include moving from the end of one windrow to the start of another windrow or moving to a specified target with a desired final orientation (e.g., to the charging station).

A user can select which windrows should be turned by the autonomous compost turner. The full route consists of a list of waypoints. One windrow is stored as two separate waypoints, i.e., the start and endpoints of the Windrow Line. Depending on which point is closer to the compost turner, the start and endpoints of the estimated route through the windrow can be switched around.

When starting the algorithm, the first stored waypoint is passed to the route planner. The route is planned with the ROS navigation stack, specifically with the move_base package which incorporates a global and a local planner. For the global path planning, the global_planner package, which contains the Dijkstra algorithm, was used. Local planning was integrated using the teb_local_planner plugin [20], which is based on the timed elastic band (TEB) method. The planner then computes the optimal route to the waypoint and outputs linear and angular velocity commands to the control module of the robot. This is repeated until the last waypoint is successfully reached.

2.6. Simulation Environment

The developed routing algorithm is tested and evaluated within Gazebo. Gazebo is a simulation environment that allows for ROS integration. Several plugins exist that enable the simulation of navigation sensors such as GNSS receivers, IMUs, or LiDARs. Within Gazebo, the compost turner is simulated and can be steered manually using a controller. Outputs from a differential drive controller are used and translated to steering commands for the robot.

The environment of the composting plant is simulated from a mesh file. This 3D model was created using data from UAV photogrammetry. The environment contains an area of approximately 50 by 70 m. In total, there are 16 windrows, each with a width of approximately 3 m and a height of 1.5 m. The windrows are 40 m long, except for the last two, which are only 15 m long. To properly test the windrow detection algorithm that estimates the optimal routes through each windrow, several tests were conducted within the simulation environment (see Section 3).

3. Results

This chapter presents the results of the validation of the routing algorithm in the simulation environment. First, the processing steps for detecting a single windrow are visualized, and then the results for multiple initialization rounds are presented and compared.

3.1. Detection Results for a Single Windrow

Figure 5 shows the result of the windrow detection algorithm for a single windrow. The simulated windrow (A) is observed in the initialization phase as a point cloud (B). Then the ridge points of the windrow are extracted (C). Finally, a line is estimated through the ridge points (D), from which the start and endpoints of the windrow can be derived as a result.

3.2. Detection Results for Multiple Initialization Rounds

To validate the windrow detection, three sets of ten individual initialization runs were executed in the simulation environment. The three sets differ regarding the input settings (see

Table 1. Windrow detection settings for the different test sets.

Setting	Set A	Set B	Set C
Downsampling Radius	0.10 m	0.20 m	0.30 m
Clustering Distance Tolerance	0.15 m	0.30 m	0.45 m
Min. Number of Points in Cluster	1000	500	300
Max. Number of Points in Cluster	100,000	50,000	30,000
Local Crop Box:
Horizontal Distance from LiDAR	25 m
Vertical Range above Ground	0.9–1.8 m
RANSAC Distance Thresholds:
Plane	0.5 m
Line 1	0.7 m
Line 2	0.5 m
Ridge Points—Keep Percentage	40%

). The Downsampling Radius was varied from 0.10 m (Set A) to 0.30 m (Set C), which also has an effect on the other parameters for the cluster detection (see Section 2.3).

Each initialization run was performed manually by steering the compost turner around the windrows of the simulated composting plant, meaning the routes are always similar but never exactly the same. Each time, all 16 windrows were successfully detected. To validate the results numerically, the estimated start and endpoints as well as the orientation of each windrow were compared. Since each windrow is detected ten times, the standard deviation of the distance from the mean start and endpoint is given in

Table 2. Windrow detection results for the three different test sets.

	Set A		Set B		Set C
	Standard Deviations		Standard Deviations		Standard Deviations
ID	Points (2D) [m]	Orientation [°]	Points (2D) [m]	Orientation [°]	Points (2D) [m]	Orientation [°]
1	0.10	0.43	0.07	0.41	0.16	0.41
2	0.05	0.16	0.15	0.35	0.11	0.34
3	0.08	0.29	0.09	0.25	0.16	0.39
4	0.06	0.15	0.06	0.18	0.12	0.35
5	0.07	0.36	0.10	0.28	0.11	0.13
6	0.06	0.28	0.12	0.28	0.13	0.50
7	0.09	0.18	0.08	0.26	0.08	0.14
8	0.10	0.08	0.13	0.23	0.11	0.29
9	0.07	0.24	0.08	0.12	0.13	0.36
10	0.08	0.20	0.11	0.31	0.14	0.43
11	0.06	0.11	0.04	0.14	0.10	0.12
12	0.05	0.16	0.08	0.43	0.10	0.32
13	0.07	0.15	0.10	0.27	0.09	0.11
14	0.04	0.07	0.08	0.22	0.16	0.26
15	0.09	0.38	0.11	0.62	0.10	1.03
16	0.10	1.13	0.09	0.90	0.11	1.45
Average	0.07	0.27	0.09	0.33	0.12	0.41

. Keep in mind that these results show the quality of the detected Windrow Lines compared to one another and not to an actual reference position of the windrow.

The results show that a higher Downsampling Radius increases the standard deviations. On average, the standard deviations for Set A of the estimated start and endpoints was 0.07 m and the average standard deviation of the orientation was 0.27°. They increase to 0.12 m and 0.41° when looking at the results of Set C. However, the results for Set C are still sufficiently accurate to navigate the compost turner to the start of a windrow. Additionally, increasing the Downsampling Radius (Set C) reduces the size of the point cloud by a factor of 8. This minimizes the computational load and can reduce the hardware requirements for the mobile processing unit of the compost turner.

The last two windrows (IDs 15 and 16) are shorter than the other windrows. Since the orientation is computed from the start and endpoints, the shorter distance between these points explains the larger standard deviation.

4. Discussion

This paper presented a route planning module for an autonomous compost turner. The module automatically detects the windrows on the composting plant in an initialization phase using LiDAR. This is achieved by first generating a global point cloud of the windrows from which the individual windrow clusters are extracted. Then the ridge points of each windrow cluster are retrieved and an optimal route through them is estimated. The routes through each windrow are then incorporated into a global route planning algorithm. Via user input, the windrows that the robot should turn can be selected and the optimal route to, through, and between the selected windrows can be calculated. The algorithm was tested in a simulation environment.

Tests have shown that the algorithm reliably manages to detect all windrows after the initialization phase. The start and endpoints of each route through a windrow were detected with a standard deviation of less than 0.15 m, while the orientation of each route through the windrow was estimated with a standard deviation of less than 0.5°. Therefore, this algorithm is suitable for the estimation of the windrow start and endpoints.

Future research will focus on the evaluation of the method using real-world data, including tests at a real composting plant. Currently, an obstacle detection and avoidance module as well as a precise local approach strategy to the windrows are being developed.

On the path toward smart compost plant management, autonomous compost turners play an important role. When equipped with additional sensors, they can be used to georeference and monitor temperatures and greenhouse gas emissions. This can help to ensure quality control. Furthermore, autonomous compost turners allow increasing efficiency at the composting site.