Robot synchronous positioning and mapping method used under corn field canopy

Abstract

The invention relates to the technical field of computer vision, and provides a synchronous positioning and mapping method for a robot under a corn field canopy, and the method comprises the steps: obtaining and preprocessing an environment image, IMU data and encoder data, and obtaining visual data, inter-frame motion increment and motion data; updating a world coordinate system state value based on the inter-frame motion increment; performing visual SFM processing on the basis of the visual data to obtain an image acquisition posture and a road sign point position; performing visual inertia combination to obtain an InEKF initial state; performing InEKF fusion based on the state value of the world coordinate system, the motion data and the initial state of the InEKF to construct a self-sensing odometer; and performing nonlinear optimization processing based on the visual data, the inter-frame motion increment, the motion data and the self-sensing speedometer to obtain a pose estimation result and a map point cloud. According to the method, feature matching, edge feature supplementation and IMU-encoder sensor fusion technologies are utilized, and high-robustness and high-precision robot positioning and map construction adaptive to the whole corn growth cycle are achieved.

Description

Technical Field

[0001] This application relates to the field of computer vision technology, and in particular to a method for simultaneous localization and mapping of robots under the canopy of cornfields. Background Technology

[0002] With global population growth and the migration of rural labor to cities, modern agriculture faces the dual challenges of increasing yields and achieving sustainable development, making smart agriculture an important development direction. Agricultural robots (such as unmanned vehicles and drones) can undertake tasks such as precision sowing, variable fertilization, and pest and disease monitoring. Their efficient and precise operation relies on accurate positioning and environmental perception capabilities, making Simultaneous Localization and Mapping (SLAM) technology a core support.

[0003] SLAM can be divided into two categories based on the sensor used: LiDAR-based and vision-based. While LiDAR can acquire high-precision environmental depth information, its high cost and large size make it difficult to widely apply in agricultural robots. Vision SLAM uses ordinary cameras to acquire images and relies on image feature extraction and matching to achieve localization and mapping. It has the advantages of low cost, small size, and easy integration, and can utilize rich features such as image texture and color to improve perception accuracy, making it more suitable for agricultural scenarios. However, traditional vision SLAM is difficult to apply directly to agricultural environments: First, farmland has a lot of repetitive textures (such as continuous corn crops), making it difficult to distinguish feature points and affecting the accuracy of localization and mapping; second, complex natural lighting (strong light, shadow, day and night, and seasonal changes) interferes with the quality of images acquired by the camera; and third, undulating terrain (potholes, mounds) can easily cause changes in camera perspective and motion blur. Furthermore, pure vision SLAM will interrupt trajectory estimation when visual information is lost (such as when corn leaves are occluded). Existing improved methods are either sensitive to light and prone to drift, or rely on pre-built datasets and network training, resulting in low generalization ability and high time and effort consumption, which cannot meet the stable localization and mapping requirements of robots under the corn canopy. Summary of the Invention

[0004] In view of the above problems, this application provides a method for robot synchronous localization and mapping under the canopy of cornfields. It utilizes SuperPoint and LightGlue feature matching, edge feature supplementation and IMU-encoder sensor fusion technology to achieve robot localization and map construction with high robustness, high accuracy and adaptability to the entire growth cycle of corn.

[0005] Embodiments of this application provide a method for simultaneous localization and mapping of robots under cornfield canopies, including: Acquire environmental images of the cornfield, IMU data of the robot, and encoder data; The environmental image is subjected to a first preprocessing to obtain visual data, which includes core feature point pairs and static edge feature sets; The IMU data is subjected to a second preprocessing to obtain inter-frame motion increments; the world coordinate system state values ​​are updated based on the inter-frame motion increments, and the world coordinate system state values ​​include position, attitude and velocity; The encoder data is subjected to a third preprocessing step to obtain motion data; Visual SFM processing is performed based on the core feature point pairs to obtain the acquisition pose and landmark position of each frame of environmental image. Visual-inertial joint processing is performed based on the attitude of each frame of environmental image acquisition, the position of the landmark point, and the inter-frame motion increment to obtain the InEKF initial state; the InEKF initial state includes the gravity direction, scale factor, gyroscope bias, and velocity corresponding to each frame of environmental image; A self-sensing odometry is constructed by fusing the world coordinate system state value, the motion data, and the InEKF initial state. Based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry, nonlinear optimization processing is performed to obtain pose estimation results and map point clouds.

[0006] In the above implementation, by simultaneously acquiring cornfield environmental images, IMU data, and encoder data, basic coverage of multi-source sensor information is achieved, solving the localization failure problem of traditional single-vision SLAM under repetitive textures, complex lighting, and canopy occlusion in cornfields. The first preprocessing of the environmental images extracts core feature point pairs and static edge feature sets to compensate for the sparse visual features under the corn canopy. The inter-frame motion increment obtained by IMU pre-integration can update the world coordinate system state in real time, providing high-frequency support for motion estimation. The motion data preprocessed by the encoder, together with the visual SFM results and IMU increments, jointly construct the InEKF initial state to ensure the accuracy of subsequent fusion. The self-sensing odometry constructed by InEKF fusion can maintain trajectory estimation when visual information is lost (such as severe leaf occlusion) and seamlessly connect after visual recovery. Finally, through nonlinear optimization and integration of multi-source data, a high-precision pose and point cloud map are output.

[0007] In some possible implementations, the first preprocessing of the environmental image to obtain visual data includes: The environmental image is subjected to feature extraction and matching processing to obtain core feature point pairs; The environmental image is subjected to edge feature supplementation processing to obtain a static edge feature set.

[0008] In the above implementation, a dual visual data support of "core features + edge features" is formed through feature extraction and matching, and edge feature supplementation: feature extraction and matching processes address the problem of repetitive textures in cornfields by acquiring core feature point pairs with strong discriminative power, providing basic feature constraints for visual localization; edge feature supplementation processes address the scenario where core features are insufficient under the canopy by extracting static edge feature sets to enrich environmental contour information. The synergistic effect of these two processes effectively alleviates the problem of sparse visual features under the corn canopy and insufficient feature reliability caused by dynamic leaf interference, providing a more comprehensive and stable visual data foundation for subsequent visual SFM, visual-inertial joint processing, and nonlinear optimization, ensuring the continuity of localization and mapping.

[0009] In some possible implementations, the step of performing feature extraction and matching processing on the environmental image to obtain core feature point pairs includes: The environmental image is processed by a SuperPoint self-supervised network to extract core feature points, and the descriptors of the core feature points are at least 128-dimensional. The core feature points of different frames are matched using the LightGlue network to obtain core feature point pairs.

[0010] In the above implementation, the SuperPoint self-supervised network is used to extract core feature points. It can generate at least 128-dimensional high-discrimination descriptors without a large amount of manually labeled data. It can process full-size images in real time and has strong adaptability to complex lighting conditions such as strong light and shadow in cornfields. It can stably capture key features of corn plants. It is paired with the LightGlue network for feature matching. It captures the relative position of features within the frame through self-attention and realizes the inter-frame feature association through cross-attention. The lightweight classifier can also adaptively adjust the inference process according to the matching difficulty, effectively solving the matching confusion problem caused by the repetition of corn leaf texture.

[0011] In some possible implementations, the edge feature supplementation processing of the environmental image to obtain a static edge feature set includes: The environmental image is segmented using HSV color to obtain a first image; The first image is processed using Canny edge processing to obtain the second image; The second image is subjected to region growing processing to obtain the third image; Optical flow tracing and gradient consistency constraint processing are performed on the third image to obtain a static edge feature set.

[0012] In the above implementation, HSV color segmentation can accurately extract the green areas of corn, achieving initial separation of corn plants from soil and weeds and reducing interference from irrelevant backgrounds. Canny edge processing, combined with Gaussian filtering, non-maximum suppression, and dual thresholding, can efficiently extract the initial edges of corn plant contours. Region growth processing can filter redundant edge fragments, retaining structurally complete and effective edges, solving the edge redundancy problem of the Canny algorithm. Optical flow tracking and gradient consistency constraints can eliminate dynamic edge points caused by leaf oscillations, ensuring the static nature of edge features. The static edge feature set obtained through the above implementation can supplement the insufficient coverage of core feature points under the canopy, enhance the SLAM system's perception of corn plant contours, reduce the positioning error of visual odometry in feature-sparse scenarios, and improve the system's robustness to complex cornfield environments.

[0013] In some possible implementations, the step of performing HSV color segmentation on the environmental image to obtain a first image further includes: If the number of features in the first image is less than a first threshold, then the environmental image is processed into a grayscale image and used as the first image.

[0014] In the above implementation, to address the problem of insufficient first image features after HSV color segmentation due to yellowing of maize leaves and reduction of green features during the maize maturity period, a grayscale image processing mechanism is added to avoid interruption of edge feature extraction due to the failure of the green mask.

[0015] In some possible implementations, the second preprocessing is IMU pre-integration, and the inter-frame motion increment includes relative rotation increment, velocity increment, and position increment.

[0016] In the above implementation, the second IMU preprocessing is set to pre-integration. Within a two-frame image interval, the relative rotation increment, velocity increment, and position increment can be directly obtained by pre-integrating the IMU accelerometer and gyroscope data, eliminating the need to re-integrate the original IMU data for each optimization, thus improving computational efficiency. Simultaneously, this inter-frame motion increment can update the position, attitude, and velocity state values ​​in the world coordinate system in real time, providing high-frequency, continuous motion constraints for robot motion estimation. Compared to directly using the original IMU data, pre-integration processing reduces computational redundancy and compensates for errors caused by IMU noise, providing a more reliable motion reference for subsequent vision-inertial joint processing.

[0017] In some possible implementations, the third preprocessing of the encoder data to obtain motion data includes: For wheeled robots, the encoder data is converted into robot motion data based on a differential drive model, and the motion data includes linear velocity and angular velocity.

[0018] In the above implementation, considering the motion characteristics of wheeled agricultural robots, encoder data is converted into robot linear and angular velocities based on a differential drive model, achieving a precise mapping between encoder data and robot motion states. The preprocessed motion data accurately reflects the motion state of the wheeled robot on flat or gently sloping terrain in cornfields, providing InEKF fusion with observations consistent with wheeled motion patterns.

[0019] In some possible implementations, the third preprocessing of the encoder data to obtain motion data includes: For legged robots, the encoder data is converted into motion data of the robot based on a forward kinematics model, and the motion data includes the position and velocity of the end of the foot.

[0020] In the above implementation, considering the joint-movement-dependent nature of legged agricultural robots, a forward kinematics model is used to accurately capture the robot's motion trajectory on undulating terrain (such as potholes and small slopes) in cornfields, solving the problem that traditional encoder processing methods are not suitable for leg joint movements. The motion data obtained from this preprocessing reflects the contact state between the foot and the ground, providing InEKF fusion with observations that conform to the laws of leg movement and avoiding odometer drift caused by terrain undulations.

[0021] In some possible implementations, the nonlinear optimization processing based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry to obtain pose estimation results and map point clouds includes: Define a state vector and construct an optimization objective function with the goal of minimizing the sum of residuals; The optimization objective function is iteratively calculated using the Ceres Solver optimization library and the LM algorithm. An adaptive kernel function strategy is used to dynamically reduce the residual weights corresponding to mismatched features, thereby obtaining the optimal solution of the state vector. Based on the optimal solution of the state vector, the pose estimation result and map point cloud are obtained.

[0022] In the above implementation, by setting a state vector that covers key parameters, the robot's motion and sensor-related feature depth information are fully covered, ensuring the dimensional integrity of nonlinear optimization. An optimization objective function is constructed with the goal of minimizing the sum of residuals, realizing the collaborative constraint of multi-source data. The Ceres Solver optimization library and the LM algorithm are used to leverage their efficient iterative solution capabilities to quickly converge to the optimal state, improving optimization efficiency. The kernel function adaptive strategy can dynamically reduce the residual weights corresponding to mismatched features (such as false features generated by dynamic leaves), effectively suppressing errors caused by dynamic disturbances in the cornfield and changes in illumination.

[0023] In some possible implementations, the state vector includes camera-IMU motion and bias states, camera-IMU extrinsic parameters, inverse depth of core feature points, and depth of static edge feature points; The camera-IMU motion and bias state includes the world coordinate system state value, IMU accelerometer zero bias, and IMU gyroscope zero bias; The camera-IMU extrinsic parameters include the position difference and attitude difference of the camera relative to the IMU; The residuals include marginalized prior residuals, IMU pre-integration residuals, SuperPoint residuals, marginal feature residuals, and InEKF residuals; the InEKF residuals are determined based on the self-sensing odometry estimate and the corresponding predicted value in the state vector.

[0024] In the above implementation, the state vector encompasses the camera-IMU motion and bias states (including world coordinate system state and IMU zero bias), camera-IMU extrinsic parameters, inverse depth of core feature points, and depth of static edge feature points. This comprehensively covers robot motion, sensor calibration, and feature space information, ensuring the completeness of parameters in nonlinear optimization and avoiding optimization deviations caused by missing key parameters. Residual types include marginalized prior residuals (inheriting historical optimization information), IMU pre-integration residuals (constraining high-frequency motion), SuperPoint residuals (constraining core visual features), edge feature residuals (constraining environmental contours), and InEKF residuals (constraining self-sensing odometry and state prediction values). This achieves complementary constraints from multiple information sources. In particular, the InEKF residuals can correct the deviation between self-sensing odometry and state prediction, further enhancing error suppression capabilities. This design ensures both accuracy and consistency in the optimization results, maintaining system stability even in complex cornfield scenarios with dense canopy layers (sparse visual information) during the corn tasseling stage.

[0025] The aforementioned technical features can be combined in various suitable ways or replaced by equivalent technical features, as long as the purpose of this application can be achieved.

[0026] Compared with existing technologies, this application proposes a SLAM method applicable to corn canopy environments. This method achieves accurate trajectory estimation by fusing data from SuperPoint, edge features, inertial measurement units (IMUs), and encoders. SuperPoint, as a state-of-the-art feature extraction algorithm, can efficiently extract salient feature points in cornfield scenes, and when combined with Lightglue, it can achieve stable and accurate feature matching. Considering the sparse texture under the corn canopy, SuperPoint may not be able to obtain sufficiently comprehensive feature information. Therefore, edge features are introduced as a supplement. These edge features have excellent descriptive ability for the contours of corn plants, greatly enriching the overall feature information. In practical applications, visual information may be lost due to factors such as leaf occlusion. Therefore, the data from the inertial measurement units and encoders are fused to construct a self-sensing odometry. This fused data can maintain the system's trajectory estimation when visual features are lost, and when visual features are recovered, the system can seamlessly recover and continue to perform accurate localization and map construction. Furthermore, (1) using SuperPoint+Lightglue to replace the traditional feature extraction and matching method can effectively extract significant feature points under the corn canopy and achieve stable and accurate feature matching, providing a more reliable feature basis for the SLAM system; (2) adding edge features that can describe the outline of the corn plant as a supplement can make up for the lack of comprehensive feature information obtained by SuperPoint in this scenario and enrich the overall feature information; (3) by fusing the odometry data of the proprioceptive sensor, when visual information is lost due to factors such as leaf occlusion, the fused data can maintain the trajectory estimation of the system; when the visual features are restored, the system can be seamlessly restored and continue to run. Attached Figure Description

[0027] The present application will be described in more detail below based on embodiments and with reference to the accompanying drawings. Wherein: Figure 1 This is a schematic diagram of a system architecture for a robot synchronous localization and mapping method under the canopy of a cornfield, provided by an embodiment of this application; Figure 2 This is a comparison of the results of feature extraction methods using SuperPoint+LightGlue and ORB+BRIEF provided in the embodiments of this application; Figure 3 This is a schematic diagram of the edge feature extraction process of a robot synchronous localization and mapping method under the canopy of a cornfield, provided by an embodiment of this application; Figure 4This is a schematic diagram of an embodiment of the present application providing a method for simultaneous localization and mapping of robots under the canopy of cornfields, which uses optical flow to trace edge features. Figure 5 The method of this application and three other algorithms are compared in four scenarios to obtain the absolute trajectory error (ATE) and relative pose error (RPE). Detailed Implementation

[0028] To make the above-mentioned objectives, features, and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art are within the scope of protection of this application.

[0029] The terminology used in the implementation section of this application is for the purpose of explaining specific embodiments of this application only, and is not intended to limit this application.

[0030] With the migration of population to cities, natural resource management faces significant challenges, and the development of modern agriculture is facing severe tests. How to increase crop yields and stabilize agricultural product supply in a more sustainable way has become a key issue that modern agriculture needs to address. Against this backdrop, the concept of smart agriculture has emerged and rapidly become an important direction for agricultural development. Smart agriculture leverages modern information technology to achieve intelligent agricultural production, automated and refined management. Its core objectives are to improve agricultural production efficiency, reduce resource consumption, optimize management models, and thus promote sustainable agricultural development. In this process, autonomous robots (such as unmanned vehicles and drones) play a crucial role. They can perform various tasks in complex farmland environments, such as precision sowing, variable-rate fertilization, pest and disease monitoring, and crop growth assessment. To ensure efficient and accurate task execution, these robots must possess precise positioning and environmental perception capabilities. To meet this need, Simultaneous Localization and Mapping (SLAM) technology has been widely applied to agricultural robots. SLAM technology enables robots to perceive data in real time and simultaneously build environmental maps, achieving self-localization in unknown environments, thereby greatly improving the robot's autonomous navigation and task execution capabilities in dynamic and complex environments.

[0031] In practical applications, SLAM can be divided into two categories: LiDAR-based SLAM and vision-based SLAM. LiDAR SLAM acquires high-precision environmental depth information through radar, enabling accurate positioning and map building. However, LiDAR equipment is expensive and bulky, limiting its application in some scenarios. In contrast, visual SLAM uses ordinary cameras to collect environmental images and relies on image feature extraction and matching technology to complete positioning and mapping, offering significant advantages: firstly, camera equipment is inexpensive, compact, and easy to integrate; secondly, visual SLAM systems can operate effectively under natural lighting conditions and can utilize the rich features of images such as texture, color, and shape to improve environmental perception accuracy. SLAM technology has achieved many significant results after years of development, continuously breaking through in positioning and map building capabilities, but these achievements are still difficult to directly apply to agricultural environments. Agricultural environments present a series of unique challenges. Firstly, agricultural environments contain a large amount of repetitive textures, such as large areas of monoculture, making it difficult for image feature extraction and matching methods relying on visual SLAM to accurately identify and distinguish feature points in different areas, thus affecting the positioning and mapping accuracy of image feature-based visual SLAM. Secondly, the complex natural lighting conditions, including strong direct sunlight, varied shadows, and diurnal and seasonal differences in lighting, all interfere with the image quality acquired by the camera, further affecting the accuracy of localization and mapping in visual SLAM. Thirdly, the uneven terrain in agricultural environments, with its potholes, mounds, and other irregular landforms, can affect the camera's field of view and produce significant motion blur. The unique characteristics of agricultural environments dictate that SLAM technology applied to this field cannot simply replicate traditional methods; it requires targeted improvements based on the specific application scenario.

[0032] In view of this, to address at least some of the technical problems existing in the prior art, this application discloses a robot simultaneous localization and mapping (SLAM) method for use under the canopy of cornfields. This method achieves accurate trajectory estimation by fusing SuperPoint features, edge features, IMU data, and encoder data. As an advanced feature extraction algorithm, SuperPoint can efficiently extract salient feature points in cornfield scenes, and combined with Lightglue, it can achieve stable and accurate feature matching. Considering the sparse texture under the corn canopy, SuperPoint may not be able to obtain sufficiently comprehensive feature information. Therefore, edge features, which have excellent corn plant contour description capabilities, are further introduced as a supplement, significantly enriching the overall feature information. In practical applications, since visual information may be lost due to factors such as leaf occlusion, IMU and encoder data are also fused as self-sensing sensors to provide odometry information. This data fusion mechanism can maintain the system's trajectory estimation when visual features are lost and seamlessly continue when visual features are recovered, continuously achieving accurate localization and mapping.

[0033] Figure 1 This application provides a system architecture diagram for a robot simultaneous localization and mapping (SLAM) method under corn canopy, as illustrated in an embodiment. This SLAM method primarily addresses the autonomous navigation problem in agricultural environments (especially under corn canopy). By fusing SuperPoint features, edge features, IMU data, and encoder data, this method maintains accurate trajectory estimation capabilities even when visual information is limited or lost. The system inputs are images, IMU data, and encoder data, and the outputs are position estimation results and a point cloud map. The processing comprises three core modules: measurement preprocessing, initialization, and nonlinear optimization. Measurement preprocessing handles image feature extraction and matching, edge feature optimization, and IMU and encoder data processing. Initialization determines the system's initial parameters through loosely coupled vision and IMU systems. Nonlinear optimization uses a sliding window to optimize and correct state variables, ensuring positioning accuracy.

[0034] Measurement Processing Measurement processing is the initial stage of the entire system, and its main function is to derive and align camera data, IMU measurements, and encoder measurements. In camera data processing, point and edge features are simultaneously detected for each input image acquired by the camera. Point feature processing uses the SuperPoint method for feature extraction and LightGlue for feature matching. In edge feature processing, the image is first preprocessed, then the Canny algorithm is used to extract and optimize edge features, and finally, the extracted edge features are tracked using the LK optical flow method. For the raw gyroscope and accelerometer measurement data from the IMU, pre-integration is performed between consecutive frames to provide more effective data support for subsequent calculations. In the encoder data processing stage, encoder data is converted into corresponding state observations based on the platform's kinematic model.

[0035] SuperPoint feature extraction and LightGlue feature matching SuperPoint, a self-supervised feature point detection and description network, is used to extract point features from images. SuperPoint possesses numerous superior characteristics. In camera data processing, for each input image captured by the camera, this network can generate a 256-dimensional feature descriptor. Its unique self-supervised network framework can simultaneously complete feature point location detection and 256-dimensional descriptor generation without requiring a large amount of manually labeled data. This method does not require processing based on image patches and can perform real-time computation on full-size images, significantly improving feature extraction efficiency. Furthermore, the interest points extracted by SuperPoint exhibit high repeatability across multiple viewpoints, meaning that key feature points can still be stably extracted even when the camera viewpoint changes, providing a reliable foundation for subsequent feature matching. SuperPoint also demonstrates strong adaptability to changes in lighting conditions, stably extracting feature points under different lighting conditions and reducing the impact of uneven lighting and shadow changes on feature extraction. This is particularly important in dynamic lighting conditions in agricultural environments. These characteristics make SuperPoint highly adaptable to complex and varied agricultural scenarios.

[0036] LightGlue is a high-efficiency feature matching network whose structure consists of L stacked identical layers. Each layer contains self-attention and cross-attention units, enabling efficient processing of local features extracted from image sets. In the self-attention mechanism, rotational coding is used to capture the relative positional relationships of feature points within the same image. In the cross-attention mechanism, points from different images pay attention to each other, updating point representations without adding additional positional information. Simultaneously, a lightweight classifier adaptively determines whether to terminate inference based on the matching results. These characteristics make LightGlue highly adaptable to image matching tasks of varying difficulty: for easily matched image pairs, it can quickly complete the matching to improve efficiency; for challenging image pairs (such as images with complex lighting and sparse texture under a corn canopy), it still maintains high accuracy and accurately identifies the correspondences between feature points.

[0037] Compared to traditional corner-based feature extraction and optical flow tracking methods, the combination of SuperPoint and LightGlue offers significant advantages. In cornfield environments, the large, monoculture crops result in sparse textures, leaf occlusion, and complex lighting variations, often leading to insufficient feature point extraction and high matching error rates, which impacts the accuracy of localization and map construction. Thanks to its self-supervised mechanism, SuperPoint is insensitive to complex lighting and sparse textures and can extract more key feature points. LightGlue's adaptive matching strategy and advanced attention mechanism effectively address matching problems caused by the similarity of corn plants and significantly reduce the false matching rate. This achieves stable and accurate feature matching, providing crucial support for subsequent trajectory estimation and map construction. Figure 2 The results of feature extraction using SuperPoint+LightGlue and ORB+BRIEF were compared. It can be seen that using SuperPoint and LightGlue in cornfields can extract more feature points and achieve higher matching accuracy.

[0038] Edge feature processing To further enhance the constraint capability of visual odometry, 240 edge features are introduced as a supplement to improve the system's perception of environmental structure. Edge features can effectively capture the contour information of corn plants, providing richer environmental representation for the SLAM system, thereby improving the accuracy of trajectory estimation and map construction. This application proposes an edge extraction algorithm that integrates color space segmentation, region growing optimization, and gridded gradient-guided feature selection. Based on the distinguishability of corn plants in the color space, the algorithm first converts the input image from the BGR color space to the HSV color space. A mask is generated by setting the HSV threshold range for the green area to extract the region where the corn plants are located. Subsequently, the Canny edge detection algorithm, known for its high edge localization accuracy and strong noise resistance, is used for initial edge extraction: robust edge detection is achieved through Gaussian filtering to smooth the image, calculating gradient magnitude and direction, non-maximum suppression, and double thresholding.

[0039] The edges extracted by the Canny algorithm often contain a lot of redundancy. To optimize these redundant edges, a 7-neighborhood-based region growing strategy is further employed for edge screening and optimization: by analyzing the spatial continuity of edge pixels, redundant line segments are removed, while edge features with structural significance are retained. Specifically, the algorithm starts from a seed point p(x,y) that meets the pixel intensity threshold and iteratively searches within its 7-neighborhood—prioritizing the current edge extension direction, followed by adjacent directions, to ensure edge continuity and avoid pixel duplication. When multiple neighboring edge points that meet the criteria are detected, the algorithm creates a new search branch and dynamically adjusts its direction. Each branch independently maintains its own set of edge points until it can no longer extend. Finally, fragmented regions with fewer than the threshold number of pixels are removed, thus preserving the complete edge structure.

[0040] After extracting high-quality edges, a gridded gradient-guided strategy is employed to select representative feature points for each edge. First, the Sobel operator is used to calculate the horizontal and vertical gradients of the edge points to determine their magnitudes. Then, the image is divided into a fixed-size grid. Within each grid, several significant edge points are selected as key features by sorting the gradient magnitudes from highest to lowest. Next, an optical flow method combining reverse optical flow and gradient direction cosine constraints is used to track these edge features, eliminating unreliable tracking results. Specifically, for each feature point, forward tracking is performed first, followed by reverse tracking. If the position returned by the reverse tracking deviates significantly from the original position, it indicates that the feature point's motion is unstable or has been disturbed. Simultaneously, the cosine similarity of the gradient direction of the feature point in consecutive frames is calculated to determine if its gradient direction remains consistent. Plant swaying often leads to significant changes in local edge motion and gradient direction; these two constraints effectively eliminate dynamic edge points caused by plant swaying, thus ensuring the accuracy and stability of the tracking.

[0041] Specifically, Figure 3 The diagram illustrates the edge feature extraction process of the SLAM method provided in the embodiments of this application. Figure 4 This is a schematic diagram of the edge features traced by optical flow using the SLAM method provided in the embodiments of this application.

[0042] IMU Pre-integration IMU pre-integration involves pre-integrating IMU accelerometer and gyroscope measurements between two frames to obtain incremental information on relative rotation, velocity, and position, which is then fused with visual information. The pre-integration results can be directly used to update the position, velocity, and rotation states in the world coordinate system.

[0043] Encoder data Encoders are primarily used to acquire robot motion information and combine it with IMU data for filtering, thereby obtaining more accurate self-sensing odometry. The way encoders process data differs depending on the type of robot. For wheeled robots, encoders can directly measure the angular velocity of the wheels, converting it into the robot's linear and angular velocities through a differential drive model, thus acquiring the robot's motion information. For legged robots, encoders record joint angle information; through forward kinematics modeling, these joint angles can be converted into the velocity and position of the foot, thus acquiring the robot's motion information. This observational data can be used to estimate the robot's motion state and fused with IMU data to correct IMU drift, thereby improving odometry accuracy.

[0044] initialization Initialization employs a loosely coupled vision and IMU scheme: First, the pose of all frames and the 3D positions of all landmarks within the sliding window are determined using SFM (Structure of Motion Recovery). These are then matched with IMU pre-integrated values ​​to solve for gravity direction, scale factor, gyroscope bias, and the corresponding velocity for each frame. After initialization, the robot's position, velocity, and rotation state in the world coordinate system calculated by IMU pre-integration can be fused with encoder observation data to obtain an estimate of the self-sensing odometry. The embodiments of this application employ an Invariant Extended Kalman Filter (InEKF) to achieve the fusion of IMU and encoder data. The purpose of InEKF is to represent the state variables on a manifold suitable for the problem and to perform state estimation using an update rule matching this manifold.

[0045] Nonlinear optimization In the nonlinear optimization, a sliding window optimization method is employed to optimize the system state variables within each sliding window. Embodiments of this application further introduce edge features and use InEKF-estimated self-sensing odometry. Furthermore, a kernel function robust to dynamic feature points is added to the visual reprojection error term. The state vector includes the camera state within the sliding window (including position, attitude, velocity, acceleration, accelerometer bias, and gyroscope bias), the camera-IMU extrinsic parameters, the inverse depth of SuperPoint feature points, and the depth of edge feature points.

[0046] Then, an optimization objective function is constructed with the goal of minimizing the sum of residuals; the residual term includes prior information residuals, IMU measurement residuals, SuperPoint residuals, edge point residuals, and InEKF residuals. Finally, the optimization objective function is iteratively calculated using the Ceres Solver optimization library and the LM (Levenberg-Marquardt) algorithm. An adaptive kernel function strategy is used to dynamically reduce the residual weights corresponding to mismatched features to obtain the optimal solution of the state vector. Based on the optimal solution of the state vector, the pose estimation results and map point cloud are obtained.

[0047] Experimental verification To evaluate the effectiveness of the method in this application, it is compared with three mainstream visual SLAM algorithms—mainly ORB-SLAM3, VINS-Fusion, and Openvins—based on two core metrics: absolute trajectory error (ATE) and relative pose error (RPE). Experimental data uses the under-canopy agriculture dataset provided by the University of Illinois, which contains 135 multi-sensor sequences from corn and soybean fields. The main data acquisition device is the TerraSentia agricultural robot platform, providing synchronized data from a binocular camera (global shutter, 640×480@30Hz), IMU (200Hz), RTK-GPS (centimeter-level accuracy), and wheel encoders, covering the entire crop growth cycle and including various complex environmental changes such as light variations, shading, weeds, and weather conditions.

[0048] Four maize field sequences at different growth stages were selected from this dataset for the experiment. Scenario 1 shows maize in the seedling stage, where the plants are relatively short and the soil area between rows is relatively large. There are many weeds in the field, and the plant leaves are semi-erect. These leaves sway periodically due to wind, causing interference with dynamic features. Scenario 2 shows maize in the jointing stage. At this stage, the maize plant height is significantly higher than in the seedling stage, the row spacing gradually decreases, and the visual passage is partially obscured by the plants in some areas. At the same time, due to the increased number and softer texture of the leaves at this stage, there is also a certain degree of interference with dynamic features. Scenario 3 shows maize in the tasseling stage. After entering the tasseling stage, the maize plants have grown to a relatively high height, forming a dense canopy. At this time, the occlusion of the maize leaves causes most of the area within the camera's field of view to be covered, making the visual information sparse and uneven, especially under conditions of drastic light changes. Occlusion and uneven light are the main environmental characteristics of the tasseling stage. Scenario 4 shows maize in the mature stage. At this stage, the plant leaves begin to turn yellow and droop. The drooping leaves reduced the visual occlusion during the heading stage, but since the leaves turned yellow and no longer remained green, the green mask was no longer applicable. At this stage, edge features were extracted directly based on grayscale images.

[0049] Figure 5The paper presents the absolute trajectory error (ATE) and relative pose error (RPE) of various algorithms evaluated using the evo tool in four scenarios. The evaluation metrics are the root mean square error (RMSE) of ATE and RPE. ATE mainly reflects the global deviation between the algorithm's estimated trajectory and the actual trajectory, and can reflect the overall accuracy of the algorithm in large-scale localization; while RPE focuses on the differences between consecutive frames, reflecting the continuity and local smoothness of the algorithm's tracking on a short time scale.

[0050] In Scenario 1, due to the short plants, numerous weeds, and significant dynamic interference from leaves, pure vision algorithms such as ORB-SLAM3 exhibited large errors in both global (ATE 22.0936) and local (RPE 0.3738) local accuracy, indicating their greater sensitivity to dynamic interference. In contrast, VINS-Fusion, OpenVINS, and the method of this embodiment all achieved significant reductions in ATE and RPE, demonstrating that IMU assistance effectively reduces errors. However, OpenVINS had a relatively high ATE (8.0687) in this scenario, indicating that its global positioning accuracy was lower than other fusion algorithms when facing information interference. The method of this embodiment achieved the lowest ATE (1.8113) and the lowest RPE (0.1565), representing reductions of 38% and 8% respectively compared to the suboptimal algorithm.

[0051] In scenario 2, the corn plant height increases significantly, but some dynamic interference still exists. In this scenario, the ORB-SLAM3 algorithm, due to its ability to extract more plant feature points, shows a reduction in both ATE and RPE compared to scenario 1, but its values ​​are still higher than other algorithms. Compared to VINS-Fusion, the method in this embodiment only reduces ATE by approximately 0.0360 (approximately 1.4%) and RPE by approximately 0.0050 (approximately 2%), a relatively small improvement. This indicates that in this scenario, dynamic interference is somewhat alleviated compared to the previous scenario, and the performance of each fusion method is similar, but the method in this embodiment still maintains a slight advantage.

[0052] In scenario 3, severe occlusion and lighting variations lead to sparse visual information, significantly impacting image feature-dependent algorithms. While ORB-SLAM3 appears to have a respectable RPE (0.2259) in local continuity, its global error (ATE of 16.7431) is significantly high. In contrast, the methods of VINS-Fusion, OpenVINS, and the embodiments of this application can simultaneously control both global error (2.5557, 3.9039, and 2.2299, respectively) and local error (all close to 0.2285), indicating that even with severe occlusion, fusing information from other sensors can maintain consistency between the overall trajectory and local motion estimation. The ATE of the method in the embodiments of this application is 12.8% better than the suboptimal VINS-Fusion.

[0053] In scenario 4, the corn has entered the ripening stage, and the shading problem is somewhat alleviated compared to scenario 3, but there is still a large amount of uneven lighting. ORB-SLAM3 experienced trajectory tracking loss, losing the first half of the trajectory, resulting in overall performance instability. The ATE for the remaining part is 4.0964, while the local RPE increases significantly to 0.5998, indicating that there are large fluctuations even when some data is available. While VINS-Fusion, OpenVINS, and the method of this application embodiment achieved closer values ​​in terms of global and local errors, the method of this application embodiment still achieved the lowest ATE (2.0119) and RPE (0.2253), both of which are 6.8% lower than the second-best VINS-Fusion.

[0054] In summary, the SLAM algorithm presented in this application integrates self-sensing odometry, effectively addressing challenges such as dynamic foliage interference, visual occlusion, and lighting changes in agricultural environments. Through a multi-sensor information fusion strategy, it significantly improves the robustness and accuracy of trajectory estimation in complex scenarios. In multi-scenario experiments at different growth stages in cornfields, the algorithm's absolute trajectory error (ATE) and relative pose error (RPE) are superior to mainstream algorithms such as ORB-SLAM3 and VINS-Fusion, providing a reliable solution for autonomous navigation of agricultural robots.

[0055] Specifically, the robot simultaneous localization and mapping (SLAM) method for use under cornfield canopy according to embodiments of this application includes the following steps: S10. Acquire environmental images of the cornfield, IMU data of the robot, and encoder data.

[0056] The sensor and data inputs include three categories: environmental images of the cornfield captured by the camera, IMU (Inertial Measurement Unit) data from the robot, and encoder data. Optionally, the resolution of the environmental images is 640×480@30Hz. Optionally, the sampling rate of the IMU data is 200Hz, and the IMU data includes acceleration and angular velocity.

[0057] S20. Perform a first preprocessing on the environmental image to obtain visual data, which includes core feature point pairs and static edge feature sets.

[0058] In some embodiments, the first preprocessing of the environmental image to obtain visual data includes: S210. Perform feature extraction and matching processing on the environmental image to obtain core feature point pairs; S220. Perform edge feature supplementation processing on the environmental image to obtain a static edge feature set.

[0059] Optionally, the environmental image is processed by a SuperPoint self-supervised network to extract core feature points, and the descriptors of the core feature points are at least 128-dimensional. S211. The core feature points of different frames are matched using the LightGlue network to obtain core feature point pairs.

[0060] In the above embodiments, the SuperPoint (Self-Supervised Interest Point Detection and Description) self-supervised network is used to extract core feature points, and combined with the LightGlue (Local Feature Matching at Light Speed) network to achieve stable matching. SuperPoint requires no manual annotation, can process full-size images in real time, and can still capture effective features under strong light and shadow. LightGlue balances matching accuracy and efficiency through a lightweight classifier, solving the matching confusion problem caused by the repetition of corn leaf texture. By using the combination of SuperPoint and LightGlue to replace traditional methods for feature extraction and matching processing, the feature sparsity problem caused by the repetitive texture of cornfields is effectively solved, and the continuity of matching trajectory is improved.

[0061] Optionally, the descriptor of the core feature point is 256-dimensional.

[0062] In some embodiments, the edge feature supplementation processing of the environmental image to obtain a static edge feature set includes: S221. Perform HSV color segmentation on the environmental image to obtain a first image; S222. Perform Canny edge processing on the first image to obtain the second image; S223. Perform region growing processing on the second image to obtain the third image; S224. Perform optical flow tracing and gradient consistency constraint processing on the third image to obtain a static edge feature set.

[0063] In the above embodiment, the corn region is segmented by generating a green mask through HSV color segmentation, the initial edge is extracted by the Canny algorithm, redundant edges are optimized by neighborhood region growth, and finally stable edges are screened by optical flow tracking and gradient consistency constraints (to remove dynamic points). This effectively improves the visual odometry constraint capability and alleviates the feature sparsity problem under the canopy.

[0064] Optionally, the region growing process is a 7-neighbor region growing.

[0065] Optionally, the step of performing HSV color segmentation on the environmental image to obtain the first image further includes: If the number of features in the first image is less than a first threshold, then the environmental image is processed into a grayscale image and used as the first image.

[0066] S30. Perform a second preprocessing on the IMU data to obtain inter-frame motion increments; update the world coordinate system state values ​​based on the inter-frame motion increments, the world coordinate system state values ​​including position, attitude and velocity.

[0067] Optionally, the second preprocessing is IMU pre-integration, and the inter-frame motion increment includes relative rotation increment, velocity increment, and position increment.

[0068] S40. Perform a third preprocessing on the encoder data to obtain motion data.

[0069] In some embodiments, the third preprocessing of the encoder data to obtain motion data includes: For wheeled robots, the encoder data is converted into robot motion data based on a differential drive model, and the motion data includes linear velocity and angular velocity.

[0070] In some embodiments, the third preprocessing of the encoder data to obtain motion data includes: For legged robots, the encoder data is converted into motion data of the robot based on a forward kinematics model, and the motion data includes the position and velocity of the end of the foot.

[0071] In the above embodiment, the relative rotation increment, velocity increment, and position increment between frames are obtained through IMU pre-integration. The encoder data is converted into robot motion observations. The two are fused by InEKF to construct a self-sensing odometry to avoid trajectory interruption when vision is lost.

[0072] S50. Perform visual SFM processing based on the core feature point pairs to obtain the acquisition posture and landmark position of each frame of environmental image.

[0073] S60. Based on the attitude of each frame of environmental image acquisition, the position of the landmark point and the inter-frame motion increment, perform visual-inertial joint processing to obtain the InEKF initial state; the InEKF initial state includes gravity direction, scale factor, gyroscope deviation and velocity corresponding to each frame of environmental image.

[0074] In the above embodiment, the pose of the frame and the 3D position of the landmark point within the sliding window are determined by visual SFM (Structure from Motion), and then the IMU pre-integration results are matched to determine parameters such as gravity direction and scale factor as the initial state of InEKF.

[0075] S70. Based on the world coordinate system state value, the motion data and the InEKF initial state, perform InEKF fusion to construct a self-sensing odometry.

[0076] In the above embodiments, InEKF represents the robot state as a group of SE2(3), updates the state through observation equations, maintains trajectory estimation when visual occlusion occurs, and seamlessly connects after visual recovery. Compared with single visual SLAM, it significantly reduces the positioning error during the corn ear stage (dense canopy).

[0077] S80. Based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry, nonlinear optimization processing is performed to obtain pose estimation results and map point clouds.

[0078] In some embodiments, the nonlinear optimization processing based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry to obtain pose estimation results and map point clouds includes: Define a state vector and construct an optimization objective function with the goal of minimizing the sum of residuals; The optimization objective function is iteratively calculated using the Ceres Solver optimization library and the LM (Levenberg-Marquardt) algorithm. An adaptive kernel function strategy is used to dynamically reduce the residual weights corresponding to mismatched features, thereby obtaining the optimal solution of the state vector. Based on the optimal solution of the state vector, the pose estimation result and map point cloud are obtained.

[0079] Optionally, the state vector includes camera-IMU motion and bias state, camera-IMU extrinsic parameters, inverse depth of core feature points, and depth of static edge feature points; The camera-IMU motion and bias state includes the world coordinate system state value, IMU accelerometer zero bias, and IMU gyroscope zero bias; The camera-IMU extrinsic parameters include the position difference and attitude difference of the camera relative to the IMU; The residuals include marginalized prior residuals, IMU pre-integration residuals, SuperPoint residuals, marginal feature residuals, and InEKF residuals; the InEKF residuals are determined based on the self-sensing odometry estimate and the corresponding predicted value in the state vector.

[0080] Among them, the camera-IMU motion and bias state is used to correct sensor drift, and it is done frame by frame within a sliding window, which can be a 10-frame sliding window; the camera-IMU extrinsic parameters are used to unify the coordinate system of visual and inertial data, avoid viewpoint misalignment, and perform minor global optimization; the inverse depth of core feature points converts 2D pixels into 3D landmarks to construct a sparse map of the cornfield, and optimizes it point by point; the depth of static edge feature points supplements the feature constraints under the canopy, avoids trajectory interruption caused by visual loss, and optimizes it point by point.

[0081] In the above embodiments, by introducing edge feature residuals and InEKF residuals, adaptive weights are added to the visual residuals, and the interference of dynamic blades and light changes is suppressed through Ceres Solver and LM algorithm iteration.

[0082] Through the above-described technical means, the embodiments of this application can achieve the following technical effects: (1) By using SuperPoint+Lightglue to replace the traditional feature extraction and matching method, significant feature points can be effectively extracted under the corn canopy and stable and accurate feature matching can be achieved, providing a more reliable feature basis for the SLAM system.

[0083] (2) The introduction of edge features that can describe the outline of corn plants as a supplement makes up for the lack of comprehensive feature information obtained by SuperPoint in this scenario and enriches the overall feature information.

[0084] (3) By fusing the odometer data of the self-sensing sensor, when visual information is lost due to factors such as leaf occlusion, the fused data can maintain the trajectory estimation of the system; when the visual features are restored, the system can achieve seamless reset and continue to operate.

[0085] (4) Strong environmental adaptability. To address the dynamic leaf, light changes, and canopy shading issues at different growth stages of maize (from seedling to maturity), the algorithm achieves better ATE and RPE than mainstream SLAM algorithms in various scenarios through multi-feature fusion and multi-sensor redundancy. In particular, it can maintain a high ATE RMSE in dense canopy scenarios during the ear stage.

[0086] (5) Low deployment cost. Based on ordinary cameras, low-cost IMU and encoder, it does not require expensive equipment such as LiDAR, reducing hardware costs by more than 50%, and does not require pre-trained semantic datasets, adapting to different corn varieties in different regions and having strong generalization ability.

[0087] While this application has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of this application. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed without departing from the spirit and scope of this application as defined by the appended claims. It should be understood that different dependent claims and features described herein can be combined in ways different from those described in the original claims. It is also understood that features described in conjunction with individual embodiments can be used in other described embodiments.

Claims

1. A method for simultaneous localization and mapping of robots under the canopy of cornfields, characterized in that, include: Acquire environmental images of the cornfield, IMU data of the robot, and encoder data; The environmental image is subjected to a first preprocessing to obtain visual data, which includes core feature point pairs and static edge feature sets; The IMU data is subjected to a second preprocessing step to obtain the inter-frame motion increment; The world coordinate system state value is updated based on the inter-frame motion increment, and the world coordinate system state value includes position, attitude and velocity; The encoder data is subjected to a third preprocessing step to obtain motion data; Visual SFM processing is performed based on the core feature point pairs to obtain the acquisition pose and landmark position of each frame of environmental image. Based on the posture of each frame of environmental image acquisition, the position of the landmark point, and the inter-frame motion increment, visual-inertial joint processing is performed to obtain the InEKF initial state; The InEKF initial state includes gravity direction, scale factor, gyroscope bias, and velocity corresponding to each frame of the environmental image; A self-sensing odometry is constructed by fusing the world coordinate system state value, the motion data, and the InEKF initial state. Based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry, nonlinear optimization processing is performed to obtain pose estimation results and map point clouds.

2. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 1, characterized in that, The first preprocessing of the environmental image to obtain visual data includes: The environmental image is subjected to feature extraction and matching processing to obtain core feature point pairs; The environmental image is subjected to edge feature supplementation processing to obtain a static edge feature set.

3. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 2, characterized in that, The step of performing feature extraction and matching processing on the environmental image to obtain core feature point pairs includes: The environmental image is processed by a SuperPoint self-supervised network to extract core feature points, and the descriptors of the core feature points are at least 128-dimensional. The core feature points of different frames are matched using the LightGlue network to obtain core feature point pairs.

4. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 2, characterized in that, The process of performing edge feature supplementation on the environmental image to obtain a static edge feature set includes: The environmental image is segmented using HSV color to obtain a first image; The first image is processed using Canny edge processing to obtain the second image; The second image is subjected to region growing processing to obtain the third image; Optical flow tracing and gradient consistency constraint processing are performed on the third image to obtain a static edge feature set.

5. The method for simultaneous robot localization and mapping under the canopy of a cornfield according to claim 4, characterized in that, The step of performing HSV color segmentation on the environmental image to obtain the first image further includes: If the number of features in the first image is less than a first threshold, then the environmental image is processed into a grayscale image and used as the first image.

6. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 1, characterized in that, The second preprocessing is IMU pre-integration, and the inter-frame motion increment includes relative rotation increment, velocity increment, and position increment.

7. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 1, characterized in that, The third preprocessing of the encoder data to obtain motion data includes: For wheeled robots, the encoder data is converted into robot motion data based on a differential drive model, and the motion data includes linear velocity and angular velocity.

8. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 1, characterized in that, The third preprocessing of the encoder data to obtain motion data includes: For legged robots, the encoder data is converted into motion data of the robot based on a forward kinematics model, and the motion data includes the position and velocity of the end of the foot.

9. The method for synchronous localization and mapping of robots under the canopy of cornfields according to claim 1, characterized in that, The process of performing nonlinear optimization based on the visual data, the inter-frame motion increment, the motion data, and the self-sensing odometry to obtain pose estimation results and map point clouds includes: Define a state vector and construct an optimization objective function with the goal of minimizing the sum of residuals; The optimization objective function is iteratively calculated using the Ceres Solver optimization library and the LM algorithm. An adaptive kernel function strategy is used to dynamically reduce the residual weights corresponding to mismatched features, thereby obtaining the optimal solution of the state vector. Based on the optimal solution of the state vector, the pose estimation result and map point cloud are obtained.

10. The method for simultaneous robot localization and mapping under the canopy of a cornfield according to claim 9, characterized in that, The state vector includes camera-IMU motion and deviation state, camera-IMU extrinsic parameters, inverse depth of core feature points, and depth of static edge feature points; The camera-IMU motion and bias status includes the world coordinate system state value, IMU accelerometer zero bias, and IMU gyroscope zero bias; The camera-IMU extrinsic parameters include the position difference and attitude difference of the camera relative to the IMU; The residuals include marginalized prior residuals, IMU pre-integration residuals, SuperPoint residuals, marginal feature residuals, and InEKF residuals; the InEKF residuals are determined based on the self-sensing odometry estimate and the corresponding predicted value in the state vector.

Images