# Autonomous Navigation Based on Pattern Matching Using 3D Sensors

Florian Gaurier<fnref n="1" />, Cyrille Pierre <fnref n="1" />, Johann Laconte <fnref n="1" /> and Roland Lenain <fnref n="1" />

***Abstract***—The significant impact of conventional agriculture on climate change is likely to be reduced through more sustainable practices, such as agroecology. These emerging practices require to adapt the machines used to perform agricultural tasks. Robotics then arises as a promising solution to alleviate these constraints by carrying autonomous work. These machines require precise guidance in the highly variable agricultural environments. It is achieved by path tracking algorithms using the Global Navigation Satellite System (GNSS), currently the standard in the industry. However, GNSS-based autonomous navigation is subject to signal-loss and does not provide plant-referenced control for the vehicles. Researchers have developed new vision-based navigation strategies to ensure precise and adaptive tracking of crop rows. The sensitivity to environmental conditions (illumination, rain, dust) of cameras has motivated the development of lidar-based navigation systems. This paper presents an adaptation of a path detection algorithm, from 2D lidar tilted with the ground to 3D sensors. The impact on navigation of the previous and new algorithms are compared with a GNSS path tracking algorithm over the recorded RTK-GNSS path associated with a longitudinal strip cropping configuration of beans and bare soil. The performances are compared in regards of the mentioned algorithms interfaced with different sensors on the same robot with the same generic control law.

***Index Terms***—Agricultural robotics, Autonomous navigation, 3D pattern-matching, structure detection, path detection algorithm.

## I. INTRODUCTION

The reduction of environmental impact has become a critical issue for modern society as pollution and the use of chemicals in human activities are widely recognized as major contributors to climate change. Conventional agricultural practices significantly contribute to this issue, necessitating substantial transformations in food production systems. While certain shifts, such as Organic Agriculture and Precision Farming, have already been implemented, more comprehensive changes in agricultural practices are currently being explored under agroecological principles. These principles emphasize the integration of diverse crop types and species to combat diseases and promote the growth of targeted crops. Such an approach requires enhanced precision in agricultural operations, along with frequent, differentiated interventions on crops, thereby increasing the demand for human resources, already insufficient. The large-scale implementation of agroecological principles requires the development of innovative tools capable of autonomous operation.

Agricultural robotics presents itself as a promising solution for facilitating these emerging agricultural practices [1]. To

be effective, such robots must demonstrate a high level of precision in their interactions with crops under varying conditions. Given the increasing complexity of fields due to agroecology practices, heterogeneity of field configurations (e.g., market gardening, vineyards, arable fields), crop types, weather conditions, and the different developments stages of plants, it becomes crucial for robots to accurately navigate with respect to crop rows. Currently, GNSS based navigation systems are the standard, but they rely on continuous, reliable GNSS signal with RTK support. This can be problematic due to limited satellite coverage in the most remote regions of the planet or obstructions like trees or buildings [2].

These limitations have lead the research to provide alternative navigation solutions based on visual servoing [3], providing localization with respect to the crops but requiring reliability and precision in navigation. Many efforts have been conducted in the beginning of the century in cameras-based navigation for agricultural robots as these sensors provide rich color information used to discriminate ground from crop row [4]. Benefiting from the advances in artificial intelligence, recent developments have enabled the discrimination between crops and weeds [5]. Nevertheless, cameras are highly sensitive to the illumination conditions, to the point that some robots are equipped with dark chambers controlling environmental illumination to detect the crops efficiently. This principal limitation along with the accumulation of dust on the optics

<figure><img src="image_1.png" /><figcaption>Fig. 1: POM4x4 of Sabi Agri manufacturer, here configured with a 2D lidar at the front</figcaption></figure>

<footnote n="1">¹All authors are with Université Clermont Auvergne, INRAE, UR TSCF, 63000 Clermont-Ferrand, France. firstname.lastname@inrae.fr</footnote>
<figure><img src="image_2.png" /><figcaption>Fig. 2: Global navigation strategy. Localization and control will remain the same for the evaluated path detection algorithms (blue)</figcaption></figure>

or perturbations under the rain has motivated recent works on lidars for autonomous navigation in fields [6]. With only lidar measurements, the robot is localized with respect to the crop rows, ensuring enough precision to navigate in various fields such as vineyards [7], market gardening [8] or arable crops [9].

In a previous work, two navigation strategies based on a 2D lidar were proposed [10]. In one of the proposed configurations, an inclined lidar was used to perform structure-from-motion 3D mapping, navigation and spraying operation. The navigation stack consists of a generic path tracking control law using the lateral and angular deviations of the robot with respect to the path identified by the detection algorithm. This former is composed of a pattern matching algorithm to find the structure-to-follow in each lidar scan, succeeded by a structure-from-motion algorithm to form the path. This navigation strategy involves a blind distance during initialization to let the structure-from-motion algorithm construct the trajectory. Moreover, the lidar inclination is at the cost of a reduced anticipation range, limiting the capabilities of the robot to adapt to curvature, absence of crops or unexpected events.

These two key limitations have prompted the upgrade of this navigation strategy to 3D sensors, presented in this paper. It allows solving both issues by using the vertical field of view to initialize and anticipate the paths to detect. The impact of the detection path algorithm and sensor modalities on navigation in agricultural robotics, especially when using 3D information, has not yet been evaluated. To address this, we propose an experimental framework to compare the efficiency of the navigation strategies in terms of precision by comparing the state-of-the-art GNSS navigation, the previously developed pattern matching strategy with 2D lidar and the pattern matching strategy using multiple 3D sensors.

## II. GLOBAL AUTONOMOUS NAVIGATION STRATEGY

In this paper, the pattern-matching path detection algorithm is adapted to 3D sensors to overcome the limited anticipation range of 2D sensors. As the performances of both path detection algorithms have not yet been compared to the usual GNSS path

tracking, the impact of the path detection algorithm over control is to be assessed. Therefore, both control and localization have to remain the same across each evaluated detection mode. The overall pipeline used for the navigation system is presented in fig. 2.

The generic control law is based on an extended kinematic model of the robot motion that is proposed in [5]. The exact linearization properties of such representation enables to compute a mathematical expression for the front steering angle to allow the convergence of the tracking error to zero, whatever the robot speed. In this paper the parameters has been set to ensure the same settling distance of 8 m in all the tested cases. This generic control law uses the lateral and angular deviations as inputs, provided by every proposed path detection algorithms regardless of the sensor used (GNSS, 2D lidar, 3D cameras). A Kalman filter is used to fuse IMU, GNSS and odometry measurements, providing the filtered odometry and position of the robot used for control and path detection. It should be noted that while GNSS is not essential, it is used in this paper to eliminate bias introduced by different localization algorithms.

## III. REMINDER ON PATTERN MATCHING PATH DETECTION ALGORITHM

Initially authored in [8], the path detection algorithm uses a lidar positioned at the front of the robot and inclined with the ground (cf. fig. 1) to detect the previous field operation wheelprints, small plants or vines. The algorithm is adapted for this paper and performs four main steps:

**Preprocessing (a)** converts the measurements of the 2D lidar from the sensor to the robot frame with (x, y, z) being respectively the front, left and up axes. The horizontal field of view can be limited to reduce the computation load of the next step.

**Detection (b)** computes the convolution values $r(k)$ at offset $k$ to find, inside the preprocessed lidar Z-axis measurements grouped in a vector $Z$ of size $n$ and median $\bar{z}$, the best matching positions of a pattern vector $M$ of size $n$ initialized to zero,
<figure><img src="image_3.png" /><figcaption>Fig. 3: Lidar Z-axis signal acquired in proposed experimental setup (top) and associated convolution score (bottom) showing the highest points in detected interval and associated convolution points (red points)</figcaption></figure>

corresponding to the crop's shape and modeled by a squared-shaped signal with p values set to height (in meters). The convolution expression used is:

$$r(k) = \frac{1}{\|M\|^2} \sum_{j=1}^{n} m_j (z_{j+k} - \bar{z}) \quad (1)$$

For each local convolution maxima $r_{\max}^i$ above a user-defined threshold, an interval $I$ is constructed. It corresponds to a bump of height $b$, such that $I = \{j \in [0, 2n+1] | r_{\max} - r(j) \le b\}$. Figure 3 (top) shows a lidar scan disturbed by plant foliage, as laser beams pass through openings between leaves and capture points from the ground, in the row extending from index 65 to 100, at index 80. The bottom figure shows the associated convolution values to the detected points. The detected point $D_i$ associated to the convolution maxima $r_{\max}^i$ is then the highest point inside this interval $I$ to ensure a ground point is not selected, such as index 80.

The selection stage (c) is required to identify inside the detected points $D_i$, the most representative of the crop-row. This work uses the two-rows detection mode adapted from [9]. For clarity, we will only express the conditions to find the most representative point of the left row, as the same conditions apply to the right one. The most representative point is the closest candidate point $D_i$ meeting the two conditions illustrated in fig. 4.

<figure><img src="image_4.png" /><figcaption>Fig. 4: Selection step of the pattern-matching algorithm over the detected points with user-defined criterias</figcaption></figure>

The first ensures the signed lateral deviation to the left row $y_{D_i}^L$, i.e. the lateral component of point $D_i$ expressed in the Frenet frame of left row, to remain in a maximal range $d_L$, representing the maximal admitted thickness of the row. The second ties the rows together by setting the signed lateral deviation to the right row $y_{D_i}^R$ of point $D_i$ to remain in the range defined by a minimal distance $d_R^m$ and maximal distance $d_R^M$. The most representative point of left row must then respect:

$$\begin{aligned} |y_{D_i}^L| &\le d_L \\ d_R^m &\le |y_{D_i}^R| \le d_R^M \end{aligned} \quad (2)$$

In real case scenario, scaling factors of the inter-row distance are used for the conditioning criteria $d_L$, $d_R^m$ and $d_R^M$. During initialization, rows are not available yet to compute the lateral deviations. Then, user-defined a priori lateral positions of the rows are used. It supposes that the robot is globally aligned with the rows. This ensures that the selection stage remains accurate by searching points at a priori known positions. The set of points composing the row is expanded whenever a representative point is found.

The trajectory construction (d) is done by updating the position of the selected points composing the rows with respect to the robot's motion, obtained by the localization algorithm. The points previously selected and close to the robot are filtered to approximate the row by a linear regression in the form:

$$y_{D_i} = a_k x_{D_i} + b_k \quad (3)$$

Such a regression can be computed if the number of points are sufficient and covering a significant distance. It then results in a blind distance at the initialization. The coefficients of the regression are used to express the lateral $y_k$ and angular $\tilde{\theta}_k$ deviations between the robot's pose $(x_r, y_r, \theta_r)$ and the row $k \in \{L, R\}$ as follows:

$$\begin{aligned} y_k &= \frac{a_k x_r - y_r + b_k}{\sqrt{(a_k^2 + 1)}} \\ \tilde{\theta}_k &= \theta_r - \arctan(a_k) \end{aligned} \quad (4)$$

These equations expressed in the robot frame become:
<figure><img src="image_5.png" /><figcaption>Fig. 5: (left) RTK GNSS antenna position when recording crops positions – (right) recorded RTK GNSS paths</figcaption></figure>

$$ 
\begin{aligned}
y_k &= \frac{b_k - y_r}{\sqrt{a_k^2 + 1}} \\
\tilde{\theta}_k &= -\arctan(a_k)
\end{aligned} 
\qquad (5) 
$$

The regression is valid as long as $\tilde{\theta} \neq \frac{\pi}{2}$, which is not frequently encountered as the robot is controlled to stay aligned with the structure. Finally, the expressions of lateral and angular deviations of the robot with respect to the left and right rows are combined as inputs for the **control law (e)** as follows:

$$ 
\begin{aligned}
y &= \frac{y_L + y_R}{2} \\
\tilde{\theta} &= \frac{\tilde{\theta}_L + \tilde{\theta}_R}{2}
\end{aligned} 
\qquad (6) 
$$

The detection path algorithm uses a convolution score to find and construct the path associated with the crop rows. However, errors during **selection (c)** can lead to regression inaccuracies during **trajectory construction (d)**, which worsens the row estimation in a feedback loop, making the initialization phase a critical step. Moreover, the use of 2D lidars reduces the anticipation range of the robot and requires to set the optimal inclination angle with respect to anticipation, safety and navigation capabilities.

## IV. ADAPTATION TO 3D SENSORS

The vertical field of view of 3D sensors in such detection path algorithm enables to reduce the blind distance and improve the precision of the control while enabling safety components. The **preprocessing (a)**, **selection (c)** and **trajectory construction (d)** steps remain the same.

The depth data obtained from 3D sensors can be organized in a matrix whose indexes are associated with the cartesian product between the elevation and azimuth angles of the sensor. A vector of measurements associated to a fixed elevation angle, named layer, can be formed from this depth matrix. The adaptation of the pattern matching algorithm to 3D sensors is to apply the **detection (b)** to each layer.

An adaptation of the convolution score is proposed to speed up the computation process as many convolution coefficients result in zero-values due to the $n-p$ zeros in model vector $M$. This adaptation reduces the pattern vector $M$ of size $n$,

<figure><img src="image_6.png" /><figcaption>Fig. 6: Bean field with sparse bean crops, low weeding (up-left and bottom) and bare soil area (top-right)</figcaption></figure>

to a vector $M_p$ of size $p$, the model width. By computing the matrix $P = Z_c M_p^T$ with $Z_c = Z - \bar{Z}$ and $\bar{Z}$ the median of $Z$, the convolution coefficient $r(k)$ is the sum over the $k^{th}$ diagonal of matrix $P$ at the offset $k \in [-\frac{n-p}{2}, \frac{n+p}{2}]$. Let $j = \max(1, p-k+1)$, $q = \min(p, p+n-k)$, eq. (1) becomes:

$$ r(k) = \frac{1}{\|M\|^2} \sum_{i=j}^{q} m_i (z_{i+k-p} - \bar{z}) \qquad (7) $$

With this new size $p$ of model, the complexity to compute the convolution changes from $O(2n^2)$ to less than $O(p^2+pn)$.

## V. EVALUATION STRATEGY AND EXPERIMENTAL SETUP

The presented path detection algorithms are compared using multiple sensing modalities showed in table I and selected with respect to price, quality and technology. They are integrated in the navigation pipeline presented in Fig.1 to show the impact of sensor and path detection algorithm on navigation. The GNSS path following is settled as reference and uses a path matching algorithm, replacing the path detection algorithm in fig. 2. It computes the lateral and angular deviations, inputs of the control law, by matching the position of the robot with respect to the surveyed path using an RTK-GNSS antenna placed at the base of crops (fig. 5).

<table><thead><tr><td>Sensor name</td><td>Sensor type</td><td>Path detection algo</td></tr></thead><tbody><tr><td>Septentrio AsteRx SB3</td><td>GNSS</td><td>GNSS Path Matching<fnref n="1" /></td></tr><tr><td>Sick LMS151</td><td>2D lidar</td><td>2D Pattern Matching</td></tr><tr><td>Stereolabs Zed 2i</td><td>3D stereo-camera</td><td>3D Pattern Matching</td></tr><tr><td>Intel Realsense D455</td><td>3D stereo-camera</td><td>3D Pattern Matching</td></tr><tr><td>Sick Visionary T-mini</td><td>3D stereo-camera</td><td>3D Pattern Matching</td></tr></tbody></table>

<caption>TABLE I: Sensing modalities for the comparative study</caption>

<footnote n="1">¹Reference.</footnote>
<figure><img src="image_7.png" /><figcaption>Fig. 7: Lateral deviation (left) and angular deviation (right) of the robot with respect to the surveyed path along the curvilinear abscissa. Deviations depicted are obtained by the path matching algorithm over the surveyed crops. Only the GNSS deviations are used as inputs of the control law.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 8: Lateral (left) and angular (right) perception defects. The perception defects are computed by the difference between the real deviation obtained via the path matching algorithm with respect to the surveyed crops, and the perceived deviation sent to the controller by the path detection algorithms.</figcaption></figure>

Remind that each of these sensing modalities are interfaced with the same control law on the same robot; therefore, the bias due to control and mechanical defects remain the same across the experiments. The real deviations with respect to the surveyed path are recorded as performance metrics of the navigation.

A laser tracker Leica AT930 and the sensor manufacturer's datasheet have been used to calibrate each sensor, mounted on a structure placed at the front of the robot.

The experiments occurred at Montoldre, Allier, France to track and follow rows in a longitudinal strip cropping configuration of beans (20 m) and bare soil, i.e. without crops (2 m) (fig. 6).

## VI. RESULTS

The deviations of the path matching algorithm are the first performance metrics to be exploited in this section. Figure 7 presents lateral and angular deviations along curvilinear abscissa. The areas with bare soil have been highlighted in red. As expected, the GNSS path tracking performs best: the robot remains globally aligned and shows lower deviation even in the bare soil areas, underlining the absence of adaptation to the crop rows, due to the use of a surveyed path, not always available in a real case scenario. The 2D-pattern matching path detection algorithm presents a maximal lateral deviation of

11.7 cm after the bare soil area at a curvilinear abscissa of 110 m. This may cause crop damages as the robot sees the next set of crops too late. This overshoot results from a latency in the control of the robot mainly due to the lack of anticipation of the 2D lidar. The 3D-pattern matching strategy completely avoids this overshooting behavior, proving the efficiency of the developed method.

These lateral and angular deviations with respect to the surveyed path show the capacities of the path detection algorithm to accurately feed the control algorithm, giving an insight on the precision of such navigation pipelines. However, these deviations are subject to the latency of robots' actuators and the control performance. Therefore, the perception defects, the difference between the deviations with respect to the surveyed path and the deviations sent to the controller, are computed in fig. 8.

The lack of anticipation of the 2D pattern matching path detection algorithm shows a maximal defect of 14.5 cm. Hopefully, the deviation has decreased before the robot have reached the command. Perception defects occurs mainly in the bare soil areas and is easily explained by absence of crops in these regions. It is confirmed that 3D pattern matching path detection algorithm performs better, whatever the sensor used.

The path following strategy is confirmed to be the most
<figure><img src="image_9.png" /><figcaption>Fig. 9: Violin plots of lateral deviation of the closed loop (left) and perception stage (right) over each sensing modality</figcaption></figure>

precise strategy in the statistical analysis showed in fig. 9. Mean lateral deviation of GNSS path tracking, is at 0.83 cm with a standard deviation of 1.38 cm. As expected, this reference performs the best due to the error over path detection minimized thanks to the surveying of crops. The 3D sensors also perform differently from each other, with a surprise on the Sick Visionary T-mini, marketed has an indoor sensor but offering the best performances of all evaluated sensors. It has to be underlined that even though the Zed2i and Realsense D455 are performing almost equally with the 2D lidar (Without the overshoot), their cost put them as good candidates for low-cost row-tracking sensors.

Figure 9 right once more shows the efficiency of the 3D pattern matching, reducing the maximal perception defect by 51.26 %. The Realsense D455 presents lower perception defects than the Sick T-mini with higher lateral deviation. This is explained by the initialization phase, presenting a bigger settling distance of the Realsense D455 compared to the Sick T-mini. The precision of GNSS path tracking is obtained by the precise surveying of crop-rows. In a real case scenario, this ground truth is not available nor desired in a usual farming cycle. Moreover, the survey should remain valid along all the cycle, which is not guaranteed. These pragmatic considerations reinforce the use of perception based navigation.

## VII. CONCLUSION

Performing safe and precise autonomous ecological work in agricultural fields requires adaptive navigation algorithms by improving the robot perception capacities and enabling localization with respect to the crop-rows. Relying on previous works in the domain, this paper improved the perception capacities of the robot by adapting the pattern-matching path detection algorithm to 3D sensors. This adaptation uses the vertical field of view of the 3D sensor to find at each elevation angle, the position of the rows, that forms locally a line from which the lateral and angular deviations can be computed and sent to a controller. An experimental framework to evaluate the navigation performances in an agricultural context have

been proposed. It relies on the exploitation of the lateral and angular deviations of robot to the ground truth, i.e. the surveyed positions of crop-rows using RTK-GNSS antenna. The workload induced to ensure a precise GNSS based path tracking along with the signal loss issues are not suitable when working in agricultural fields. In this regard, perception-based navigation strategies offer suitable tracking performances for agricultural tasks. The proposed algorithm, in a two-row tracking setup, is adapted to 3D sensors and overcomes the limitations of its predecessor, a 2D pattern-matching detection path algorithm. Mainstream stereo-vision cameras have been tested on the adapted algorithm and perform as low-cost candidates while Time of Flight cameras offer lower tracking errors. In future works, 3D lidar performances will be assessed along with the impact of misplacement at initialization and the presence of curvature in the field. The impact of the reference path in GNSS path tracking is also to be deepened.

## REFERENCES

[1] L. Ditzler and C. Driessen, "Automating Agroecology: How to Design a Farming Robot Without a Monocultural Mindset?," *Journal of Agricultural and Environmental Ethics*, vol. 35, p. 2, Mar. 2022.

[2] M. Pini, G. Marucco, G. Falco, M. Nicola, and W. De Wilde, "Experimental Testbed and Methodology for the Assessment of RTK GNSS Receivers Used in Precision Agriculture," *IEEE Access*, vol. 8, pp. 14690–14703, 2020.

[3] J. Shi, Y. Bai, Z. Diao, J. Zhou, X. Yao, and B. Zhang, "Row Detection BASED Navigation and Guidance for Agricultural Robots and Autonomous Vehicles in Row-Crop Fields: Methods and Applications," *Agronomy*, vol. 13, p. 1780, June 2023.

[4] Y. Bai, B. Zhang, N. Xu, J. Zhou, J. Shi, and Z. Diao, "Vision-based navigation and guidance for agricultural autonomous vehicles and robots: A review," *Computers and Electronics in Agriculture*, vol. 205, p. 107584, Feb. 2023.

[5] C. Samson, P. Morin, and R. Lenain, "Modeling and Control of Wheeled Mobile Robots," in *Springer Handbook of Robotics* (B. Siciliano and O. Khatib, eds.), pp. 1235–1266, Cham: Springer International Publishing, 2016. Series Title: Springer Handbooks.

[6] K. Zywanowski, A. Banaszczyk, and M. R. Nowicki, "Comparison of camera-based and 3D LiDAR-based place recognition across weather conditions," in *2020 16th International Conference on Control, Automation, Robotics and Vision (ICARCV)*, (Shenzhen, China), pp. 886–891, IEEE, Dec. 2020.
[7] D. Iberraken, F. Gaurier, J.-C. Roux, C. Chaballier, and R. Lenain, "Autonomous Vineyard Tracking Using a Four-Wheel-Steering Mobile Robot and a 2D LiDAR," *AgriEngineering*, vol. 4, pp. 826-846, Sept. 2022.

[8] T. Tourrette, R. Lenain, R. Rouveure, and T. Solatges, "Tracking footprints for agricultural applications: a low cost lidar approach," in *International Conference on Intelligent Robots and Systems (IROS)*, Workshop "Agricultural Robotics: learning from Industry 4.0 and moving

into the future", (Vancouver, Canada), Sept. 2017.

[9] C. Pierre, R. Lenain, J. Laneurit, and V. Rousseau, "A Multi-Control Strategy to Achieve Autonomous Field Operation," *AgriEngineering*, vol. 4, pp. 770-788, Aug. 2022.

[10] D. Iberraken, R. Lenain, F. Gaurier, C. Pierre, and J.-C. Roux, "Structure tracking strategies in different agricultural configurations," *IFAC-PapersOnLine*, vol. 56, no. 2, pp. 8926-8933, 2023.