2.2 Improved LeGO-LOAM for Greenhouse Environment Mapping

To mitigate accumulated pose estimation errors and achieve precise navigation in complex greenhouse environments, a relocalization module was integrated into the LeGO-LOAM framework in this study.
Initially, the newly collected LiDAR point cloud data are set as \(P\ =\ \{p_{1}\ ,\ \ p_{2}\ ,\ \ ...,\ p_{n}\}\),where \(p_{i}\ =\ (x_{i}\ ,\ \ y_{i}\ ,\ \ z_{i})\ \)is a point in the LiDAR coordinate system. Then the point cloud \(P\) is transformed into the global coordinate system through the following equation:
\(P^{\prime}\ =\ RP\ +\ t\)      (1)
where \(P^{\prime}\ \)represents the point cloud data in the global coordinate system, and \(R\) and \(t\) denote the rotation matrix and translation vector from the LIDAR coordinate system to the global coordinate system. Subsequently, the process involves downsampling and filtering, the details of which can be found in the original text. For the sake of brevity, we will concentrate on delineating our specific contributions:
1) Initial Relocalization via Map Matching. In the process of map matching, our objective is to find a transformation \(T\) that minimizes the Euclidean distance between the transformed point cloud \(P^{\prime}\) and the historical map data \(M\), represented as \(M\ =\ \{m_{1}\ ,\ \ m_{2}\ ,\ \ ...,\ m_{m}\}\) . Each point\(m_{i}\) of \(M\) is within the global coordinate system. This optimization objective can be formalized as the following minimization problem:
\(\text{mi}n_{T}\sum_{i=1}^{n}{\mid\mid T*p_{i}^{{}^{\prime}}-m_{i}\mid\mid^{2}}\)     (2)
The term \(\mid\mid T*p_{i}^{{}^{\prime}}-m_{i}\mid\mid^{2}\)  represents the Euclidean distance between the transformed point in the point cloud\(P^{\prime}\) and the corresponding point in the map \(M\). The pose \(T\), representing a rigid body transformation, is estimated via the Iterative Closest Point (ICP) optimization algorithm, a method typically employed for minimizing the difference between two clouds of points. The resulting pose \(T\) is a six degrees-of-freedom (6-DoF) pose that encapsulates three components of rotation \((yaw,\ pitch,\ roll)\ \)and three components of translation \((x,\ y,\ z)\).
2) Refinement. The estimated pose is then refined using a Kalman filter (Simon, 2001). The methodology consists of two sequential steps:firstly,we utilize the dynamics model of the system to predict the current state and covariance of the state. This prediction is formulated by the equations:
\({\hat{x}}_{k}^{\text{cal}}=Fx_{k-1}+Bu_{k-1}\)    (3)
\({\hat{P}}_{k}=FP_{k-1}F^{T}+Q\)      (4)
where \(x_{k-1}\) and \(P_{k-1}\) are the predicted state and covariance, \(F\) is the state transition matrix, \(u_{k}\) is the control input, \(B\) is the control matrix, and \(Q\) represents the process noise covariance matrix.
Subsequently, we use the observed data to update the predicted state and its covariance. This update can be represented by the equations:
\(K_{k}=\frac{{\hat{P}}_{k}C^{T}}{C{\hat{P}}_{k}C^{T}+R}\text{\ \ }\)   (5)
\({\hat{x}}_{k}={\hat{x}}_{k}^{\text{cal}}+K_{k}\ \left(z_{k}-H{\hat{x}}_{k}^{\text{cal}}\right)\text{\ }\)   (6)
 \(P_{k}=\left(I-K_{k}*H\right){\hat{P}}_{k}\text{\ }\)    (7)
where \(K_{k}\) is the Kalman gain, \(z_{k}\) is the observation, \(H\)is the observation model matrix, and \(R\) is the observation noise covariance matrix.
Through these two steps, the Kalman filter optimizes the estimated pose, leading to a more robust and precise localization in the context of LeGO-LOAM.
Finally, the pose and relocalization data are published. Upon the receipt of new mapping data or loss of localization, the system repositions itself following the identical procedural sequence. This methodology provides the robot with precise positioning capabilities in complex environments.