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.