CK-SLAM, Crop-Row and Kinematics-Constrained SLAM for Quadruped Robots Under Corn Canopies

Abstract

To address the localization and mapping challenges for quadruped robots autonomously scouting under corn canopies, this paper proposes CK-SLAM, a SLAM algorithm integrating robot motion constraints and crop row features. The algorithm is implemented on the Jueying Mini quadruped robot, fusing data from 3D LiDAR, IMU, and joint sensors. First, an Invariant Extended Kalman Filter (InEKF) fuses multi-source motion information, dynamically adjusting observation noise via a foot contact probability model (derived from joint torque data) to achieve initial motion state estimation and reliable pose references for point cloud deskewing. Second, three feature extraction schemes are designed, inheriting line/plane features from LeGO-LOAM and adding an innovative crop plane feature extraction module, which uses grid filtering, differential evolution for crop row detection, and RANSAC plane fitting to capture corn plant structural features. Finally, a two-step Levenberg–Marquardt iteration realizes feature matching and pose optimization, with factor graph optimization fusing motion constraints and laser odometry for global trajectory and map refinement. CK-SLAM effectively adapts to gait-induced measurement noise and enhances feature matching stability under canopies. Experimental validation across four corn growth stages shows it achieves an average Absolute Pose Error (APE) RMSE of 2.0939 m (15.7%/56.4%/72.2% lower than A-LOAM/LeGO-LOAM/Point-LIO) and an average Relative Pose Error (RPE) RMSE of 0.0946 m, providing high-precision navigation support for automated field monitoring.

1. Introduction

United Nations projections indicate that the global population will reach 9.2–10.1 billion by 2050, requiring a 70% increase in food production (vs. 2009 levels) to address undernourishment and growing demand [1,2]. This imperative drives the development of precision agriculture technologies, among which field scouting is critical for monitoring crop status and enabling targeted management [3,4]. As a staple food crop, corn’s yield stability directly impacts global food security, but its narrow row spacing and closed mid-to-late growth stage canopy create significant challenges for traditional manual scouting—limiting efficiency, coverage, and observational detail [5]. Smart agricultural robots have emerged as promising solutions for automated cornfield scouting.

Quadruped robots offer unique advantages for cornfield operations: their multi-legged mobility adapts to sloped and furrowed terrain, minimizes soil compaction, and allows row-crossing movement [6,7]. Their payload capacity supports integrated sensors for real-time data collection, establishing them as reliable scouting platforms [8]. Autonomous navigation is a prerequisite for their deployment. Traditional agricultural navigation largely relies on crop-row extraction. Several methods have been effective in structured environments such as orchards or vineyards, where row spacing is wider and canopy occlusion is less severe [9,10]. For narrow-row crops, Higuti et al. [11] used 2D LiDAR for histogram-based row segmentation; Gasparino et al. [12] enhanced stalk features to improve robustness under partial occlusion; and Affonso et al. [13] applied a self-supervised neural network to predict crop row parameters via LiDAR point cloud projection. However, crop row-based navigation has notable limitations in cornfields: it depends on continuous, intact rows (accuracy drops sharply under closed canopies, weeds, or missing plants) and only enables local “feature-following” without global positioning capability.

Simultaneous Localization and Mapping (SLAM) technology, which enables multi-feature fusion and the construction of a global environmental map, presents a viable solution for autonomous navigation within complex under-canopy settings. Nevertheless, prevailing LiDAR-SLAM methodologies encounter substantial limitations when deployed in densely planted cornfields. Conventional 2D SLAM algorithms, such as GMapping [14] and Hector SLAM [15], are constrained by their planar sensing capability and exhibit marked vulnerability to visual occlusion. Meanwhile, typical 3D SLAM methods face fundamental difficulties. Matching-based algorithms like ICP [16] suffer from performance degradation in environments dominated by repetitive plant structures, while even advanced frameworks that rely on feature extraction and matching, including LOAM [17], LeGO-LOAM [18], and their subsequent derivatives [19,20,21], are prone to pose estimation errors induced by dynamic foliar movement, uneven terrain, and a general scarcity of distinctive, stable geometric features.

To adapt SLAM to agricultural environments, researchers have developed field-specific improvement schemes, which form the core foundation for crop field navigation. For olive groves, Cheein et al. [22] proposed an extended information filter-based SLAM method, fusing visual HOG features with LiDAR data to enhance olive trunk detection robustness, achieving reliable state correction while maintaining estimation consistency. Shalal et al. [23] integrated camera and LiDAR data for orchard mapping, using tree/non-tree positions as priors and combining with an extended Kalman filter for localization—achieving a 0.103 m Euclidean distance RMSE in real orchard tests. Lepej et al. [24] used 2D LiDAR and phase correlation to calculate inter-frame poses, providing stable positioning for robots moving parallel to crop rows in apple and grape orchards. Pierzchala et al. [25] built GTSAM-based point cloud maps in forests, extracting trunk features to support forestry vehicle positioning. These studies confirm SLAM’s effectiveness in sparse, structurally stable agricultural scenes (e.g., orchards) by focusing on salient feature extraction (e.g., trunks). However, cornfields differ drastically: sub-meter narrow rows limit movement, mid-to-late growth closed canopies block features, and dynamic factors (leaf swaying, weeds) reduce feature stability—rendering orchard SLAM methods infeasible. Targeting cornfield specificity, recent progress includes: Li et al. [26] fused odometry to reduce 2D LiDAR motion distortion, cutting GMapping’s cornfield mapping error by 0.05 m; Dong et al. [27] proposed a stalk semantic feature-based LOAM improvement, segmenting corn stalks via region growing and parameterizing them as linear models, achieving a 0.88% relative trajectory error (outperforming traditional methods) with clearer plant edges. Nevertheless, facing extreme conditions such as high-density occlusion under corn canopies and dynamic plant deformation, further enhancement of feature stability and anti-interference capability is still required.

Notably, while quadruped robots show great potential in cornfield scouting, existing SLAM algorithms tailored for quadruped platforms further highlight the research gap. Most of these quadruped-specific SLAM methods are designed for relatively simple or structured terrains, lacking adaptation to cornfield complexity. For instance, Ou et al. [28] proposed Leg-KILO, a kinematic-inertial-lidar odometry framework that tightly couples leg odometry, lidar odometry, and loop closure via graph optimization to mitigate high-dynamic motion impacts. Yang et al. [29] developed Cerberus, a visual-inertial-leg odometry system achieving sub-1% drift through online kinematic calibration for agile locomotion in general indoor/outdoor environments. Zhou et al. [30] proposed a tightly coupled LiDAR-IMU SLAM method for quadruped robots, reducing positioning error by 65.08% compared to LOAM via NDT-sliding window registration. These algorithms leverage obvious environmental features or open spaces to ensure performance, but they cannot address the core challenges of cornfield navigation.

Although existing research has made progress in cornfield-oriented feature extraction and quadruped robot motion-induced anti-interference for SLAM systems, two critical limitations remain. First, conventional SLAM methods struggle to adapt to measurement noise caused by gait variations, as they inadequately account for sensor jitter and abrupt pose changes resulting from the high-dynamic motion of quadruped robots. Second, the inadequate integration of crop row features with unstructured terrain leads to feature matching failures in extreme scenarios such as straw obstruction and dense weed growth. To address these issues, this paper proposes the CK-SLAM algorithm, which integrates quadruped robots’ kinematic constraints with crop row information. Its key innovations are as follows:

• A body perception fusion strategy for legged robots based on the Invariant Extended Kalman Filter (InEKF) is proposed to establish a leg odometer, which adapts to the motion characteristics and terrain adaptability of legged robots.
• An innovative crop plane feature extraction and matching method is designed to enhance adaptability to complex farmland scenarios.
• A multi-feature hierarchical constraint and factor graph global optimization framework is developed to achieve precise robot pose estimation in farmland environments.

2. Materials and Methods

2.1. Experimental Platform

In this study, the JueYing Mini quadruped robot developed by Hangzhou CloudMinds Technology was adopted as the experimental platform. It is equipped with a core computing unit consisting of an Intel Core i7 processor (Intel Corporation, Santa Clara, CA, USA) and an NVIDIA Jetson Xavier NX edge computing module (NVIDIA Corporation, Santa Clara, CA, USA), which can provide powerful parallel computing capabilities and real-time data processing performance. This configuration meets the computational requirements of feature extraction, point cloud registration, and multi-sensor fusion in SLAM algorithms. The JueYing Mini quadruped robot has a total weight of 20 kg, with a ground contact pressure of 0.06 MPa (calculated based on the contact area of four feet, each with a contact area of 80 cm²). This parameter is critical for agricultural scenarios, as excessive soil compaction may affect crop root growth. This contact pressure is far lower than the critical pressure for soil compaction (typically 0.2–0.3 MPa for agricultural soil [31]). The robot has a standing dimension of approximately 700 mm × 400 mm × 500 mm, featuring a compact mechanical structure. It supports various gaits such as diagonal walking, trotting, and jumping, and possesses excellent dynamic balance performance and all-terrain adaptability. These characteristics enable the robot to navigate flexibly through corn rows with a spacing of 0.8–1.2 m, as well as handle common terrain undulations and small obstacles in the field.

In terms of sensor configuration, the robot is standardly equipped with a RS-LiDAR-16 (with a horizontal field of view of 360°, vertical field of view of −15°~+15°, ranging distance of 0.1~100 m, and point cloud frequency of 10 Hz) and absolute encoders for 12 driving joints, which can collect in real time 3D environmental point clouds, joint angles, and output torque data. To improve the accuracy of motion state perception and the capability of positioning ground truth verification, an MTi-710 high-precision IMU (Xsens Technologies B.V., Enschede, Overijssel, The Netherlands) and a Lite RTK module (Qianxun Spatial Intelligence Inc., Shanghai, China)are additionally configured. Specifically, the MTi-710 IMU achieves a pitch and roll accuracy of 0.2°, a heading accuracy of 1°, a gyroscope bias stability of 10°/h, and an accelerometer bias stability of 15 μg, with a maximum output frequency of 2 kHz, enabling precise capture of the robot’s dynamic motion states. The Lite RTK module supports signals from five major satellite systems (Beidou, GPS, GLONASS, GALILEO, and QZSS) and incorporates a u-blox ZED-F9P centimeter-level positioning module. It delivers an RTK positioning accuracy of ±(1 cm + 1 ppm) in the horizontal direction and ±(2 cm + 1 ppm) in the vertical direction, with an update rate of 10 Hz, capable of providing centimeter-level positioning ground truth. As shown in Figure 1, the experimental platform integrates the above-mentioned core components.

All sensor data are synchronously transmitted via the EtherCAT high-speed bus (Beckhoff Automation GmbH & Co. KG, Verl, North Rhine—Westphalia, Germany) with a synchronization accuracy of ≤1 ms, and data routing, management, and fusion processing are completed based on the Robot Operating System (ROS Noetic). This provides a stable and reliable hardware support for the testing and verification of field SLAM algorithms.

2.2. System Overview

The overall system framework is shown in Figure 2. The system fuses data from 3D LiDAR, IMU, joint encoders, and joint torque sensors. The InEKF module first fuses motion information from the IMU, joint encoders, and contact detection results based on joint torque, completing the prediction and iterative update of the robot’s motion state, outputting a high-frequency motion estimate with controlled drift. Subsequently, the 3D LiDAR data undergoes preprocessing based on this motion estimate for motion distortion compensation, eliminating point cloud distortion induced by robot motion. The distortion-compensated point cloud data further extracts three types of features: line features, plane features, and crop plane features. The LiDAR odometry uses these features with a scan-to-map matching method, aligns current scan features with an incrementally built local submap, and fuses the motion state output from InEKF to calculate the LiDAR odometry. The system simultaneously maintains a robot-centric incremental map, which is stored using an incremental kd-tree (ikd-tree). This map supports dynamic addition and deletion of point clouds and manages keyframes by defining a robot-centric circular search region. Finally, the backend uses a factor graph optimization framework to fuse the leg odometry factor from the InEKF motion estimate and the LiDAR odometry factor from scan-to-map alignment. Through Maximum a Posteriori (MAP) estimation, it minimizes the residuals of each factor, achieving global optimization of the robot trajectory and map geometry.

2.3. Quadruped Robot State Estimation Based on InEKF

2.3.1. State Equation

During the motion of a quadruped robot, due to frequent foot-ground impacts on the ground, the accelerometer data acquired by the IMU is unstable. This instability leads to significant drift when IMU measurements are directly used for state estimation, especially noticeable in the z-axis (vertical) direction. On the other hand, due to the uneven terrain of cornfields, their soft soil structure and irregular surface can cause foot slippage or sudden contact state changes, further exacerbating IMU measurement errors. Therefore, this study adopts the InEKF method proposed in previous work, filtering IMU data by fusing motion information from the IMU, joint encoders, and contact detection results based on joint torque. This method models foot slippage as a bias term in the body velocity and incorporates it into the state model, while dynamically adjusting the observation noise covariance using a foot contact probability model, providing a relatively accurate initial pose and velocity reference for the system.

We define the state vector of a quadruped robot in the world coordinate system $\left\{W\right\}$ on a group:

$${}_W\mathbf{X}_t=\left[\begin{array}{cc}{}_W\mathbf{R}_t& \begin{array}{ccccccc}{}_W\mathbf{v}_t& {}_W\mathbf{p}_t& {}_W\mathbf{p}_t^{{\mathrm{f}}_1}& {}_W\mathbf{p}_t^{{\mathrm{f}}_2}& {}_W\mathbf{p}_t^{{\mathrm{f}}_3}& {}_W\mathbf{p}_t^{{\mathrm{f}}_4}& {}_W\mathbf{b}_t^v\end{array}\\ {\mathbf{0}}_{7,3}& {\mathbf{I}}_7\end{array}\right]$$

(1)

where ${}_W\mathbf{R}_t$ represents the body orientation, ${}_W\mathbf{p}_t$ denotes the fuselage position, ${}_W\mathbf{v}_t$ indicates the fuselage velocity, while ${}_W\mathbf{p}_t^{{\mathrm{f}}_1}$, ${}_W\mathbf{p}_t^{{\mathrm{f}}_2}$, ${}_W\mathbf{p}_t^{{\mathrm{f}}_3}$, ${}_W\mathbf{p}_t^{{\mathrm{f}}_4}$ correspond to the positions of the four foot ends (front-left, front-right, rear-left, rear-right respectively). ${}_W\mathbf{b}_t^{\mathrm{v}}$ signifies the slip velocity caused by unstable contact or sliding at the foot end, modeled as an exponentially decaying process with decay rate $\alpha$ [32]:

$${}_W\dot{\mathbf{b}}_t^{\mathrm{v}}=-\alpha {}_W\mathbf{b}_t^{\mathrm{v}}+{\mathbf{n}}_t^{bv}, {\mathbf{n}}_t^{bv}\sim \mathcal{N}\left(0_{3\times 1},{\mathrm{\Sigma }}^{{\mathrm{b}}_t^v}\right)$$

(2)

Neglecting IMU biases (i.e., gyroscope and accelerometer biases), the dynamic model of the system is given by:

$$\begin{array}{l}{}_W\dot{\mathbf{R}}_t={}_W\mathbf{R}_t{\left[{\tilde{\mathbf{w}}}_t-{\mathbf{n}}_t^w\right]}_{\times },{}_W\dot{\mathbf{v}}_t={}_W\mathbf{R}_t\left({\tilde{\mathbf{a}}}_t-{\mathbf{n}}_t^a\right)+\mathbf{g}\\ {}_W\dot{\mathbf{p}}_t={}_W\mathbf{v}_t,{}_W\dot{\mathbf{p}}_t^{{\mathrm{f}}_i}={}_W\mathbf{R}_t{h}_{\mathit{R}}({\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_i})(-{\mathbf{n}}_t^{{\mathrm{f}}_i}),i\in \{1,\dots ,4\}\end{array}$$

(3)

Among them, ${\tilde{\mathbf{w}}}_t$ and ${\tilde{\mathbf{a}}}_t$ are the angular velocity and acceleration measured by the IMU, respectively. ${\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_i}$ denotes the measured joint angles of the $i$-th leg, acquired from joint encoders. $\mathbf{g}$ is the gravity vector, ${\mathbf{n}}_t^w$, ${\mathbf{n}}_t^a$, ${\mathbf{n}}_t^{{\mathrm{f}}_i}$ are Gaussian noise terms, ${\left[\cdot \right]}_{\times }$ denotes the skew-symmetric matrix of vector, and $h_R({\tilde{\mathbf{\theta }}}_t^{f_i})$ is derived from forward kinematics, representing the measured orientation of the contact frame relative to the IMU frame. Rewritten in matrix form:

$${\displaystyle \frac{d}{dt}}{}_W\mathbf{X}_t=\left[\begin{array}{cc}{}_W\mathbf{R}_t{({\tilde{\mathbf{w}}}_t)}_{\times }& \begin{array}{ccccccc}{}_W\mathbf{R}_t{\tilde{\mathbf{a}}}_t+\mathbf{g}& {}_W\mathbf{v}_t& {\mathbf{0}}_{3,1}& {\mathbf{0}}_{3,1}& {\mathbf{0}}_{3,1}& {\mathbf{0}}_{3,1}& -\alpha {}_W\mathbf{b}_t^{\mathit{v}}\end{array}\\ {\mathbf{0}}_{7,3}& {\mathbf{0}}_{7,7}\end{array}\right]$$

(4)

where

$${\mathbf{n}}_t\triangleq \mathrm{vec}\left({\mathbf{n}}_t^w,{\mathbf{n}}_t^a,\mathbf{0},h_R({\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_1}){\mathbf{n}}_t^{{\mathrm{f}}_1},h_R({\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_2}){\mathbf{n}}_t^{{\mathrm{f}}_2},h_R({\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_3}){\mathbf{n}}_t^{{\mathrm{f}}_3},h_R({\tilde{\mathbf{\theta }}}_t^{{\mathrm{f}}_4}){\mathbf{n}}_t^{{\mathrm{f}}_4},{\mathbf{n}}_{bv}(\mathit{t})\right)$$

(5)

${\left(\cdot \right)}^{\wedge }\mathbf{:}{ℝ}^{\dim \mathfrak{g}}\to \mathfrak{g}$ is an isomorphism that maps elements of the tangent space of the Lie group $\mathcal{G}$ at the identity element to their corresponding matrix representations. In the InEKF, the left-invariant errors ${}_W\mathbf{\eta }_t^l={}_W\mathbf{X}_t^{-1} {}_W\widehat{\mathbf{X}}_t$ and right-invariant errors ${}_W\mathbf{\eta }_t^r={}_W\widehat{\mathbf{X}}_t {}_W\mathbf{X}_t^{-1}$ are used to define the error between the estimated and true values, where ${}_W\widehat{\mathbf{X}}_t$ denotes the estimated state of the system at time $t$. This study employs right-invariant error derivation:

$${\displaystyle \frac{d}{dt}}{}_W\mathbf{\eta }_t^r={}_W\widehat{\mathbf{X}}_t {}_W\mathbf{X}_t^{-1}=\left[\begin{array}{cc}{}_W\mathbf{\eta }_{R_t}& \begin{array}{ccccccc}{}_W\xi _{v_t}& {}_W\xi _{p_t}& {}_W\xi _{p_t^{{\mathrm{f}}_1}}& {}_W\xi _{p_t^{{\mathrm{f}}_2}}& {}_W\xi _{p_t^{{\mathrm{f}}_3}}& {}_W\xi _{p_t^{{\mathrm{f}}_4}}& {}_W\xi _{b_t^v}\end{array}\\ {\mathbf{0}}_{7,3}& {\mathbf{I}}_7\end{array}\right]$$

(6)

where

$$\begin{array}{l}{}_W\mathbf{\eta }_{{\mathrm{R}}_t}={}_W\widehat{\mathbf{R}}_t {}_W\mathbf{R}_t^{\mathrm{T}},{}_W\xi _{{\mathrm{v}}_t}={}_W\widehat{\mathbf{v}}_t-{}_W\widehat{\mathbf{R}}_t {}_W\mathbf{R}_t^{\mathrm{{\rm T}}} {}_W\mathbf{v}_t\\ {}_W\xi _{{\mathrm{p}}_t}={}_W\widehat{\mathbf{p}}_t-{}_W\widehat{\mathbf{R}}_t {}_W\mathbf{R}_t^{\mathrm{{\rm T}}} {}_W\mathbf{p}_t,{}_W\xi _{{\mathrm{b}}_t^v}={}_W\widehat{b}_t^v-{}_W\widehat{\mathbf{R}}_t {}_W\mathbf{R}_t^{\mathrm{{\rm T}}} {}_{W}b_t^v\\ {}_W\xi _{{\mathrm{p}}_t^{{\mathrm{f}}_i}}={}_W\widehat{\mathbf{p}}_t^{f_i}-{}_W\widehat{\mathbf{R}}_t {}_W\mathbf{R}_t^{\mathrm{{\rm T}}}{}_W\mathbf{p}_t^{{\mathrm{f}}_i},i\in \{1,\dots ,4\}\end{array}$$

(7)

Let the Lie algebra corresponding to ${}_W\eta _{R_t}$ be ${}_W\xi _{R_t}$, i.e., ${}_W\eta _{R_t}=\mathrm{Exp}({}_W\xi _{R_t})$, where $\mathrm{Exp}(\cdot )$ denotes the exponential map, which maps an element of the Lie algebra $\mathfrak{s}\mathfrak{e}\left(3\right)$ to the corresponding element of the Lie group $\mathrm{SE}\left(3\right)$ and an element of $\mathfrak{s}\mathfrak{o}\left(3\right)$ to the corresponding element of $\mathrm{SO}\left(3\right)$. Using the first-order Taylor approximation of the exponential function, ${}_W\eta _{R_t}\approx I+{\left[{}_W\xi _{R_t}\right]}_{\times }$, we obtain:

$${\displaystyle \frac{d}{dt}}{}_W\dot{\eta }_t^r\approx \left[\begin{array}{cc}{}_W\dot{\xi }_{R_t}& \begin{array}{ccccccc}{}_W\dot{\xi }_{v_t}& {}_W\dot{\xi }_{p_t}& {}_W\dot{\xi }_{p_t^{f_1}}& {}_W\dot{\xi }_{p_t^{f_2}}& {}_W\dot{\xi }_{p_t^{f_3}}& {}_W\dot{\xi }_{p_t^{f_4}}& {}_W\dot{\xi }_{b_t^v}\end{array}\\ 0_{7,3}& 0_{7,7}\end{array}\right]={\displaystyle \frac{d}{dt}}{\left({}_W\xi _t\right)}^{\wedge }$$

(8)

Subsequently, the time derivative of the error state vector is derived as:

$${}_W\dot{\xi }_t=F_t{}_W\xi _t+{\mathrm{Ad}}_{{}_W\widehat{X}_t}{n}_t$$

(9)

where $F_t$ is the state transition matrix:

$$F_t=\left[\begin{array}{cccc}{0}_{3,3}& 0_{3,3}& 0_{3,15}& 0_{3,3}\\ {[g]}_{\times }& 0_{3,3}& 0_{3,15}& 0_{3,3}\\ 0_{3,3}& I_3& 0_{3,15}& 0_{3,3}\\ 0_{12,3}& 0_{12,3}& 0_{12,15}& 0_{3,3}\\ 0_{3,3}& 0_{3,3}& 0_{3,3}& -\alpha I_3\end{array}\right]$$

(10)

and ${\mathrm{Ad}}_{{\widehat{x}}_t}$ denotes the group adjoint map [33]. The covariance matrix $P_t$ is solved via the Riccati equation:

$$\begin{array}{c}{\displaystyle \frac{d}{dt}}{P}_t=F_t{P}_t+P_t{F}_t^{\mathrm{{\rm T}}}+{\overline{Q}}_t\\ {\overline{Q}}_t={\mathrm{Ad}}_{\widehat{X}}\mathrm{Cov}(n_t){\mathrm{Ad}}_{\widehat{X}}\end{array}$$

(11)

2.3.2. Observation Equation

Based on the forward kinematics of the quadruped robot, the position of each foot relative to the body frame $\left\{B\right\}$ can be obtained as:

$${}_B\mathbf{p}_t^{{\mathrm{f}}_i}=h({\tilde{\theta }}_t^{{\mathrm{f}}_i})={\mathbf{R}}_t^{\mathrm{{\rm T}}}\left({}_W\mathbf{p}_t^{{\mathrm{f}}_i}-{}_W\mathbf{p}_t\right)+n_t^{{\mathrm{FP}}_i}$$

(12)

This equation describes the relationship between the foot-end position relative to the body frame (derived from joint angles) and the global foot-end position, with $n_t^{{\mathrm{FP}}_i}$ representing the measurement noise of the foot-end position. The body velocity observation is given by

$${}_B\mathbf{v}_t={\displaystyle \frac{{\displaystyle {\sum }_{i\in C}p\left(S_i=1 | f_t^i\right){}_B\mathbf{v}_t^{{\mathrm{f}}_i}}}{{\displaystyle {\sum }_{i\in C}p\left(S_i=1 | f_t^i\right)}}}$$

(13)

where $P\left(S_i=1 | f_t^i\right)={\displaystyle \frac{1}{1+\exp \left(-{\beta }_1{f}_{z,i}^t-{\beta }_2\right)}}$ represents the probability of stable contact between foot $i$ and the ground at time $t$. $C$ denotes the set of feet in potential contact with the ground. $S_i=1$ indicates that the i-th foot is in stable contact with the ground. ${\mathbf{f}}_t^i$ is the ground reaction force acting on foot $i$ at time $t$. $f_{z,i}^t$ is the z-axis component of ${\mathbf{f}}_t^i$. ${\beta }_1$ and ${\beta }_2$ can be regarded as the weights of a logistic regression classifier. ${}_B\mathbf{v}_t$ is the fuselage velocity in the body frame and ${}_B\mathbf{v}_t^{{\mathrm{f}}_i}$ is the velocity of foot $i$ in the body frame at time $t$:

$${}_B\mathbf{v}_t^{{\mathrm{f}}_i}=-\left(J{\dot{\tilde{\theta }}}_t^{{\mathrm{f}}_i}+{}_B\omega \times h({\tilde{\theta }}_t^{{\mathrm{f}}_i})\right)+n_t^{{\mathrm{FV}}_i}$$

(14)

where $J$ denotes the Jacobian matrix of the forward kinematics for the $i$-th leg, ${}_B\omega$ is the angular velocity of the robot in body frame. To align the body velocity with the world coordinate system and account for slip effects, we transform the foot-end velocity from the body frame to the world frame:

$${}_W\mathbf{v}_t={}_W\mathbf{R}_t{}_B\mathbf{v}_t+{}_{W}b_t^v+n_t^v$$

(15)

where $n_t^v$ is the body velocity error, including errors from each foot velocity observation. The final observation model is expressed as:

$$\left[\begin{array}{c}{z}_t^{p_{{\mathrm{f}}_1}}\\ z_t^{p_{{\mathrm{f}}_2}}\\ z_t^{p_{{\mathrm{f}}_3}}\\ \begin{array}{l}{z}_t^{p_{{\mathrm{f}}_4}}\\ z_t^v\end{array}\end{array}\right]=\left[\begin{array}{cccccccc}{0}_{1,3}& 0& 1& -1& 0& 0& 0& 0\\ 0_{1,3}& 0& 1& 0& -1& 0& 0& 0\\ 0_{1,3}& 0& 1& 0& 0& -1& 0& 0\\ 0_{1,3}& 0& 1& 0& 0& 0& -1& 0\\ 0_{1,3}& 1& 0& 0& 0& 0& 0& 1\end{array}\right]{}_W\xi _t+\left[\begin{array}{c}{n}_t^{p_{{\mathrm{f}}_1}}\\ n_t^{p_{{\mathrm{f}}_2}}\\ n_t^{p_{{\mathrm{f}}_3}}\\ n_t^{p_{{\mathrm{f}}_4}}\\ n_t^v\end{array}\right]$$

(16)

$z_t^{p_{{\mathrm{f}}_i}}$ and $z_t^v$ are observation variables derived from the system’s right-invariant observation model, and their details are consistent with the framework proposed in [32]. All noise terms $n_t^{p_{{\mathrm{f}}_1}}$, …, $n_t^v$. Based on the definition of right-invariant errors. Yielding the observation equation in the form $z_t=C{\xi }_t+V_t$, the update process of the InEKF is implemented as follows:

$$\begin{array}{c}{K}_t=P_t{C}_t^{\mathrm{{\rm T}}}{\left(C_t{P}_t{C}_t^{\mathrm{{\rm T}}}+N_t\right)}^{-1}\hfill \\ {}_{W}X_t^{+}=\mathrm{Exp}\left(K_t{z}_t\right){}_W\widehat{X}_t\hfill \\ P_t^{+}=\left(I-K_t{C}_t\right)P_t\hfill \end{array}$$

(17)

2.4. Point Cloud Preprocessing

In the cornfield operation scenario, the quadruped robot’s body is prone to bumping, tilting, and other irregular movements during walking, coupled with posture mutations caused by foot slippage, which significantly increases the movement of the LiDAR during a scanning cycle, leading to geometric distortions like stretching and offset in the point cloud. Therefore, in the preprocessing stage, deskewing of the LiDAR point cloud is necessary. Due to the instability of IMU acceleration measurements on the quadruped robot, this study uses the leg odometry derived from InEKF for distortion compensation.

During a single scanning cycle, the LiDAR sequentially collects multiple laser points and forms a frame of point cloud data. Assuming the start time of the scanning cycle is $t_a$ and the end time is $t_b$, each $i$-th laser point carries a relative time offset $\delta t_i$ in the raw point cloud data, where $\delta t_i\in \left[0,t_b-t_a\right]$ representing the time interval from the start of the scan to the completion of data acquisition for that point. Thus, the absolute acquisition time of the $i$-th laser point is:

$$t_i=t_0+\delta t_i$$

(18)

The core of point cloud deskewing is to match each laser point with the precise robot pose at its acquisition moment, thereby eliminating motion interference through coordinate transformation. The Invariant Extended Kalman Filter (InEKF) outputs the robot’s motion states at a frequency of 200 Hz, with its time series $\left\{t_n\right\}$ covering the global timeline. For the acquisition moment $t_i$ of the $i$-th laser point, the adjacent state timestamps of InEKF are selected according to the timeline inclusion principle:

$$t_k=\max \left\{t\in \left\{t_n\right\} | t\le t_i\right\}, t_{k+1}=\min \left\{t\in \left\{t_n\right\} | t\ge t_i\right\}$$

(19)

Let the poses of the InEKF at times $t_k$ and $t_{k+1}$ be $\left({}_W\mathbf{R}_k,{}_W\mathbf{p}_k\right)$ and $\left({}_W\mathbf{R}_{k+1},{}_W\mathbf{p}_{k+1}\right)$, respectively, then the pose $\left({}_W\mathbf{R}_{t_i},{}_W\mathbf{p}_{t_i}\right)$ corresponding to the $i$-th laser point’s acquisition moment is calculated as follows:

$$\left\{\begin{array}{l}{}_W\mathbf{R}_{t_i}=\mathrm{SLERP}\left({}_W\mathbf{R}_k,{}_W\mathbf{R}_{k+1},{\displaystyle \frac{t_i-t_k}{t_{k+1}-t_k}}\right)\\ {}_W\mathbf{p}_{t_i}=\mathrm{LERP}\left({}_W\mathbf{p}_k,{}_W\mathbf{p}_{k+1},{\displaystyle \frac{t_i-t_k}{t_{k+1}-t_k}}\right)\end{array}\right.$$

(20)

Herein, SLERP denotes Spherical Linear Interpolation, and LERP denotes Linear Interpolation. Given the extrinsic parameters between the LiDAR and IMU as $\left({\mathbf{R}}_{BL},{\mathbf{p}}_{BL}\right)$, the coordinates of the $i$-th laser point in the world coordinate system after deskewing are:

$${}_{W}l_i={}_W\mathbf{R}_{t_i}\left({\mathbf{R}}_{BL} {}_{L}l_i+{\mathbf{p}}_{BL}\right)+{}_W\mathbf{p}_{t_i}$$

(21)

Here ${}_{W}l_i$ denotes the raw coordinate vector of the $i$-th laser point in the LiDAR coordinate system $\left\{L\right\}$, and ${}_{W}l_i$ denotes the coordinate vector of the deskewed $i$-th laser point in the world coordinate system $\left\{W\right\}$.

2.5. Feature Extraction

The feature extraction module serves as a crucial hub linking system perception, pose estimation, and map construction. Using the deskewed 3D LiDAR data, the module first separates the ground point cloud from the non-ground point cloud by leveraging the ground segmentation method from LeGO-LOAM. Specifically, after projecting the point cloud into a range image, the angular and distance relationships of neighboring points are analyzed column-by-column. Points that conform to the geometric characteristics of the ground are selected to constitute the ground point cloud, and the remaining point set constitutes the non-ground point cloud. Furthermore, a three-category feature system tailored to the complex cornfield environment is established based on different point cloud scopes. On one hand, it directly inherits the core framework for feature extraction of LeGO-LOAM, extracting planar features from the ground point cloud and line features from the non-ground point cloud, thereby fully utilizing its advantages of being lightweight and highly robust. On the other hand, an innovative crop planar feature extraction method is designed according to the characteristics of the farmland scenario, realizing accurate perception of crop structures in complex farmland environments. These three types of features collectively provide comprehensive constraints for subsequent pose optimization and map construction.

2.5.1. Line Feature Extraction

The line feature extraction fully adopts the method of LeGO-LOAM and is only carried out based on the non-ground point cloud to accurately capture the edge structures of corn plants. Specifically, the non-ground point cloud is divided into local neighborhoods. The edge characteristics are quantified by calculating the local smoothness of points (the normalized value of distance deviations among points within the neighborhood), where a higher smoothness indicates a more prominent edge in the region where the point is located. Subsequently, within multiple sub-regions divided in the 360° horizontal field of view, feature points are selected by sorting according to smoothness and subjected to Non-Maximum Suppression (NMS) to ensure the uniform spatial distribution of line features. The line features extracted by this method can accurately match the edges of the slender structures of plants, such as corn stalks and leaf contours.

2.5.2. Planar Feature Extraction

Planar feature extraction also adopts the method proposed in LeGO-LOAM, with features selected exclusively from the ground point cloud. The key process involves the following steps: First, the local smoothness of each point in the ground point cloud is calculated (quantified by the distance deviations among points within the neighborhood). A lower smoothness indicates that the region where the point is located is flatter and more consistent with planar characteristics. Subsequently, points are sorted by smoothness within horizontal sub-regions, and the less smooth points are selected as planar feature candidates. Finally, a set of ground planar features is formed by integrating the uniform sampling rule across sub-regions. This method screens points with flat and geometrically stable properties on the ground via local smoothness quantification. Meanwhile, uniform sampling in sub-regions ensures comprehensive coverage of features across the field surface. Thus, these planar features can stably reflect field undulations and furrow distributions. Beneath the corn canopy, they can effectively distinguish between the ground and plant-covered areas.

2.5.3. Crop Planar Feature Extraction

Corn plants grow in regular rows, forming continuous planar clusters along the row direction. These crop row planes exhibit stable geometric characteristics, which can serve as reliable scene constraints to make up for the lack of salient features under dense canopies. The crop plane feature extraction module follows a “coarse screening→precise positioning→plane fitting” logic: First, it screens out potential crop areas from the complex point cloud environment via grid filtering; then it precisely locates crop row positions through differential evolution optimization; finally, it fits crop plane features via the RANSAC algorithm to provide effective constraints for SLAM.

Coarse screening of potential crop areas: The non-ground point cloud is projected into a 2D grid space to establish a local coordinate system with the sensor position as the origin. Let the grid resolution be $r$ and the grid size be $L\times W$. The mapping relationship between grid coordinates and 3D spatial coordinates is established through the row index $i=⌊{\displaystyle \frac{y}{r}}+{\displaystyle \frac{L}{2}}⌋$ and column index $j=⌊{\displaystyle \frac{x}{r}}+{\displaystyle \frac{W}{2}}⌋$, where $\left(x,y\right)$ are the horizontal coordinates of the point cloud in the local coordinate system, and $⌊\cdot ⌋$ denotes the floor function. The number of points $N\left(i,j\right)$ in each grid is counted, and dense regions satisfying $N\left(i,j\right)\ge T$ are selected via thresholding as potential crop distribution regions. The entire process of coarse screening and the resulting potential crop areas are illustrated in Figure 3.

Precise positioning of crop rows: A differential evolution algorithm is adopted for crop row region detection. The crop row model is parameterized as $\left(\theta ,d,s\right)$, where $\theta$ is the crop row direction angle (angle with the $x$-axis), $d$ is the distance from the first crop row to the origin, and $s$ is the spacing between two adjacent crop rows. The objective function of the algorithm is defined as the weighted product of the total number of points in the two crop row regions and a balance factor:

$$f\left(\theta ,d,s\right)=\left(N_1+N_2\right)\cdot {\left({\displaystyle \frac{\min \left(N_1,N_2\right)}{\max \left(N_1,N_2\right)}}\right)}^w$$

(22)

where $N_1$ and $N_2$ are the total number of points in the two crop row regions, respectively, with $N_1={\displaystyle \sum _{\left(x,y\right)\in L_1}N\left(i,j\right)}$ and $N_2={\displaystyle \sum _{\left(x,y\right)\in L_2}N\left(i,j\right)}$, $w$ is the weight of the balance factor. $L_1$ and $L_2$, represent the region ranges of the two crop rows (with width $W_{\mathrm{row}}$), satisfying

$$\begin{array}{l}{L}_1:\left|x\cos \theta +y\sin \theta -d\right|\le {\displaystyle \frac{W_{\mathrm{row}}}{2}}\\ L_2:\left|x\cos \theta +y\sin \theta -\left(d+s\right)\right|\le {\displaystyle \frac{W_{\mathrm{row}}}{2}}\end{array}$$

(23)

The differential evolution algorithm generates an initial population within the parameter space $\theta \in \left[{\theta }_{\min },{\theta }_{\max }\right]$, $d\in \left[d_{\min },d_{\max }\right]$, $s\in \left[s_{\min },s_{\max }\right]$, and obtains the optimal parameters $\left({\theta }^{*},d^{*},s^{*}\right)$ after $K$ iterations through mutation, crossover, selection, and other operations.

Crop planar fitting and feature extraction: Crop planar fitting and feature extraction are performed. For the detected two crop rows, point clouds within a region of width $W_{\mathrm{plane}}$ around each crop row are extracted from the original non-ground point cloud as planar fitting candidates, where the distinction between crop lines and crop planes is illustrated in Figure 4. and the RANSAC algorithm is employed to fit the crop planes. During the fitting process, inliers are screened using a distance threshold $t_{\mathrm{fit}}$ and reliable plane models with the number of inliers exceeding $N_{\min }$ are retained to form the crop planar feature point set.

The module is designed with strong environmental adaptability, and its handling strategies for typical complex scenarios are as follows:

• Non-crop areas: The module judges whether valid crop rows are extracted based on two core criteria derived from the detected crop lines. First, it checks if the number of points crossing each fitted crop line is sufficient (meeting the preset requirement for effective crop row representation). Second, it verifies that the number of points between the two detected crop lines is not significantly different (ensuring balanced distribution of crop plants). When both criteria are satisfied, the system confirms the presence of crop rows and incorporates the extracted crop plane features into odometry calculation, using them to constrain the yaw, x, and y directions. If either criterion is not met, the module determines that no valid crop rows are present (i.e., the area is a non-crop region such as field ridges or paths) and disables crop plane feature extraction. In this case, the system relies solely on line and ground plane features for odometry calculation to maintain continuous navigation.
• Uneven crop row spacing: The cornfield in the experimental scenario is planted with a standard row spacing of 1 m. However, due to sowing equipment errors, terrain undulations, or manual operation deviations, the actual row spacing may deviate from the standard value. To adapt to this unevenness, the differential evolution algorithm in the module sets the search range of the row spacing parameter s to 0.8–1.2 m (covering the common deviation range of the 1 m standard row spacing). This design allows the algorithm to flexibly fit crop rows with non-uniform spacing within the reasonable deviation range.
• Weed interference: Weeds usually distribute sparsely and randomly, and their point cloud performance is significantly different from the dense continuous distribution of corn plants. The module uses a two-stage suppression strategy: in the grid preprocessing stage, sparse grids are directly removed to eliminate most scattered weed point clouds; in the plane fitting stage, for locally dense weeds, the crop row direction constraint is applied—only the planes whose normal vector direction is consistent with the overall crop row direction are retained, thereby further excluding weed interference and ensuring the purity of crop plane features.

2.6. Feature Matching

The feature matching method is carried out around the three types of features extracted from LiDAR (line features, planar features, and crop planar features).

For the matching of line features and planar features, the real-time pose provided by leg odometry (based on the InEKF) is used as the initial pose guess. An ikd-tree is employed to quickly search for the nearest neighbor features of the current frame’s line feature points and planar feature points in the local map, thereby constructing initial matching pairs.

The matching of crop planar features is based on their core geometric characteristics. The crop planar feature is defined as $\mathbb{F}=\left\{c,\mathbb{E},\pi \right\}$ consisting of the plane centroid $c$, boundary convex hull vertex set $\mathbb{E}$, and unit normal vector $\pi$. These three types of core features are extracted from the pre-fitted crop planar point cloud, comprehensively characterizing the spatial position, spatial scope, and spatial attitude of the plane. For a crop planar point set $\mathbb{P}=\left\{{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_n\right\}$(where ${\mathbf{p}}_i=(x_i,y_i,z_i)$), the centroid $c=\left(\overline{x},\overline{y},\overline{z}\right)$ is calculated as:

$$c=\left({\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{x}_i,}{\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{y}_i,}{\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{z}_i,}\right)$$

(24)

which is used to represent the spatial position of the crop plane. The boundary convex hull vertex set $\mathbb{E}=\left\{e_1,e_2,\dots ,e_m\right\}$ (where $m$ is the number of convex hull vertices) is obtained by projecting the crop planar point set onto a local plane with $\pi$ as the normal vector and then extracting the vertices of the minimum convex polygon, serving to define the spatial scope of the crop plane. The unit normal vector $\pi$ is solved via Principal Component Analysis (PCA): first, perform PCA on the crop planar point set $\mathbb{P}$ to construct the covariance matrix $S={\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n({\mathbf{p}}_i-\overline{\mathbf{p}})}{({\mathbf{p}}_i-\overline{\mathbf{p}})}^T$, then, conduct eigenvalue decomposition on $S$ the eigenvector corresponding to the smallest eigenvalue, after normalization, is the plane normal vector $\pi ={\left[{\pi }_1,{\pi }_2,{\pi }_3\right]}^{\mathrm{{\rm T}}}$, which characterizes the spatial attitude of the crop plane.

Under the constraints of these three core features, the crop plane is searched and matched in the local map. Based on the initial pose ${\xi }_{\mathrm{init}}$ provided by leg odometry, the current crop planar feature ${}^m\mathbb{F}_{cur}=\left\{{}^{m}c_{cur},{}^m\mathbb{E}_{cur},{}^m\pi _{cur}\right\}$. Subsequently, the historical plane object closest to ${\mathbb{F}}_{cur}$ is selected from the local map. Let the historical crop planar features stored in the local map be ${\mathcal{F}}_{hist}=\left\{{\mathbb{F}}_{hist,1},{\mathbb{F}}_{hist,2},\dots ,{\mathbb{F}}_{hist,k}\right\}$, where $k$ is the number of historical crop planes. For the j-th historical crop planar object ${\mathbb{F}}_{hist,j}=\left\{c_{hist,j},{\mathbb{E}}_{hist,j},{\pi }_{hist,j}\right\}$ in the local map, the proximity between ${\mathbb{F}}_{cur}$ and ${\mathbb{F}}_{hist,j}$ in three dimensions (spatial position, scope, and attitude) is quantified as follows:

$$\begin{array}{l}{\delta }_{c,j}={\displaystyle \frac{\left|{\pi }_{hist,j}\cdot \left({}^{m}c_{cur}-c_{hist,j}\right)\right|}{‖{\pi }_{hist,j}‖}}\\ {\delta }_{\mathbb{E},j}=1-{\displaystyle \frac{\mathrm{Area}\left({}^m\mathbb{E}_{cur}\cap {\mathbb{E}}_{hist,j}\right)}{\mathrm{Area}{}^m\mathbb{E}_{cur})}}\\ {\delta }_{\pi ,j}=\arccos \left(\left|{}^m\pi _{cur}\cdot {\pi }_{hist,j}\right|\right)\end{array}$$

(25)

where $\mathrm{Area}(\cdot )$ denotes the convex hull area calculation function, and $\cap$ denotes the intersection operation. To comprehensively evaluate the overall proximity, a weighted comprehensive proximity index is defined:

$${\Delta }_j=w_1{\delta }_{c,j}+w_2{\delta }_{\mathbb{E},j}+w_3{\delta }_{\pi ,j}$$

(26)

with the constraint $w_1+w_2+w_3=1$(where $w_1$, $w_2$, $w_3$ are weights). By iterating through all historical plane objects, the ${\mathbb{F}}_{hist,j}$ that satisfies ${\Delta }_j<{\Delta }_{th}$ and ${\Delta }_j=\min \left\{{\Delta }_1,{\Delta }_2,\dots ,{\Delta }_k\right\}$ is selected as the optimal matching object for the current crop plane.

2.7. LiDAR Odometry

The LiDAR odometry proposed in this study adopts three types of LiDAR-extracted features (ground planar features, line features, and crop planar features) as core constraints, and employs a two-step Levenberg–Marquardt (LM) iteration strategy for pose estimation. Ground planar features primarily constrain the roll angle, pitch angle, and $z$-axis (height) components of the pose, mitigating pose deviations in the vertical direction and attitude tilt. Non-ground features (including crop planar features and line features) focus on constraining the yaw angle, $x$-axis, and $y$-axis (horizontal plane) components, ensuring the accuracy of horizontal pose estimation in cornfield scenarios. By precisely associating features with optimization dimensions and dynamically adjusting priorities, the odometry adapts to pose estimation requirements in complex cornfield scenarios.

The ground planar feature residual is defined as the distance from the current frame’s ground planar feature points to the corresponding historical planes in the local map:

$${\mathrm{\Phi }}_g(q_{g,k},{\mathcal{P}}_{\mathrm{map}})=\left|{\pi }_{g,\mathrm{hist}}\cdot (q_{g,k}-c_{g,\mathrm{hist}})\right|$$

(27)

where $q_{g,k}$ is the $k$-th ground planar feature point in the current frame, ${\mathcal{P}}_{\mathrm{map}}$ denotes the local map, and ${\pi }_{g,\mathrm{hist}}$ and $c_{g,\mathrm{hist}}$ are the unit normal vector and centroid of the matched historical ground plane in the local map, respectively.

The crop planar feature residual adopts the same form as the ground planar feature residual, i.e., the distance from the current frame’s crop planar feature points to the corresponding historical crop planes in the local map:

$${\mathrm{\Phi }}_c(q_{c,k},{\mathcal{P}}_{\mathrm{map}})=\left|{\pi }_{c,\mathrm{hist}}\cdot (q_{c,k}-c_{c,\mathrm{hist}})\right|$$

(28)

where $q_{c,k}$ is the $k$-th crop planar feature point in the current frame, and ${\pi }_{c,\mathrm{hist}}$ and $c_{c,\mathrm{hist}}$ are the unit normal vector and centroid of the matched historical crop plane in the local map, respectively.

The line feature residual is defined as the distance from the current frame’s line feature points to the corresponding historical line segments in the local map:

$${\mathrm{\Phi }}_l(q_{l,k},{\mathcal{P}}_{\mathrm{map}})={\displaystyle \frac{‖(q_{l,k}-{\mathbf{p}}_{l,{\mathrm{hist}}_1})\times (q_{l,k}-{\mathbf{p}}_{l,{\mathrm{hist}}_2})‖}{‖{\mathbf{p}}_{l,{\mathrm{hist}}_1}-{\mathbf{p}}_{l,{\mathrm{hist}}_2}‖}}$$

(29)

where $q_{l,k}$ is the $k$-th line feature point in the current frame, ${\mathbf{p}}_{l,{\mathrm{hist}}_1}$, ${\mathbf{p}}_{l,{\mathrm{hist}}_2}$ are the two endpoints of the matched historical line segment in the local map. $\times$ denotes the vector cross product, and $‖\cdot ‖$ denotes the 2-norm of a vector.

Pose optimization takes the initial pose ${\xi }_{\mathrm{init}}$ provided by leg odometry as the iteration starting point and gradually converges to the optimal pose through two-step LM iterations. In the first step, focusing on ground planar feature residuals, a nonlinear least squares problem for the pose parameter $\xi$ is constructed to primarily correct deviations in the roll angle, pitch angle, and z-axis drift in the initial pose:

$${\min }_{\xi }{\displaystyle \sum _{k=1}^{N_g}{\mathrm{\Phi }}_g}(q_{g,k},{\mathcal{P}}_{\mathrm{map}})$$

(30)

where $N_g$ is the number of ground planar feature points. This optimization problem is solved via LM iterations, outputting the preliminarily optimized pose ${\xi }_1$.

In the second step, based on the pose output from the first step, the yaw angle, x-axis, and y-axis components are further optimized. The optimization strategy is dynamically adjusted according to the matching results of crop planar features. When crop planar features exist and are successfully matched, a joint optimization objective is constructed with crop planar feature residuals as the main constraint and line feature residuals as the auxiliary constraint. Weight coefficients satisfying ${\omega }_c>{\omega }_l>0$ are used to ensure the dominant constraint role of crop planes:

$${\min }_{\xi }{\omega }_c{\displaystyle \sum _{k=1}^{N_c}{\mathrm{\Phi }}_c^2}(q_{c,k},{\mathcal{P}}_{\mathrm{map}})+{\omega }_l{\displaystyle \sum _{k=1}^{N_l}{\mathrm{\Phi }}_l^2}(q_{l,k},{\mathcal{P}}_{\mathrm{map}})$$

(31)

When crop planar features are missing or matching fails, the optimization objective is constructed solely using line feature residuals to ensure the continuity of horizontal pose optimization:

$${\min }_{\xi }{\displaystyle \sum _{k=1}^{N_l}{\mathrm{\Phi }}_l^2}(q_{l,k},{\mathcal{P}}_{\mathrm{map}})$$

(32)

This problem is also solved via LM iterations, ultimately outputting the twice-optimized pose ${\xi }_2$, i.e., the pose of the current frame’s LiDAR odometry.

After the LiDAR odometry solves the current frame’s pose, a keyframe selection mechanism is used to filter valid frames to support local map maintenance and subsequent optimization. The core logic of keyframe selection revolves around two dimensions: “motion significance” and “environmental information novelty.”

First, the pose deviation between the current frame and the previous keyframe is calculated. Motion significance is determined if the translation deviation $\Delta d>{ϵ}_d$ or the rotation deviation $\Delta \theta >{ϵ}_{\theta }$ (where ${ϵ}_d$, ${ϵ}_{\theta }$ are preset thresholds). Second, the overlap $\gamma$ between the current frame’s point cloud and historical keyframe point clouds is calculated based on the local map. Environmental information novelty is determined if $\gamma <{ϵ}_{\gamma }$ (where ${ϵ}_{\gamma }$ is the overlap threshold). The current frame is marked as a valid keyframe only if both conditions are satisfied.

2.8. Optimization

In the backend fusion stage, factor graph optimization is adopted to achieve tight coupling between legged odometry and LiDAR odometry. This framework takes the robot states corresponding to LiDAR keyframes as nodes, where each node is defined by the attitude rotation matrix ${}_W\mathbf{R}$ and position vector ${}_W\mathbf{p}$ in the world coordinate system. The residual of the legged odometry factor $r_{K_{ij}}$ is defined as the deviation between the predicted pose transformation $T_{X_j}{T}_{X_i}^{-1}$ calculated based on the states of nodes $i$, $j$, and the measured pose transformation ${\mathcal{K}}_{ij}$ from the legged odometry. The residual of the LiDAR odometry factor $r_{L_{ij}}$ is defined as the deviation between the predicted pose transformation $T_{X_j}{T}_{X_i}^{-1}$ and the measured pose transformation ${\mathcal{L}}_{ij}$ from the LiDAR odometry. The initial state residual $r_0$ corresponds to the deviation of the prior state obtained through static initialization when the robot starts. For covariance matrix adjustment: the covariance matrix ${\Sigma }_{{\mathcal{K}}_{ij}}$ of the legged odometry factor is determined by the posterior covariance matrix of the InEKF. The covariance matrix ${\Sigma }_{L_{ij}}$ of the LiDAR odometry factor is determined by the statistical characteristics of the point cloud matching residuals. Subsequently, the objective function is constructed based on the MAP criterion:

$${\mathcal{X}}_k^{*}=\underset{{\mathcal{X}}_k}{argmin}{‖r_0‖}_{{\Sigma }_0}^2+{\displaystyle \sum _{(i,j)\in \mathcal{K}}{‖r_{{\mathcal{K}}_{ij}}‖}_{{\Sigma }_{{\mathcal{K}}_{ij}}}^2}+{\displaystyle \sum _{(i,j)\in \mathcal{L}}{‖r_{{\mathcal{L}}_{ij}}‖}_{{\Sigma }_{{\mathcal{L}}_{ij}}}^2}$$

(33)

2.9. Mapping

The mapping module involves the maintenance of local and global maps. The local map adopts a maintenance mechanism centered on the robot’s current pose, with its core data derived from valid keyframes within the region. Combined with ikd-tree technology, efficient addition, deletion, and retrieval of feature point clouds are achieved. Line features and planar features are stored in two independent ikd-trees, ${\mathrm{ikd}}_l$ stores line feature points ${\mathcal{P}}_l=\left\{q_{l,k}\in {\mathit{R}}^3 | k=1,2,\dots ,N_l\right\}$ and ${\mathrm{ikd}}_{\mathit{p}}$ stores planar feature ${\mathcal{P}}_{\mathit{p}}=\left\{q_{\mathit{p},k}\in {\mathit{R}}^3 | k=1,2,\dots ,N_{\mathit{p}}\right\}$. For crop planar feature storage, a collaborative mode of ikd-tree for inliers and hash table for geometric parameters is adopted, ${\mathrm{ikd}}_c$ stores the crop planar inlier set ${\mathcal{P}}_c=\left\{q_{c,k}\in {\mathit{R}}^3 | k=1,2,\dots ,N_c\right\}$, while core geometric parameters (centroid $c$ boundary convex hull vertex set $\mathbb{E}=\left\{e_1,e_2,\dots ,e_m\right\}$ and unit normal vector $\pi$) are stored in a hash table ${\mathcal{I}}_c$. The index key is the unique identifier of the crop plane, enabling rapid association between inlier retrieval and parameter invocation.

The construction of the global map is based on the keyframe poses fused with LiDAR odometry and legged odometry constraints after factor graph optimization, achieving cross-local-region feature integration and global consistent representation. Specifically, downsampled line feature points, planar feature points, and crop planar feature points are extracted from the valid keyframes output by factor graph optimization. These feature points are uniformly transformed from the local coordinate system to the world coordinate system according to the optimized poses of the keyframes. For line features and planar features, a global ikd-tree is used for storage and management, and duplicate redundant points are eliminated through nearest-neighbor retrieval to realize cross-keyframe feature fusion. The crop planar features adopt a storage mode consisting of inliers and their corresponding geometric parameters: after coordinate transformation, the inliers are stored in the global crop feature ikd-tree, while their geometric parameters are synchronously updated to the global hash table to maintain the efficiency of feature retrieval and parameter invocation.

3. Experiments and Results Analysis

3.1. Experimental Scenarios and Data Collection

The experimental site is located in an experimental field in Santai County, Mianyang City, Sichuan Province, as shown in Figure 5. The terrain is generally flat but characterized by complex microtopography: small drainage ditches, shallow gullies, and field ridges are widely distributed, resulting in local terrain undulations. All experiments were conducted under low-wind conditions: during the early and late jointing stages (corn height < 220 cm), the sparse-to-moderate canopy led to slight leaf sway (amplitude < 5 cm) and a small number of dynamic points in the LiDAR point cloud; in the tasseling and milk ripening stages, the closed canopy effectively reduced wind penetration, resulting in minimized leaf motion (amplitude < 2 cm) and stable crop structural features. The experiments were performed under relatively dry soil conditions. Based on field observations throughout the tests, the robot’s feet exhibited no sinking—even after light rainfall—though occasional slippage was observed. This slippage was attributed to slight soil moisture retention following light precipitation, without any signs of muddiness or deep adhesion that would impede navigation. Corresponding to these soil conditions, soil moisture content is estimated to range from 10% to 23%.

To cover environmental differences throughout the corn growth cycle, tests were conducted across 4 key growth stages. The corn morphology and field environmental characteristics at each stage are summarized in

Table 1. Morphological and Field Environmental Characteristics of Corn at Four Key Growth Stages in the Experimental Field of Santai County, Mianyang City, Sichuan Province.

Growth Stage	Date	Plant Height Range (cm)	Canopy Density	Field Environmental Characteristics	Test Trajectory Parameters
Early Jointing	4 May 2025	80–120	Sparse	Slender, soft stems; small, scattered leaves; wide inter-row visibility; bare ground (no obstruction); distinct corn row boundaries.	4 corn rows (round trip), total length 130 m
Late Jointing	20 May 2025	180–220	Moderate	Thick, upright stems; increased leaf expansion; top leaves start overlapping; large inter-row space.	4 corn rows (round trip), total length 50 m
Tasseling Stage	2 June 2025	200–230	Dense	Tassels emerged; fully expanded leaves; canopy closing; inter-row channels visible only in lower section.	4 corn rows (round trip), total length 130 m
Milk Ripening Stage	17 July 2025	220–250	Dense	Fully closed canopy; severely overlapping leaves; drooping corn ears; inter-row channels almost fully blocked.	4 corn rows (round trip), total length 130 m

.

The test trajectory for the early jointing, tasseling, and milk ripening stages was set to a 130 m round trip across 4 corn rows, following the same pre-defined path. However, due to inherent farmland microtopographic variations and occasional minor foot slippage induced by soil moisture, the RTK ground truth trajectories of repeated tests for these stages cannot be guaranteed to be completely identical. The test trajectory length for the late jointing stage was specifically designed as 50 m (shorter than the 130 m used for other stages) based on prior preliminary tests. These preliminary evaluations revealed that mainstream IMU-fused SLAM algorithms (i.e., LeGO-LOAM and Point-LIO) exhibited significant positioning drift and even loss of localization in long-distance cornfield scenarios (≥100 m) due to gait-induced IMU jitter and cumulative integration errors. In contrast, the late jointing stage features moderate canopy density and relatively stable environmental structure, which enables these algorithms to maintain valid localization results within a short trajectory (50 m).

3.2. Comparative Algorithms

To verify the superiority of CK-SLAM, mainstream methods covering different technical routes are selected as baselines to ensure the comprehensiveness and fairness of the comparison. All baselines are tested under the same parameter settings, hardware platform, and data input conditions to eliminate artificial biases. Priority is given to algorithms that can stably output valid results in the experiments, and three representative schemes are finally determined. A-LOAM—A classic LiDAR Odometry and Mapping (LOAM) algorithm. It relies on edge and planar features for point cloud registration without IMU fusion, representing a pure LiDAR SLAM scheme without inertial assistance, with parameters consistent with its open-source default configuration. LeGO-LOAM—A feature-based LiDAR SLAM scheme fused with IMU. IMU data is integrated in a loose coupling manner, mainly used for motion distortion compensation and initial pose guess for registration, representing a scene-adaptive feature extraction scheme with IMU assistance, adopting the same IMU data and parameter settings as CK-SLAM. Point-LIO—A tightly coupled LiDAR SLAM scheme fused with IMU. It does not rely on traditional edge or planar feature division but deeply fuses raw LiDAR points with IMU pre-integration data, representing an explicit feature-independent scheme under IMU tight coupling, with parameters kept consistent with its open-source default configuration and using the same sensor data as CK-SLAM.

3.3. Evaluation Metric Design

Evaluation metrics are designed from two dimensions—localization accuracy and real-time performance—combining quantitative and qualitative analyses.

3.3.1. Localization Accuracy Metrics

Two metrics are adopted: Absolute Pose Error (APE) and Relative Pose Error (RPE). APE evaluates global pose accuracy, defined as the direct deviation between the pose estimated by the algorithm and the ground truth pose. RPE assesses local pose accuracy, referring to the deviation of pose changes between adjacent moments (or at intervals of). Both metrics are typically presented in the form of Root Mean Square Error (RMSE), with the calculation formulas as follows:

$$\begin{array}{l}{\mathrm{APE}}_{\mathrm{RMSE}}=\sqrt{{\displaystyle \frac{1}{N}}{\displaystyle \sum _{i=1}^N{‖e_i‖}_2^2}}\\ {\mathrm{RPE}}_{\mathrm{RMSE}}=\sqrt{{\displaystyle \frac{1}{N-\Delta t}}{\displaystyle \sum _{i=1}^{N-\Delta t}{‖e_i^{\Delta t}‖}_2^2}}\end{array}$$

(34)

where $e_i$ is the error between the estimated pose $T_{est,i}$ and the ground truth pose $T_{gt,i}$ at the $i$-th moment, and $e_i^{\Delta t}$ is the deviation of pose changes within the adjacent time interval $\Delta t$.

3.3.2. Real-Time Performance Metrics

To ensure comprehensive and reliable real-time evaluation, statistical characteristics of each algorithm running on the quadruped robot are recorded, including average single-frame runtime, minimum runtime, and maximum runtime. These metrics comprehensively reflect the algorithm’s overall processing efficiency, time redundancy capability in complex scenarios, and optimal response speed under ideal working conditions.

3.4. Experimental Results and Analysis

Figure 6 presents the top-down views of the trajectories of each algorithm across the four corn growth stages, where the black dashed line represents the RTK ground truth trajectory. It should be noted that the start and end points of LeGO-LOAM and Point-LIO trajectories in Figure 6 are inconsistent with the ground truth and other algorithms. This phenomenon arises from the trajectory alignment setting adopted in the evo trajectory evaluation tool: to eliminate the influence of initial pose offsets between different algorithms and facilitate quantitative comparison of trajectory shape consistency and drift characteristics, each algorithm’s trajectory was aligned with the RTK ground truth trajectory using a rigid-body transformation (including translation and rotation). During this alignment process, the relative positional relationship of each trajectory’s internal points is preserved, while the absolute start and end coordinates are adjusted to minimize the overall deviation from the ground truth. This alignment operation does not affect the validity of the experimental results. On one hand, the core evaluation metrics (APE RMSE and RPE RMSE) are calculated based on the deviation between the aligned trajectories and the ground truth, which can objectively reflect the localization accuracy of each algorithm. On the other hand, the relative drift trend, shape consistency with the ground truth, and algorithmic stability displayed by each trajectory (e.g., whether LeGO-LOAM maintains a stable path or Point-LIO produces chaotic trajectories) remain unchanged after alignment, ensuring the reliability of qualitative analysis.

Both CK-SLAM and A-LOAM demonstrate favorable localization performance throughout all growth stages. However, during the tasseling and milk ripening stages, although A-LOAM can maintain the trajectory shape, a significant deviation from the ground truth trajectory is observed. The advantages of CK-SLAM gradually become prominent, which stems from its integrated crop row extraction and crop planar feature constraint modules. As corn stems thicken and plant height increases, CK-SLAM can more easily extract crop line information. LeGO-LOAM performs adequately only in the short-trajectory scenario of the late jointing stage, while it completely loses accuracy in the early jointing stage. Except for the late jointing stage, Point-LIO exhibits chaotic localization in all other growth stages, indicating extremely poor adaptability.

Figure 7 shows the trajectory components over time for each algorithm across four growth stages, displaying the time-varying curves of the X, Y, and Z coordinates. During early jointing, CK-SLAM and A-LOAM closely matched RTK true values on all three axes, while LeGO-LOAM and Point-LIO exhibited significant errors on the Y-axis. During the late jointing stage, the shorter trajectory length prevented cumulative errors from fully manifesting, resulting in minimal algorithmic differences. By the tasseling and milk stages, as corn stalks thickened and plant height increased, row features became easier to extract. CK-SLAM’s crop row constraint mechanism enabled high alignment with true values across all three axes, while A-LOAM exhibited slight single-axis drift. After 100 s of operation, LeGO-LOAM exhibited noticeable deviations on both the X- and Y-axes, while Point-LIO showed severe deviation throughout the entire process.

Figure 6 and Figure 7 visually demonstrate the trajectory tracking performance of various algorithms on a quadruped robot platform. The LeGO-LOAM and Point-LIO algorithms, which fuse IMU data, exhibit significant trajectory deviations. In contrast, the pure LiDAR-based A-LOAM and CK-SLAM algorithms produce trajectories closer to the ground truth, with CK-SLAM maintaining high alignment with the ground truth throughout the entire testing period. The primary reason may be that the gait motion of quadruped robots causes high-frequency jitter in IMU data. Algorithms incorporating IMU failed to effectively process this jitter interference, instead introducing additional errors. LeGO-LOAM’s loosely coupled IMU failed to accurately compensate for the robot’s bumpy motion. Although Point-LIO’s tightly coupled architecture is theoretically superior, it was not optimized for the robot’s non-stationary motion characteristics, leading to accumulated IMU pre-integration errors and ultimately inferior positioning accuracy compared to pure LiDAR algorithms.

Table 2. Localization Accuracy and Real-Time Performance of Different Algorithms Across Growth Stages (Note: Real-time performance metrics represent per-frame runtime in milliseconds (ms)).

Growth Stage	Method	Accuracy Metrics		Real-Time Performance Metrics (ms)
		APE RMSE	RPE RMSE	Average Time	Minimum Time	Maximum Time
Early Jointing	A-LOAM	2.2183	0.1021	14.33	7.38	57.41
	LeGO-LOAM	7.5265	0.0991	12.39	10.00	80.50
	Point-LIO	8.8630	0.1089	12.92	2.49	21.02
	CK-SLAM	2.1097	0.0990	35.42	17.36	89.19
Late Jointing	A-LOAM	1.9547	0.0961	16.70	8.02	57.87
	LeGO-LOAM	2.5728	0.0915	16.74	10.00	96.82
	Point-LIO	1.8393	0.1178	13.18	2.49	21.02
	CK-SLAM	1.9182	0.0914	34.37	14.95	93.78
Tasseling Stage	A-LOAM	2.9861	0.1003	14.21	3.79	58.37
	LeGO-LOAM	2.7556	0.0913	13.77	10.00	90.99
	Point-LIO	9.6048	0.1104	14.87	2.49	21.02
	CK-SLAM	2.3638	0.0984	36.27	18.96	69.48
Milk Ripening Stage	A-LOAM	2.8080	0.0959	15.08	3.37	57.75
	LeGO-LOAM	6.3427	0.0868	14.09	10.00	152.88
	Point-LIO	9.8624	0.1030	12.90	5.28	15.00
	CK-SLAM	1.9840	0.0897	35.88	19.25	57.07
Average	A-LOAM	2.4917	0.0986	15.08	5.64	57.85
	LeGO-LOAM	4.7994	0.0921	14.24	10.00	105.29
	Point-LIO	7.5423	0.1100	13.46	3.19	19.51
	CK-SLAM	2.0939	0.0946	35.48	18.70	77.38

Note: Values in bold indicate the best-performing algorithm in each category.

presents the statistical posture accuracy and real-time performance metrics for each algorithm across four growth cycles. Data represents the average of five test rounds, with bolded values indicating the optimal result.

In terms of the APE RMSE metric, CK-SLAM achieved an average value of 2.0939, representing reductions of 15.7%, 56.4%, and 72.2% compared to A-LOAM (2.4917), LeGO-LOAM (4.7994), and Point-LIO (7.5423), respectively, demonstrating a notable advantage in global positioning accuracy. The most pronounced performance improvement occurs during the milk ripening stage (characterized by the densest canopy and most severe occlusion). CK-SLAM achieves an APE RMSE of 1.9840, while A-LOAM, LeGO-LOAM, and Point-LIO register 2.8080, 6.3427, and 9.8624, respectively. This demonstrates that CK-SLAM, through targeted optimization for agricultural scenarios, effectively mitigates feature extraction interference caused by dense canopy cover, resolving the severe positioning drift issues faced by traditional algorithms in complex agricultural environments.

Regarding the RPE RMSE metric, CK-SLAM achieved an average value of 0.0946, maintaining a leading position. LeGO-LOAM attained lower RPE RMSE in certain growth stages but exhibited insufficient overall stability. Point-LIO’s average RPE RMSE of 0.11 was the highest among all algorithms. This reflects its inability to accurately capture subtle motion state changes caused by robot gait transitions or terrain undulations due to the combined effects of IMU pre-integration errors and failed point cloud matching during pose calculation between adjacent time steps. Its local pose estimation capability was significantly weaker than other algorithms. A-LOAM demonstrated stable RPE RMSE performance but was slightly inferior to CK-SLAM overall.

Figure 8 and Figure 9 show the runtime trends of each core module and the single-frame computation time histogram of CK-SLAM across four growth stages, respectively. The histogram reveals that CK-SLAM exhibits a typical unimodal distribution in single-frame computation time across all four scenarios, with the peak concentrated in the 33–40 ms range and an average time around 35 ms. This demonstrates the algorithm’s high processing stability and consistent scene adaptability. Among the four core modules, the feature extraction module consumes the most time, accounting for an average of 40% of the total runtime, mainly due to the innovative crop plane feature extraction process. The laser odometry module follows, while preprocessing and factor graph optimization have relatively lower and stable time consumption. The runtime trends of each module show no significant fluctuations across stages, with only a slight increase in the feature extraction module’s runtime during the tasseling stage due to denser canopies. Although CK-SLAM’s average single-frame computation time (approximately 35 ms) is longer than that of traditional algorithms such as LeGO-LOAM (average 14.24 ms) and A-LOAM (average 15.08 ms), it fully meets the real-time requirements for a 10 Hz frame rate of lidar systems.

3.5. Ablation Experiment

To quantify the independent contributions of CK-SLAM’s three core innovations—InEKF-based leg odometry, crop plane feature extraction, and multi-feature factor graph optimization—to localization performance, three ablation variants were designed by removing one core module each from the complete CK-SLAM algorithm, with the full algorithm serving as the baseline. The ablation experiments included four test groups: the full CK-SLAM system (referred to as “Full System”) and three variants with each core module removed sequentially, denoted as “W/O InEKF” (excluding the InEKF-based leg odometry module), “W/O Crop Plane” (removing the crop plane constraint module), and “W/O Optimization” (excluding the backend factor graph optimization module). The specific adjustments for each variant are as follows: For “W/O InEKF”, the point cloud deskewing and initial pose guess for laser odometry relied solely on raw IMU data integration, without fusing joint encoder data or the foot contact probability model, and the backend optimization only retained laser odometry factors. For “W/O Crop Plane”, the feature extraction process skipped grid filtering, differential evolution-based crop row detection, and RANSAC crop plane fitting, with laser odometry optimization constrained solely by line features and ground plane features. For “W/O Optimization”, the laser odometry results after two-step LM iteration were directly used as the final pose output, without global consistency optimization via factor graph fusion of leg odometry and laser odometry factors.

Table 3. Quantitative Comparison of Localization Performance of CK-SLAM and Its Ablation Variants Across Different Growth Stages.

Growth Stage	Method	APE RMSE (m)	APE Change (%)	RPE RMSE (m)	RPE Change (%)
Early Jointing	Full System	0.6104	-	0.3217	-
	W/O Crop Plane	0.6758	+10.713%	0.3199	−0.573%
	W/O Optimization	2.6665	+336.820%	0.3174	−1.331%
	W/O InEKF	7.6589	+1154.668%	0.3211	−0.186%
Late Jointing	Full System	1.0012	-	0.7085	-
	W/O Crop Plane	1.0181	+1.684%	0.7106	+0.294%
	W/O Optimization	1.8387	+83.637%	0.7086	+0.015%
	W/O InEKF	0.9735	−2.768%	0.7087	+0.022%
Tasseling Stage	Full System	0.9651	-	0.4355	-
	W/O Crop Plane	1.3270	+5.629%	0.4360	+0.109%
	W/O Optimization	2.5894	+168.278%	0.4317	−0.875
	W/O InEKF	4.3656	+352.302%	0.4362	+0.163%
Milk Ripening Stage	Full System	0.6780	-	0.3200	-
	W/O Crop Plane	0.8029	+18.427%	0.3206	+0.179%
	W/O Optimization	2.9249	+331.383%	0.3193	−0.229%
	W/O InEKF	4.4387	+554.647%	0.3230	+0.929%
Average	Full System	0.8137	-	0.4464	-
	W/O Crop Plane	0.9517	+9.113%	0.4467	+0.072%
	W/O Optimization	2.5049	+207.826%	0.4442	−0.488%
	W/O InEKF	4.3592	+435.703%	0.4472	+0.182%

Note: Calculation formulas for percentage changes: APE Change (%) = [(APE RMSE of Ablation Variant − APE RMSE of Full System)/APE RMSE of Full System] × 100%. RPE Change (%) = [(RPE RMSE of Ablation Variant − RPE RMSE of Full System)/RPE RMSE of Full System] × 100%. Bold values indicate the best performance in each category

presents the quantitative comparison results of the four test groups across all growth stages, and Figure 10 visually displays the global trajectories of each variant in representative stages (tasseling stage and milk ripening stage), while Figure 11 presents the XYZ axis position error curves of the variants in these stages. As shown in Table 3, the Full System achieved the lowest average APE RMSE (0.8137 m) across all stages, confirming the synergistic effectiveness of the three core modules in addressing cornfield navigation challenges throughout the crop growth cycle.

The “W/O InEKF” variant exhibited the most significant overall performance degradation, with an average APE RMSE of 4.3592 m (a 435.703% increase compared to the Full System). This degradation was particularly pronounced in the early jointing stage and milk ripening stage: in the early jointing stage, the variant’s APE RMSE reached 7.6589 m (1154.668% higher); in the milk ripening stage with dense canopy and severe occlusion, the APE RMSE surged to 4.4388 m (554.647% higher). Notably, this variant, which relies solely on raw IMU data for motion state estimation, exhibits trajectory behaviors similar to those of traditional SLAM algorithms such as Lego-LOAM and Point-LIO, including trajectory regression and oscillation phenomena. This indicates that the InEKF-based leg odometry module effectively suppresses motion noise caused by quadruped robot gait variations and foot slippage by fusing joint encoder data and foot contact probability models. In contrast, raw IMU data alone cannot compensate for accumulated jitter and terrain-induced errors, leading to unstable trajectory estimation and obvious regression-oscillation characteristics.

The “W/O Crop Plane” variant showed an average APE RMSE of 0.9517 m (a 9.113% increase) and an average RPE RMSE of 0.4467 m (a 0.072% increase), with performance degradation exhibiting a distinct correlation with navigation distance. As visualized in Figure 11, during long-distance navigation tasks that involve traversing multiple crop rows and completing turning maneuvers, the “W/O Crop Plane” variant fails to maintain the expected parallel trajectory relationship between pre-turn and post-turn paths. Instead, the trajectory gradually drifts as the navigation distance increases, ultimately resulting in crossing between the post-turn and pre-turn trajectories. This phenomenon stems from the fact that the crop plane feature, as a scene-specific geometric constraint customized for cornfield environments, leverages the regular spatial distribution of crop rows to provide continuous and stable long-distance positioning references. It effectively restricts the cumulative drift of lateral position and heading angle during extended navigation, ensuring the consistency and parallelism of trajectories across multiple crop rows. Without this module, the algorithm relies solely on traditional line and ground plane features, which lack sufficient scene adaptability to constrain long-term drift in complex cornfield environments. As a result, the positioning errors accumulate continuously with increasing navigation distance, leading to trajectory crossing during row switching and turning in long-distance tasks. The quantitative results further verify this trend: in the early jointing stage with relatively short navigation distances, the APE increase is only 10.713%; while in the milk ripening stage involving longer navigation paths and denser vegetation, the APE increase rises to 18.427%, reflecting the growing importance of the crop plane constraint in long-distance navigation.

The “W/O Optimization” variant had an average APE RMSE of 2.5049 m (a 207.826% increase) while maintaining a relatively stable average RPE RMSE (0.4442 m, a 0.488% decrease). This trend was consistent across all stages: the variant’s RPE remained close to the Full System in short-term motion estimation, but its APE diverged significantly as the trajectory lengthened. A key function of the backend factor graph optimization module is to effectively restrict lateral drift of the robot during row switching and turning maneuvers. As observed in Figure 11, when the robot performs row switching and turning operations, the absence of this module leads to inadequate suppression of lateral position deviation and angular error. Specifically, during U-turn maneuvers intended to transition to an adjacent parallel crop row, the variant exhibits excessive lateral drift and angular deviation, resulting in the post-turn trajectory failing to maintain parallelism with the pre-turn path and instead producing crossing phenomena. This indicates that the backend factor graph optimization module integrates leg odometry and laser odometry factors to correct local estimation errors in real time, thereby maintaining the accuracy of lateral position and heading angle during dynamic processes such as row switching and turning. Without this module, the algorithm cannot eliminate cumulative errors from local motion estimation, leading to uncontrolled lateral drift during key navigation phases and subsequent trajectory crossing. This phenomenon is particularly prominent in long-distance navigation across all growth stages, even though short-term local motion performance remains stable.

4. Discussion

Corn plants exhibit rapid growth in height and density throughout their lifecycle, posing significant challenges for manual field scouting due to narrow row spacing, dense canopy occlusion, and complex terrain. Autonomous scouting by quadruped robots has emerged as a promising solution, but reliable positioning remains a critical bottleneck. This study proposed CK-SLAM, integrating an Invariant Extended Kalman Filter (InEKF), innovative multi-feature extraction (including crop plane features), and a multi-constraint global optimization framework to address this bottleneck. Experimental validation across four corn growth stages demonstrated that CK-SLAM achieves more stable and accurate localization compared to mainstream SLAM methods, providing valuable insights for real-world agricultural robotics applications while also highlighting key limitations and future directions.

4.1. Performance Interpretation of Localization Accuracy Metrics

CK-SLAM achieved an average Absolute Pose Error (APE) RMSE of 2.0939 m, reducing errors by 15.7%, 56.4%, and 72.2% compared to A-LOAM, LeGO-LOAM, and Point-LIO, respectively. Previous studies in structured agricultural environments (e.g., orchards) reported lower positioning errors, but these are not directly comparable due to simpler environmental complexity—CK-SLAM targets mid-to-late corn growth stages with closed canopies and sub-meter row spacing, where traditional methods struggle.

For practical autonomous scouting, CK-SLAM’s 2 m average APE RMSE is sufficient for inter-row navigation (corn row spacing 0.8–1.2 m), ensuring the robot avoids crop collisions. It meets the needs of tasks like canopy density assessment and disease detection, which prioritize coverage over centimeter-level precision. Notably, its performance is most impressive in the milk ripening stage (APE RMSE = 1.9840 m), where canopy occlusion is severe, addressing a key limitation of traditional methods.

4.2. Adaptability to Quadruped Robot Platform and Agricultural Scenarios

For quadruped robots, gait-induced motion noise (e.g., IMU jitter from foot-ground impacts and body bumps) is a major source of positioning error. Most mainstream SLAM algorithms (including LeGO-LOAM and Point-LIO) fuse IMU data but fail to account for the high-dynamic motion characteristics of legged robots. LeGO-LOAM’s loosely coupled IMU integration cannot effectively compensate for bumpy motion, while Point-LIO’s tightly coupled architecture relies on the assumption of “approximate motion smoothness” (valid for wheeled robots but not quadruped robots). As a result, LeGO-LOAM and Point-LIO exhibit chaotic trajectories in most corn growth stages, with APE RMSE values 2.3–3.6 times higher than CK-SLAM in the milk ripening stage.

In contrast, CK-SLAM’s InEKF-based leg odometry module addresses this issue by fusing IMU data with joint encoder data and a foot contact probability model (derived from joint torque). This fusion strategy dynamically adjusts observation noise covariance to suppress motion noise from gait variations and foot slippage, providing a reliable initial pose reference for point cloud deskewing. Ablation experiments confirmed the critical role of this module: the variant excluding InEKF (W/O InEKF) exhibited an average APE RMSE of 4.3592 m, a 435.7% increase compared to the full CK-SLAM system.

CK-SLAM’s innovative crop plane feature extraction module enhances cornfield adaptability through dual stability support. On one hand, it uses grid filtering, differential evolution, and RANSAC to capture crop row planar clusters, directly complementing the line and plane features inherited from LeGO-LOAM and addressing the scarcity of stable geometric features in cornfields. On the other hand, the crop plane features primarily consist of points from corn stalks, as the extraction process targets the planar clusters formed by crop rows. Compared to corn leaves, stalks are less susceptible to external disturbances such as wind, maintaining structural stability even under dynamic field conditions. This dual advantage ensures reliable feature matching, especially under dense canopies, where traditional features are scarce or easily disrupted.

4.3. Limitations of the Study

Despite promising performance, this study has several limitations.

4.3.1. Algorithmic Limitation: Sensitivity to Curved Crop Rows

Corn is predominantly planted in straight rows, with minor, scattered deviations from sowing tolerances or terrain undulations—scenarios CK-SLAM adapts to well. A core premise of the differential evolution algorithm is that crop rows appear as two parallel lines within the LiDAR scanning range. This enables adaptation to large-radius curved rows, as local segments approximate parallel lines sufficiently for parameter optimization and plane fitting. However, small-radius curved rows deviate significantly from this parallel linear model within the scanning window. The differential evolution algorithm, constrained by its parallel linear parameterization, cannot accurately capture the curved structure, leading to failure in extracting valid crop lines or the extraction of incorrect crop lines. It is worth noting that such small-radius curved planting scenarios are extremely rare in corn production, as they conflict with standard agronomic practices and mechanized farming operations, limiting the practical impact of this limitation.

4.3.2. Computational Efficiency Limitation: High Time Cost of Feature Extraction

CK-SLAM’s multi-feature strategy increases computational complexity. The differential evolution algorithm (20–30 iterations, ~40% of single-frame runtime) and crop plane fitting/matching add overhead, resulting in an average single-frame runtime of 35.48 ms—2.3–2.6 times longer than LeGO-LOAM and A-LOAM. While meeting 10 Hz LiDAR real-time requirements, reduced time redundancy raises frame loss risks in extreme scenarios. High computational costs also restrict deployment on low-power edge platforms.

4.3.3. Other Application Limitations

CK-SLAM’s ~2 m APE RMSE suffices for inter-row navigation but not high-precision tasks (e.g., targeted pesticide application) requiring centimeter-level accuracy. In scenarios with high corn lodging rates or dense weed coverage, performance may degrade—lodging disrupts crop row structure, while dense weeds introduce false features that grid filtering and direction constraints cannot fully suppress. These scenarios were not fully tested, and future work should evaluate robustness under such conditions.

It should be noted that Figure 6 and Figure 7 present representative single-test trajectories. LeGO-LOAM and Point-LIO’s start/end points differ from RTK ground truth due to evo tool rigid-body alignment, which eliminates initial pose offsets without affecting metric validity. Future visualizations will clarify this process.

5. Conclusions

This study addresses the high-precision localization and mapping challenge for quadruped robots in cornfield scouting—where narrow row spacing, dense canopy occlusion, and complex terrain hinder traditional SLAM algorithms—by proposing CK-SLAM, a framework integrating quadruped kinematic constraints and crop row feature extraction. Its core innovations include an InEKF-based leg odometry for adapting to legged robots’ high-dynamic motion, an innovative cornfield-specific crop plane feature extraction module, and a factor graph optimization framework fusing multi-source constraints for global pose refinement. Validated across four key corn growth stages (early jointing to milk ripening), CK-SLAM outperforms mainstream baselines (A-LOAM, LeGO-LOAM, Point-LIO) with an average APE RMSE of 2.0939 m (15.7–72.2% lower than competitors) and an average RPE RMSE of 0.0946 m, excelling particularly in the milk ripening stage (APE RMSE = 1.9840 m) under severe occlusion. Despite a longer average single-frame runtime (35 ms), it meets the real-time requirements of 10 Hz LiDAR systems, providing a reliable navigation solution for automated cornfield monitoring and laying a foundation for SLAM adaptation in other dense crop scenarios.

Notably, CK-SLAM has inherent limitations: it struggles with small-radius curved crop rows (rare in practical corn production), it faces computational overhead from the differential evolution-based crop row detection (restricting low-power edge deployment), and its ~2 m APE RMSE is insufficient for centimeter-level precision tasks (e.g., targeted pesticide application). Additionally, performance may degrade in high corn lodging or dense weed scenarios.

Future work will focus on three key directions: optimizing the computational efficiency of the differential evolution module to reduce runtime overhead, integrating multi-modal sensors (e.g., visual or semantic sensors) to enhance robustness under extreme occlusion and dynamic plant interference, and extending the algorithm to handle non-straight crop rows and high-disturbance field conditions (e.g., lodging and dense weeds) for broader agricultural applicability.