LiDAR-Only Crop Navigation for Symmetrical Robot

This paper presents a navigation approach for autonomous agricultural robots based on LiDAR data. This navigation approach is divided into two parts: a line finding algorithm and a control algorithm. The paper proposes several line finding algorithms (based on PEARL/Ruby approach) that extract lines from a LiDAR data set. Once the lines have been processed from the data set, a control algorithm filters these lines and, using a fuzzy controller, generates the wheel speed commands to move the robot among the crop rows. This navigation approach was tested using a simulator built on ROS middle-ware and Gazebo (the source codes of the simulation are available on GitHub). The results of the simulated experiments show that the proposed approach performs well for a large range of crop configurations (with or without considering weeds, with or without holes in the crop rows…).


Context
The development of robotic tools for agriculture is a growing field. Current research is exploring a variety of tasks ranging from weeding robots [1,2] to harvesting robots [3,4]. Most of the platforms being developed aim to be autonomous.
One of the most fundamental problems for autonomous robots is autonomous navigation. In an agricultural application, such as market gardening, the robot must be able to follow the crop rows regardless of the weather conditions, the surrounding luminosity, or the configuration of the field.
Most recent works consider GPS or camera data for navigation [5,6]. However, these sensors do not provide reliable data in all conditions. Indeed, at the edge of a forest or in a greenhouse, the GPS signal is degraded. In these situations, range sensors are commonly used. This paper proposes an approach based on LiDAR (Light Detection and Ranging) data.
The work presented here aims to propose an approach for the navigation of a symmetrical robot based on LiDAR sensor data only. This paper presents a number of novelties, including: • The improvement of the existing line finding algorithm (Figures 1 and 2 introduces the line finding idea). The interested reader can refer to Section 4 for the algorithm behavior comparison; • The setup of filters and a fuzzy controller to have a full navigation stack. The control algorithm presented in this paper (Section 3) allows the simulated robot to move autonomously through the entire field. Considering a symmetrical robot eases the row changing maneuver; • The development of a new simulation based on ROS and Gazebo. To ease the reusability of the work and to be able to test the algorithms for several crop configurations, a simulation has been developed based on ROS Robot Operating System middle-ware and Gazebo (Section 4). Note that all the simulator source codes and documentation are available in Ref. [7].

LiDAR Motors Line finding
Control Motor speed Set of models Set of points The objective is to identify the crop rows from LiDAR data. The left part of the figure depicts a top view of the scene (a robot moving between two crop rows), the middle part depicts the data from the sensors (i.e., points corresponding to plants) and the right part shows an ideal result from the line finding algorithm (that is, the left and right crop rows have been identified from the LiDARs data set).

Overview of the Approach
Using LiDAR data, without prior knowledge of the field, the objective is for the robot to be able to autonomously navigate between the rows. The considered robot is a symmetrical two-wheeled differential robot. The navigation approach presented in this paper can be divided into two parts: a line finding algorithm and a control algorithm, as depicted in Figure 1.
To be able to navigate among the crops, it is necessary to identify the crop rows from the LiDAR data. This step corresponds to the line finding process of the approach and is depicted in Figure 2. One should note that it is assumed that the robot is equipped with 2D LiDAR sensors and that the sensors can detect the crop plants and the weeds (i.e., the LiDAR is low enough or the plants are high enough). In other words, plants have to be detected by the sensors, otherwise this approach can not be applied. Nomenclature resumes the considered notations.
The line finding algorithm thus provides a set of models (i.e., a set of lines) that best fit the points detected by the sensors. Those models (lines) are then filtered in order to hopefully correspond to actual rows in the crop. This filtering is done by the control algorithm, which also provides a fuzzy controller to maintain the robot in the middle of the rows. Section 2.1 presents several line finding algorithms while Section 3 describes the considered control algorithm. Section 4 presents the designed simulation and the results obtained when testing the algorithms. Finally, Section 5 concludes this paper.

Line Finding Algorithms
To ease the reading of this paper, the Ruby algorithm introduced in Ref. [8] is presented in Section 2.1.1. The novelties brought to this algorithm are then presented in Section 2.2.

The Ruby Algorithm
The Ruby algorithm is based on the Pearl method presented in [9]. Pearl, thus Ruby, is a method that aims at minimizing a function called energy. In the latter, a model L j : f j (x) = a j x + b j corresponds to a line, and L i depicts a set of models. A point p (an obstacle detected by the LiDARs) is associated with a model L(p). Note that L(p) could be the empty model L ∅ (if p is an outlier for instance). These notations are listed in Nomenclature.
The considered energy function is described in Equation (1).
For a set of models L, the energy E (L) is divided into three terms: the outlier energy E ∅ (L), Equation (2), the penalty energy E N (L), Equation (3), and the sum of all the model energies E L (L j ), Equation (6).
The outlier energy E ∅ (L i ) aims at taking into account the points that are not associated with a model (i.e., those that are associated with the empty model L ∅ ). It is defined as where ∑ p∈P(L ∅ ) 1 is the number of points in the current empty model L ∅ ∈ L i and ρ is a constant value that is heuristically chosen. As it is assumed that the LiDAR will detect more crops than weeds, it is appropriate to penalize the points that are not attached to any model (i.e., outliers and weeds). The penalty function tends to favor the association of two nearby points (according to the Euclidean distance) to the same model. This energy is defined as where N is the set of neighboring points such that an element (p, q) ∈ N corresponds to two points p and q in the same neighborhood, with p associated with the model L(p) and q associated with the model L(q). λ is a constant heuristically chosen. δ = (L(p), L(q)) is defined as additionally where ||p − q|| is the Euclidean distance between the points p and q, while ζ is a heuristically chosen constant. Finally, the energy of a model L j is defined as where ||p − L j ||is the Euclidean distance between a point p attached to the model L j and that model.

Details of the Algorithm
The Ruby algorithm is presented in Figure 3 and detailed in Algorithm 1. The different steps are explained in the latter.

1.
Line 1 Algorithm 1, which corresponds to the initialization. The initial set of models L 0 is initialized with the one from the previous find line computation L −1 i . Note that if it is the first ever call of line finding, the initial set of models is initialized with only an empty model: , all the points are associated with the empty model, Z being the set of all the points. This is relevant because in a crop navigation context, it is assumed that between two LiDAR scans, the robot will sense mostly the same crops and weeds: the rows will not be completely different from two consecutive find line calls.

2.
Lines 5 to 10 Algorithm 1. Three random points (p, q, z) are picked in L ∅ and the best model is computed (linear regression) so that it fits those points. This creates a new model that is added to the model set. This process is then repeated until half of the outliers are attached to a model (or below a threshold).

3.
Lines 11 to 16 Algorithm 1, the merging part. It merges all the models that are too close to each other.

4.
Lines 17 to 20 Algorithm 1, the deleting part. It removes all the models that do not meet the constraint E L (L j ) where E L (L j ) is the energy of the model L j as described in Equation (6), α is the constant chosen heuristically and ψ L (L j ) is the number of models of L i parallel to the model L j . ψ L i (L j ) is defined as with where a j and a k are the slopes of the models L j and L k , and β a constant chosen heuristically.

5.
Lines 21 to 24 Algorithm 1. All the points are detached from their models and reattached to the closest model. Then the new energy is computed for the complete set of models. 6.
Lines 26 to 27 Algorithm 1. If the energy of the new set of models is worse (bigger) than the previous iteration, the previous set of models is kept instead of the new computed one. 1 < threshold do 6 (p, q, z) ∈ P(L ∅ );

Ruby Suggested Refinements
In this section we will present refinements for the Ruby algorithm. These refinements are part of the originality of the work presented in this paper. They lead to an improvement of the robot navigation results, as shown in Section 4.

Ruby Genetic
Pearl and Ruby algorithms are very similar to genetic algorithms [10]. To enhance this resemblance, the Ruby genetic refinement proposes to modify the deleting part of step 2 ( Figure 3). In the classical Ruby approach, a constant α is used (Equation (7)) to decide if a model has to be removed or not. Ruby genetic proposes a fitness criterion instead, named ϕ j and defined as with E L (L j ), defined in Equation (6), ψ L i (L j )m defined in Equation (8) and where τ is a heuristically chosen constant. By using this criterion, the best models are sorted, while the others are removed.

Ruby Genetic One Point
The idea of this refinement is based on the assumption that one plant of the crop will generate several LiDAR readings. Thus, the data that are close to each other can be merged together as they may belong to the same plant. This leads to the computation of a new point set named Z * . The process is described in Algorithm 2.
For this Ruby Genetic One Point refinement, the input data is no longer Z, as defined in Algorithm 1, but Z * . The rest of the algorithm remains the same as for the Ruby genetic.

Ruby Genetic One Point Positive/Negative
This refinement proposes to change the way models are found (step 1 in Figure 3). For the classical Ruby approach, three points are randomly taken from the empty model L ∅ . However, assuming that the robot will most of the time be parallel to the crop rows, and not perpendicular to them, the points can be separated into two sets P le f t (L ∅ ) and P right (L ∅ ), defined as Then the research for models is done into those two subsets. Thus, step 1 of the Ruby algorithm ( Figure 3), i.e., lines 5 to 10 of Algorithm 1, becomes as detailed in Algorithm 3. An example of this new model research approach is depicted in Figure 4.  Figure 4. The idea behind considering P le f t (L ∅ ) and P right (L ∅ ). Without doing so, there is a chance that the model research considers improbable models regarding the orientation of the robot (middle part of the illustration), reducing the chance of finding the best models. On the other hand, while dividing the model research into left and right points, as the rows should be on the left or on the right, the chances of finding the correct models are increased (the right section of the illustration).
Algorithm 3: Ruby Genetic One Point Positive/Negative-Search models-step 1.

Ruby Genetic One Point Positive/Negative Infinity
The last considered refinement was proposed after noticing that as a point can only be associated with one model, a bad model can consume a point of a potentially good model, as it can be noticed in the middle part of Figure 4. To handle this limitation, this refinement proposes to allow a point to be attached to several models at the same time.
The main drawback of this approach is that it is time-consuming: all the points have to be checked all the time.

The Control Algorithm
Once models (lines) have been extracted from the points (the data set from the LiDAR), a control can be processed based on those models. The control algorithm presented here can be divided into three main steps: an initialization, filtering, and control step. The steps are depicted in Figure 5.

Initialization
The idea of the overall approach is to allow the robot to autonomously navigate into a crop with the minimum of prior information (width and length of the rows for instance). The only assumptions about the field are that: • The field is organized in straight lines, as this is the case most of the time [11,12]; • The distance between two rows does not change inside a crop; • The initial position of the robot is somewhere between two crop rows; • The initial orientation of the robot is roughly parallel with the crop rows.
It can be noticed that the two first assumptions (about the field configuration) do not reduce the application as it is done in most crop row detection approaches [13,14]. The last two assumptions (about the robot's initial position) may be more restricting, but as long as it is assumed that the robot can be placed in the crop, it is not an issue.
Based on these assumptions, an initialization step has been developed. The idea is to compute the distance between the rows from the models given by the line finding algorithm before starting to move the robot.
To that end, the initialization process extracts from the models the closest pair of models that are equidistant to the robot (the robot should be in the middle of two rows for its initial position) and have a slope close to 0 (the robot should be parallel to the crop rows). Once a pair of models {L 1 , L 2 } is found, the distance between the rows is computed as follows (see Figure 6).  Figure 6. Computing the distance between two parallel models L 1 and L 2 . Note that a 1 = a 2 for the models to be parallel.
With θ r the orientation of the robot, d the distance between two rows in the field and (a i , b i ) the parameters of the model L i . As It can be concluded that To have a more stable distance, this distance computation is done several times (according to several results of the line finding algorithm) and an average of all the computed distances is done to get an estimation of the distance between two rows. This estimation is then stored for use by the robot's navigation.

Filtering
The line finding algorithm, as detailed in Section 2.1, provides models (lines) according to a point set (LiDAR data). However, most of the time some returned models do not correspond to effective rows in the field (Figure 7). Filtering is then required to remove models that do not meet the field geometry assumptions (distance d between the rows and previously computed and verified models). This filter is presented in Algorithm 4 and is detailed in the latter.
The result of the filtering is a filtered model set L n = {L 1 , . . . , L i , . . . , L n }, with n a fixed number of models. Note that a filtered model L i may be extracted from the models L provided by the line finding algorithm or may be computed according to other filtered models L j and the row distance d. Figure 8 depicts an expected filtered result according to the situation described in Figure 7. The filtering algorithm can be divided into two parts: searching for expected models and computing missing models.
The search for the expected models, lines 4 to 8 of Algorithm 4, is based on the assumption that between two iterations the robot will not move significantly, i.e., the rows are mostly at the same place as they were for the prior iteration. It means that the new models should be similar to the previous ones. Thus, from the new model set L provided by the line finding algorithm, we search all the models that are close enough (according to a threshold) to the previous filtered ones L n,−1 . If a match is found, this new model is added to the new filtered set L n . Note that if during this step no match is found, the previous filtered models are kept (L n = L n,−1 ). If this happens for several iterations then the robot is considered lost and stops navigating.
Once all the models from L have been tested, some expected filtered models L i may not have a match in the model set L (at most n − 1). That is, they have to be computed regarding the found filtered models and the row distance d, lines 9 to 12 of Algorithm 4. The computation of a missing model L i is done as where a is the average slope of the found filtered models and where i best is the index of the best match At the end of this filter step we have n models L n = {L 1 , ..., L n } that should be consistent with the crop and the robot configuration. These are the models that are considered by the controller presented in the next section.

Fuzzy Controller
A fuzzy controller is a classical approach when dealing with a mobile robot control that can be applied to agricultural robots [12,15]. The objective of the controller detailed in the latter is, according to the filtered models L n , to compute the needed wheel speeds for the robot to move between the rows. The fuzzy controller presented here has three steps: • Fuzzification: transforms the inputs into fuzzy inputs (Section 3.3.1); • Rule evaluation: defines how the inputs impact the outputs (Section 3.3.2); • Defuzzification: from the fuzzy outputs, generated by the fuzzy inputs and the rules, defines a non-fuzzy output (Section 3.3.3).

Inputs of the Controller
From the filtered models L n we are only interested in the model L le f t that is directly to the left of the robot and the model L right that is directly to the right of the robot. For instance, in the example depicted in Figure 8, L le f t = L 2 and L right = L 3 .
The orientation of the robot in the field is computed as: The position of the robot between the models L le f t and L right is defined as These are the inputs of the fuzzy controller (x r and θ r ). To proceed to the fuzzification, the membership functions depicted in Figure 9 are considered. For instance, a position of −0.25 will be considered as 0.5 left and 0.5 center.

The Rules
Before defining the rules, it is necessary to define the fuzzy output membership functions. These functions are the same for the left and the right wheels (note that the considered robot is a two-wheeled differential robot) and are depicted in Figure 10.  A fuzzy controller's last step is to define its rules. These rules aim to keep the robot at the center of two rows, and parallel to them. Figure 11 shows a graphical representation of the defined rules and Table 1

Operators Summary
The considered operators for the fuzzy controller are as follows: • AND operator: minimum; • OR operator: maximum; • Implication method: Algebraic product; • Aggregation method: maximum; • Defuzzification method: the centroid method. This corresponds to the "center of mass" of the results.

Simulation and Results
To test the algorithms under several controlled environments, a simulation has been designed based on ROS middle-ware and Gazebo robot simulation. These tools are widely used in the robotics community, and thus in agricultural robotics [16][17][18]. It can be noticed that all the source code of this simulation can be downloaded from GitHub [7]. This section presents the developed simulation as well as the methodology used to test the algorithms and the results of the conducted tests. Figure 12 presents an overview of the simulation. Gazebo is used to simulate the physics of the system: it generates the LiDAR measurements from the environment and the robot's pose, and moves the simulated robot according to the wheel speed.

The Simulation
ROS nodes (i.e., programs) were developed to implement the line finding algorithms and the control algorithm.
To handle the fuzzy controller, the open source FuzzyLite C++ library was used [19].

The Robot
The simulated robot is a two-wheeled differential robot, with two caster wheels for stability reasons. Figure 13 depicts the considered robot. The robot is equipped with two LiDAR sensors, one at the front and one at the back of the robot. Both LiDAR characteristics are: (20) Two LiDAR sensors are used so that the robot will have the same amount of information from its front as from its back. This symmetry helps to handle the change of row: indeed, the robot still detects the crops when moving out of a row as the back LiDAR sensor still detects the plants behind it. Furthermore, the robot does not have to turn around when changing rows, it just has to go backwards.

The Simulated Environments
Four fields were designed to test the algorithms. From the easiest to the hardest, the set-ups are: • Crop 1, Figure 14a. This corresponds to the easiest configuration, which is a five-row crop, with equal rows, proper plant positioning, and without any weed; • Crop 2, Figure 14b. In this configuration weeds are still not considered. Nevertheless, the plants are randomly spaced among the rows; • Crop 3, Figure 14c. This is the first configuration with weeds (depicted as 50 red dots on the Figure 14); • Crop 4, Figure 14d. This corresponds to the hardest configuration, which is 5 uneven rows with holes and weeds (100 in this case). It can be noticed that the distances between the rows are not the same for all the simulated crops, but remain the same inside a crop.

Methodology
The following experiments were designed to test two things: • How the presented refinements affect the line finding results; • Considering the presented navigation algorithm: if the robot is able to autonomously navigate in the fields and how reliable the navigation is.
To do that, the same control approach (described in Section 3) has been tested with six different line finding algorithms: The two published algorithms Pearl [9] and Ruby [8], and the four proposed Ruby refinements (Section 2.2), which are Ruby Genetic (RG), Ruby Genetic One Point (RGOP), Ruby Genetic One Point Positive/Negative (RGOPPN), and Ruby Genetic One Point Positive Negative Infinity (RGOPPNI).
These six navigation algorithms (a navigation algorithm is the association of a line finding algorithm with the control approach) were tested in the four previously described environments (Figure 14).
A test run was done as follows: given a crop and a navigation algorithm, the robot is initially placed at the (0, 0) position of the environment and oriented according to the rows (as shown in Figure 15). Then the robot has to autonomously move through the rows until it reaches the end of the fifth row. Figure 15 presents an example of successful trajectory in the crop 3 environment. Before each run, the robot only has three pieces of information: • It is at the beginning of the first row between two plant rows; • It has to navigate through five rows; • The first new row will be on the left. Aside from this, it does not know the number of outliers, the distance between the rows, nor the length of the rows.
It can be noticed that for all the runs, the algorithms constants and thresholds do not change, even when changing the field environment. Furthermore, when starting a new run, all the information gathered during the previous run (e.g., the distance d), is removed.
The full experiment is presented in Algorithm 5: all the algorithms are tested five times over all the crops.

Experiment Results
Several criteria were considered to compare these algorithms.

•
The ratio of successful runs, computed as the number of successful runs divided by the total number of runs (i.e., 5). A run is considered successful when the robot managed to reach the end of the fifth row without running over the "green" plants, i.e., the crop. Figure 15 presents an example of a successful run while Figure 16 presents an example of a failed run. Table 2 details the results according to this criterion. • The mean square error when the robot navigated between two rows (not including the changing row maneuver). In a perfect situation, the robot should be exactly equidistant to the direct left and right rows at any time (once again, not including the changing row maneuver). Table 3 details the results according to this criterion. • At the end of a row, the LiDAR sensor will have fewer points than it would have between two rows (because it detects fewer plants as it is at the end of the crop). That is, in order to have a fair representation of the following criteria, they were obtained over the 500 first iterations only (that is, before reaching the end of the first row).

-
The average execution time of the line finding algorithm. As the control algorithms are the same for all the tested navigation approaches, it is only relevant to test the line finding execution time (the only part that differs from one navigation algorithm to another). Table 4 details the results according to this criterion.

-
The average number of points processed by the line finding algorithms. While some algorithms consider the raw point data set Z, others use the filtered one Z * (Section 2.2.2). That is, the average number of points, presented in Table 5, allows us to verify that for the 500 first iterations the line finding algorithms considered the same number of points (all the Z and all the Z * have consistent sizes).

-
The average execution time per 100 points. Since the line finding algorithm does not consider the same number of points (Z versus Z * ), the execution time presented in Table 4 may not be comparable regarding only the line finding processes. That is, Table 6 provides an execution time normalized on the processing of 100 points.   Regarding the data presented in the tables, it appears that filtering the input data with a genetic approach (Ruby Genetic One Point) increases the robustness and improves the precision and the computation time. As a matter of fact, combining the Ruby Genetic One Point line finding algorithm with the control algorithm provides a 100% success rate over the five experimental crops (Table 2). It also appears to be the most stable approach (Table 3). However, adding the positive/negative filtering and allowing a point to be associated with several models at the same time results in worse performance than RGOP ( Table 2).
The number of processed points is consistent with what is expected: the first algorithms named Ruby and RG consider the same number of points, while the algorithms RGOP, RGOPPN, and RGOPPNI use fewer points (due to the filter point set Z * ). This can be seen in Table 5.
The considered sensors have a rate of 40 Hz (Section 4.1.1), thus they provide a data set every 25 ms. According to Table 4, the RGOP approach needs less than 3 ms to process a data set. That is, it can be used in real time with this sensor, as can all the other algorithms.
It must be emphasized that the crops (more precisely the weed positions) were randomly generated (the source code for the generation of the simulated crops is available in Ref. [7]). This suggests that this navigation approach will provide reliable results in a wide range of crop configurations. Even if the approach needs to be tested in real conditions, the simulation results are more than encouraging.

Conclusions
In this paper a novel approach to extract crop rows from LiDAR data is presented. This approach has been coupled with a fuzzy controller to propose a complete navigation algorithm.
A simulator was developed to test this approach in various field situations, and the results are positive: according to the simulated environments, the approach appears to perform well even when the crop has uneven rows with holes and weeds.
Future work will be focused on implementing this approach into an experimental platform and performing navigation in actual fields. Furthermore, cameras will be added to the loop in order to limit the current assumptions about the field.

Conflicts of Interest:
The authors declare no conflict of interest. Nomenclature p ∈ R 2 A 2D point (provided by the LiDAR sensor for instance), p = (x p , y p ) N The set of neighboring points such that an element (p, q) ∈ N corresponds to two points p and q in the same neighborhood L j A model (a line for the crop navigation case), L j : f (x) = a j x + b j L j The j th filtered model, L j : f (x) = a j x + b j L le f t The filtered model closest to the left of the robot, L le f t : f (x) = a le f t x + b le f t L right The filtered model closest to the right of the robot, L right : f (x) = a right x + b right L j, −1 The j th filtered model of the previous iteration L ∅ The empty model (for the outliers) L i = {L j } A set of models L −1 A set of models from the previous call of line finding L n A set of n filtered models, L n = {L 1 , L 2 , ..., L n } L n, −1 The set of n filtered models of the previous iteration L(p) The model associated with the point p E (L) The total energy of the model set L E N (L) The penalty energy of associating close points to different models E ∅ (L) The outlier energy of the model set L E L (L j ) The energy of the model L j P(L j ) The set of points that are associated with the model L j , P(L j ) = {p ∈ Z |L(p) = L j } Z = {p} The set of all the points Z * A set of points based on Z, such that all the points that are close enough in Z are fused together ||X|| The Euclidean distance of the X expression ψ j Number of models parallel to model L j ρ, λ, ζ, τ, α, β Constants, heuristically chosen δ = (L(p), L(q)) A function that equals 1 if the point p is not associated with the same model as the point q, 0 otherwise w pq A weight associated with δ = (L(p), L(q)) in the computation of E N (L) δ (L j , L k ) A function that equals 1 if the models are parallel, 0 otherwise ϕ j Fitness criteria for Ruby Genetic x r The position of the robot between two rows θ r The orientation of the robot in the field d The distance between the rows (constant over the all crop) i best The index of the previous filter that best match the current expected i th one b i best The b value of the previous filter that best match the current expected i th one