**2. Materials and Methods**

### *2.1. The Composition of the Pusher Robot System in the Farm*

The pusher robot needs to replace the labor for the feeding process, thereby reducing the feed cost and labor intensity of feeding dairy cows. The pusher robot can meet the functions of autonomous walking and pushing. Therefore, the pusher robot was mainly composed of a vehicle navigation hardware system, pusher operation system, and navigation and operation control system. Among them, the vehicle navigation hardware system and the pushing operation system were the specific execution systems of the instructions, which were responsible for receiving and executing the task instructions issued by the control system to complete the autonomous navigation and pushing operation. The navigation and operation control system were responsible for setting the working mode of the vehicle system, issuing target point instructions, displaying the robot position in real time, and controlling the pushing operation system. Through the fusion and analysis of various sensor information, the pusher robot could realize autonomous positioning and navigation in the natural environment.

The hardware device and the control system communicated in real time via a wireless network to complete the autonomous navigation and operation tasks of the pusher robot on the farm together, as shown in Figure 1. The vehicle navigation hardware system mainly included a robot chassis, drive module, control module, environmental information perception module, communication module, and power supply module.

**Figure 1.** The vehicle navigation hardware system.

According to the task of the farm operation and the needs of the environment, the driving module of the robot adopted a two-wheel differential drive structure, and the steering control of the robot could be realized by setting different speeds for the two driving wheels. This drive system was not only simple in structure and small in turning radius, but also more flexible in movement, which greatly improved the control accuracy of the whole machine. The power system was provided by 60 V lithium battery modules. In order to ensure that the robot had powerful computing functions, the main control unit used Jetson Nano development board (NVIDIA, Shanghai, China), equipped with Tegra X1 heterogeneous SOC (NVIDIA, Shanghai, China), the size of this unit was 100 mm × 80 mm × 29 mm. The basic framework of ROS navigation was built under the Ubuntu 18.04 system, and information was exchanged with the chassis using RS-485 communication. The generated signal was transmitted to the main control unit via USB3.0. The car was equipped with the STM32F103 embedded motherboard (Haoyao, Shenzhen, China) as the underlying controller. According to the speed information provided by the encoder, the odometer data (moving speed, driving distance, and turning angle) of the vehicle system were obtained through kinematics calculation. Finally, the control of the vehicle-mounted system and the pushing operation system was completed through the control algorithm.

The environmental information perception module uses a 16-beam miniature LiDAR (RS-LiDAR-16, Sagitar Juchuang, Shenzhen Sagitar Juchuang Technology Co., Ltd., Shenzhen China). The compact housing of the RS-LiDAR-16, mounted with 16 laser/detector pairs, rapidly spins and sends out high-frequency laser pulses to continuously scan the

surrounding environment, collecting real-time 3D point clouds. The 3D space point cloud data and object reflectivity are provided by the distance measurement algorithm, so that the pusher robot can digitally model the cowshed, providing a strong guarantee for its positioning, navigation, and obstacle avoidance. The lidar is installed in the center of the front end of the robot chassis, at a height of 0.6 m from the ground, and its performance parameters are shown in Table 1.

**Table 1.** Parameters of LiDAR.


The 3D schematic diagram and physical map of the installation of each module of the pushing robot are shown in Figure 2. The overall length of the pusher robot is 1.78 m, the width is 1.15 m, the height is 1.40 m, and the rated load is 1 m3.

**Figure 2.** The Pusher Robot.

#### *2.2. Collection and Preprocessing of Point Cloud Data*

#### 2.2.1. Collection of Point Cloud Data

The 3D point cloud data of cowsheds were collected in Jinlan Dairy Farm (Figure 3) in Tai'an City, Shandong Province, China from 16 to 30 October 2021. The cowshed is arranged in a double row with a distance (D) of 6.25 m between the two pens. The point clouds of the cowshed are unevenly distributed, with dense clouds at the near end and gradually sparse ones at the far end (Figure 3b). As shown in Figure 3b, taking the geometric center of the lidar as the origin point O, the forward direction of the robot is the positive direction of the *Y*-axis, the vertical *Y*-axis to the left is the positive direction of the *X*-axis, and the *Z*-axis is determined by the right-hand rule to establish a 3D lidar local coordinate system. This study extracts the 3D point cloud data in the range of *X*-axis (0–10 m), *Y*-axis (−20 to 10 m), and *Z*-axis (0–2 m) from the coordinate system as the region of interest (Region of Interest, ROI). Feeding of dairy cows will lead to the cluttered distribution of some of the far-end feeds, and the collected point cloud data will be messier. Therefore, it is necessary to filter out the ground point cloud to reduce the interference of the ground point cloud data on the initial path extraction.

**Figure 3.** Cattle farm 3D point cloud acquisition: (**a**) Experimental cattle farm, (**b**) 3D point cloud of the original cattle farm.

#### 2.2.2. Preprocessing of Point Cloud Data

There are about 16,000 points in each frame of the collected 3D point cloud data of the cowshed, which is a huge amount of data. In order to reduce the amount of calculation, it is first necessary to preprocess the 3D point cloud data of the cowshed to remove noise and outliers [23,24]. Then, use the pass-through filtering algorithm to extract the ROI point cloud; the centroid of the cube is used to represent all points in the cube, and the voxel filtering algorithm downsamples the point cloud to greatly reduce the number of 3D point clouds while preserving the structural features of the 3D point cloud data. Therefore, this study uses a cube with a side length of 0.1 m to downsample the ROI point cloud. There are still many noise points and outliers in the filtered 3D point cloud, so statistical filtering is used to remove the noise and outliers [25]. In order to reduce the interference of the ground point cloud on the cowshed outline extraction, the ground plane fitting (GPF) algorithm proposed in the literature [26] is used to segment the ground and non-ground point clouds.

#### *2.3. Fence and Initial Path Extraction*

To make the segmented fence show a better effect, the preprocessed bullpen 3D point cloud was segmented by a high threshold method. The height threshold was determined according to the actual height of the cowshed and empirical methods, and the height threshold here was set to 0.1 m. The fence point cloud has apparent line features. The fence point cloud is projected onto the XY plane, and the fence lines are extracted by LSM and RANSAC, respectively, and the extraction effects of the two are compared. Project the fence point cloud on the XY plane, extract the boundary contour features of the fence point cloud, and calculate the navigation path of the pusher robot according to the fence's boundary outline to improve the mobile robot's accuracy in pushing grass during the operation.

#### 2.3.1. The Least-Squares Method

LSM is a mathematical tool that has been widely used in many disciplines of data processing such as error estimation, system identification and prediction, and forecasting. It finds the best function parameters for point cloud data by minimizing the sum of squared errors. The basic principle is as follows: data {(*xi*, *yi*), *i* = 1, 2, . . . , *m*}, obtain the data fitting function *ϕ*(*x*). Then, the fitting function *ϕ*(*x*) should reflect the changing trend of all data as much as possible, but it is not required to pass all data points; that is to say, there is a certain error between the fitting function *ϕ*(*x*) and the actual measured data at *xi*. Here, it is represented by *εi*:

$$\varepsilon\_{i} = \varphi(\mathfrak{x}\_{i}) - f(\mathfrak{x}\_{i})(i = 1, \dots, n)$$

In order to meet the requirement that the fitting function curve can reflect the change trend of all data as much as possible, its 2-norm is required to be a minimum.

$$\|\|E\|\|\_{2} = \left\{ \sum\_{i=1}^{n} [\varphi(\mathfrak{x}\_{i}) - f(\mathfrak{x}\_{i})]^{2} \right\}^{\frac{1}{2}}$$

where *E* <sup>2</sup> is the 2 norm of error.

In order to facilitate calculation, analysis, and application, the square of 2 norm is usually calculated, namely:

$$\|\|E\|\|\_{2}^{2} = \sum\_{i=1}^{n} [\varphi(\boldsymbol{x}\_{i}) - f(\boldsymbol{x}\_{i})]^{2}$$

This fitting method, which requires the minimum sum of squares of errors, is called the least-squares method.

The Fence Fitting Line was extracted by LSM fitting the point clouds on both sides of the mobile robot's driving direction. When the point cloud coordinates satisfy the minimum value of *F*(*W*), *W* is the parameter matrix of the fitted Fence Fitting Line equation, as shown in Equation (1):

$$F(\mathcal{W}) = \min \left(X^\prime \mathcal{W} - Y\right)^T \left(X^\prime \mathcal{W} - Y\right) \tag{1}$$

where *W* = [*k d*] *<sup>T</sup>* is the parameter matrix of the fence line; k is the slope of the fence line; d is the fence line intercept, and *m* : *X <sup>n</sup>*×<sup>2</sup> = [*X I*] is the matrix composed of the point cloud *X*-axis coordinate value matrix *Xn*×1=[*x*<sup>1</sup> *x* <sup>2</sup> ... *xn*] *<sup>T</sup>* and the unit matrix I; *Yn*×<sup>1</sup> = [*y*1*y*<sup>2</sup> ... *yn*] *<sup>T</sup>* is the matrix composed of the Y coordinate values of the point cloud. Taking the derivative of Equation (1), when *X TX* is a positive definite matrix, the parameter matrix *W* of the fence line equation is shown in Equation (2):

$$\mathcal{W} = \left(\boldsymbol{X}^{\prime T}\boldsymbol{X}^{\prime}\right)^{-1}\boldsymbol{X}^{\prime T}\boldsymbol{Y} \tag{2}$$

### 2.3.2. Random Sampling Consistency

The RANSAC method can iteratively estimate the parameters of a mathematical model from a set of observational data sets containing "outliers" [27,28]. The random sampling consensus algorithm can well estimate the model parameters from the data containing a large number of outliers, and can eliminate the interference of outliers on the estimated overall data model, and obtain the global optimal solution. It is an indeterminate algorithm. It has a certain probability of producing a good result, and to increase the likelihood, the number of iterations must be increased.

The purpose of RANSAC is to find the optimal parameter matrix so that the number of data points that satisfy the matrix is the largest. Usually, *h*<sup>33</sup> = 1 is used to normalize the matrix. Since the homography matrix has 8 unknown parameters, at least 8 linear equations are needed to solve, corresponding to the point position information, a set of point pairs can list two equations, and at least 4 sets of matching point pairs are included. The resulting matrix equation is shown in Equation (3):

$$\mathbf{s} \begin{bmatrix} \mathbf{x'}\\\mathbf{y'}\\\mathbf{1} \end{bmatrix} = \begin{bmatrix} h\_{11}h\_{12}h\_{13} \\ h\_{21}h\_{22}h\_{23} \\ h\_{31}h\_{32}h\_{33} \end{bmatrix} \begin{bmatrix} \mathbf{x} \\ \mathbf{y} \\ \mathbf{1} \end{bmatrix} \tag{3}$$

where *S* represents the sample data and *hij*represents a single element in the normalized matrix.

The RANSAC algorithm randomly selects 4 samples from the matching data set and ensures that the 4 samples are not collinear, calculates the homography matrix, then uses this model to test all data, and calculates the number and projection of data points that satisfy this model. Error (i.e., cost function), if this model is the optimal model, the corresponding cost function is the smallest. The resulting loss function is shown in Equation (4):

$$L = \sum\_{i=1}^{n} \left( \mathbf{x}\_{i\prime}^{\prime} \frac{h\_{11}\mathbf{x}\_{i\prime} + h\_{12}\mathbf{y}\_{i\prime} + h\_{13}}{h\_{31}\mathbf{x}\_{i\prime} + h\_{32}\mathbf{y}\_{i\prime} + h\_{33}} \right)^{2} + \left( y\_{i\prime}^{\prime} \frac{h\_{21}\mathbf{x}\_{i\prime} + h\_{22}\mathbf{y}\_{i\prime} + h\_{23}}{h\_{31}\mathbf{x}\_{i\prime} + h\_{32}\mathbf{y}\_{i\prime} + h\_{33}} \right)^{2} \tag{4}$$

where *xi* , *yi* are the elements in the parameter matrix; *xi*, *yi* are the elements in the surrogate matrix.

A matrix is obtained by random sampling, and using Equation (3), it is verified whether other points conform to the model, and then the conforming points become "internal points", and the nonconforming points become "external points". Next time, extract points from the "new interior point set" to construct a new matrix, and recalculate the error. The final error is the smallest, and the maximum number of points is the final model. The steps of the RANSAC algorithm:


$$k = \frac{\log(1 - p)}{\log(1 - w^m)}\tag{5}$$

where *p* is the confidence level, which is generally taken as 0.995; *w* is the proportion of "inner points"; *m* is the minimum number of samples required to calculate the model;

(4) If the number of iterations is greater than *k*, exit; otherwise, add 1 to the number of iterations, and repeat the above steps.

RANSAC is used to extract the fence lines on both sides, and a subset is selected from the point clouds of the fences on both sides by random sampling to establish a straight-line model. Then, the number of interior points of the straight-line model is calculated to check the correctness of the straight-line model, and iterate continuously to obtain the optimal straight-line model, which is the extracted fence line (Figure 4).

**Figure 4.** Flow chart of ridgeline extraction based on RANSAC.

The iteration threshold *KRANSAC* is a key parameter for random sampling fitting. If the value of *KRANSAC* is set too large, it will take too long, and if the value of *KRANSAC* is set too small, the fitting effect will be poor. The selection basis of the K value is shown in Equation (6):

$$K\_{RANSAC} = \lg(1 - \alpha) / \lg(1 - \omega^N) \tag{6}$$

where *α* is the probability that all points selected in the iterative process are interior points, %; *ω* is the probability that an interior point is selected from the data, %; *N* is the total number of data points.

#### *2.4. Work Path Extraction*

#### 2.4.1. Noise Processing

The real-time performance is evaluated by the processing time of extracting grid lines [29], and the anti-noise ability is the resistance ability of the fence line extraction method to two kinds of noise [30]. This study evaluates the effect of LSM and RANSAC in extracting fence lines from the aspects of real-time performance and anti-noise ability, so that the extraction method can have strong real-time performance and anti-noise ability. Add Gaussian noise and man-made noise to the fence point cloud; among them, Gaussian noise cancels the statistical filtering process, and processes the Gaussian noise with the mean value of 0 and the variance of 0.1 on the point cloud of the fence. The artificial noise is achieved by adding interfering points between two fence point clouds.
