Hilly terrain sugarcane farm machine multi-sensor online self-calibration method and system

Abstract

The application discloses a hilly terrain sugarcane agricultural machine multi-sensor online self-calibration method and system, comprising: S1, multi-source operation perception data acquisition; S2, multi-source perception consistency error model construction based on operation path feedback; S3, dynamic error discrimination based on path deviation characteristics; S4, internal and external parameter collaborative online updating; S5, path perception correction driven by calibration results; S6, online closed-loop iterative operation and safety control. Through the autonomous operation process of the hilly terrain sugarcane harvester, a multi-dimensional error model integrating geometry, path and operation state consistency is constructed, and feedback and multi-time scale dynamic discrimination are carried out based on operation path execution deviation, so that online perception and collaborative optimization updating of sensor parameter drift are realized without relying on artificial targets, the long-term accuracy and self-adaptive maintenance capability of multi-sensor calibration parameters in the hilly dynamic environment are improved, and the problem of decline of fusion perception quality caused by changes in the position and posture of the sensor and parameter drift is solved.

Description

Technical Field

[0001] This invention relates to a method and system for online self-calibration of multiple sensors for sugarcane agricultural machinery in hilly terrain, belonging to the field of intelligent agricultural equipment technology. Background Technology

[0002] With the continuous improvement of agricultural mechanization and intelligence, sugarcane harvesting operations are gradually developing towards automation and autonomy. Given the undulating terrain and narrow plots characteristic of hilly and mountainous areas in southern my country, miniaturized and intelligent sugarcane harvesters have become an important development direction. In such terrain conditions, harvesters need to integrate multiple sensors, such as lidar, vision sensors, and inertial measurement units, to achieve real-time perception of the environment, terrain, crop rows, and their own posture, providing data support for autonomous operation. This makes multi-sensor fusion perception crucial for achieving autonomous operation.

[0003] However, when operating in hilly terrain, the machine continuously undergoes attitude changes such as roll and pitch, accompanied by vibration, torsional deformation, and load fluctuations, causing dynamic changes in the relative pose relationships between multiple sensors. This makes it difficult for sensor parameters calibrated in a static environment to remain accurate over a long period of time, and their sensing errors are amplified in complex terrain, directly affecting the accuracy of operation path planning and tracking, leading to path deviation, frequent corrections, or even operation interruption.

[0004] Existing multi-sensor calibration technologies are mainly concentrated in fields such as robotics and autonomous driving. Typical methods include offline calibration methods that rely on manually placed calibration objects, and targetless joint calibration methods that utilize natural geometric features. In the field of agricultural machinery, pre-calibrated multi-sensor fusion schemes are also used for environmental perception. However, these existing technologies generally have the following limitations: First, their calibration processes are mostly based on the static assumption that the sensor pose is fixed and unchanging, failing to fully consider the dynamic changes in sensor pose caused by walking on slopes and continuous vibrations in hilly operations, leading to the gradual invalidation of pre-calibrated parameters in actual operations; second, existing calibration methods mostly use geometric errors as optimization targets, lacking a perception consistency evaluation mechanism oriented towards the execution effect of the work path, and cannot effectively reflect the direct impact of perception errors on the quality of actual operations; finally, the calibration module is usually independent of the path planning and control module, making it difficult to timely and automatically trigger and complete the online discrimination and correction of sensor parameters based on the evolution characteristics of path tracking deviations during operations. Summary of the Invention

[0005] Purpose of the invention: To address the shortcomings of existing technologies, this invention provides a multi-sensor online self-calibration method and system for sugarcane harvesters in hilly terrain. This invention constructs a multi-dimensional error model that integrates geometry, path, and operational status consistency during the autonomous operation of sugarcane harvesters in hilly terrain. Based on the deviation of the operational path, feedback and dynamic discrimination at multiple time scales are performed, realizing online perception and collaborative optimization updates of sensor parameter drift without relying on manual targets, forming a closed-loop self-calibration system with deep integration of operation, perception, and calibration.

[0006] Technical Solution: Online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain, including the following steps:

[0007] S1. Multi-source operation perception data acquisition: Real-time acquisition of multi-source operation perception data during the autonomous operation of a small sugarcane harvester in hilly terrain;

[0008] S2. Construction of a multi-source perception consistency error model based on operation path feedback: Based on the multi-source operation perception data collected in step S1, the observation results of different sensors are uniformly mapped to the coordinate system of the small sugarcane harvester. Constraints caused by attitude changes and vibration interference caused by hilly terrain undulations, as well as geometric constraints caused by the center line, row direction or row spacing of sugarcane operation rows, are introduced to construct a multi-source perception consistency error model based on operation path feedback, so as to reduce the impact of perception instability on calibration results in complex hilly environments.

[0009] S3. Dynamic error discrimination based on path deviation characteristics: Based on the multi-source sensing consistency error model constructed in step S2, the path consistency error is analyzed at multiple time scales, and the path deviation caused by agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field, or adjusting across rows is distinguished from the potential calibration drift trend. Based on this, it is determined whether the external parameter relationship parameters of multiple sensors and the internal parameter parameters of individual sensors have drifted. If it is determined that a drift has occurred, proceed to step S4; if it is not determined that a drift has occurred, the current parameters are kept unchanged and proceed to step S6.

[0010] S4. Collaborative Online Update of Internal and External Parameters: Without the deployment of artificial calibration targets, the internal and external parameters of multiple sensors are collaboratively updated online, taking into account the constraints of hilly operation status and sugarcane row structure. Constraints based on prior knowledge are introduced during the parameter update process to limit the range and rate of change of parameter updates and prevent the optimization results from deviating from the physical reality of sensor installation relationships and mechanical structures.

[0011] S5. Calibration result-driven path awareness correction: The multi-sensor intrinsic and extrinsic parameters updated in step S4 are used to update the coordinate transformation and fusion relationship of the multi-source sensing data, correct the environmental modeling results and positioning results, and provide the corrected results to the operation path planning and path tracking process. Based on the corrected operation path feedback information, the calibration parameters are evaluated to see if they have reached a stable state. If the evaluation result shows that the calibration parameters have not reached a stable state, the process returns to step S3 to continue parameter drift discrimination. If the evaluation result shows that the calibration parameters have reached a stable state, the process proceeds to step S6.

[0012] S6. Online closed-loop iterative operation and safety control: During continuous operation, steps S1 to S5 are executed cyclically. When a parameter update is detected that causes a sudden increase in any consistency error item, or when the updated internal and external parameters exceed the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameters are rolled back to the most recent stable state, or a safe operation mode of freezing parameter updates or degrading operation is entered, forming a multi-sensor online self-calibration closed loop adapted to hilly terrain and sugarcane operation characteristics based on operation path feedback.

[0013] This invention constructs a multi-source sensing consistency error model based on operational path feedback during the autonomous operation of a sugarcane harvester in hilly terrain. It dynamically identifies sensor parameter drift based on multi-timescale analysis of path deviations, and then combines operational status and crop row structure constraints to collaboratively update and correct the internal and external parameters of multiple sensors online. This achieves an online self-calibration closed loop that does not rely on manual target calibration and can adapt to the dynamic hilly operating environment. It solves the technical problems of existing technologies, such as calibration parameters being prone to failure and unable to be corrected in a timely manner in hilly operations due to static calibration assumptions, lack of an operational performance-oriented evaluation mechanism, and the separation of calibration and operation control modules, thus affecting the accuracy of operational path planning and tracking.

[0014] Preferably, in order to provide a comprehensive, synchronous sensing data foundation with hilly terrain representation capabilities for subsequent error modeling and drift discrimination, the multi-source operational sensing data in S1 specifically includes:

[0015] Environmental point cloud data collected by lidar to characterize the spatial structure of the working environment;

[0016] Environmental perception data collected by visual sensors to characterize the geometric structure of sugarcane work rows;

[0017] The inertial measurement unit (IMU) collects body attitude data and motion state data to characterize the hilly terrain undulation characteristics. The body attitude data includes at least pitch angle and roll angle to characterize the hilly terrain undulation characteristics. The pitch angle and roll angle serve as direct quantitative indicators of hilly terrain undulation and are used to model and compensate for systematic errors caused by terrain undulation in subsequent consistency error modeling and parameter updates. The motion state data includes acceleration or angular velocity data to characterize the intensity of operational vibration.

[0018] The operation status data reflects the overall machine operating status during the operation. The multi-source operation sensing data is collected synchronously according to a fixed operation cycle, which is 50~200ms.

[0019] By explicitly collecting multi-source data, including environmental point clouds, visual features of sugarcane rows, and IMU attitude and motion states that directly quantify terrain undulations and vibrations, and setting a fixed synchronous acquisition cycle of 50~200ms, high-frequency and collaborative perception of the dynamic working environment and machine status in hilly areas was achieved, providing stable and reliable data input for online calibration.

[0020] In a preferred embodiment, to overcome the limitations of evaluating solely based on geometric error and to establish an evaluation system directly related to work performance, S2 specifically comprises:

[0021] The multi-source sensing consistency error model includes at least one of the following error terms or a combination thereof:

[0022] Geometric consistency error term The geometric consistency error is used to characterize the spatial consistency of observation results from different sensors in the same vehicle coordinate system. That is, the geometric consistency error is constructed based on the high-level geometric features of the sugarcane operation row centerline, row direction or row spacing extracted from the lidar point cloud and visual image, respectively, in order to reduce the sensitivity to local occlusion, vibration noise and single-frame perception anomalies.

[0023] Path consistency error term This is used to characterize the deviation between the actual operation path and the corresponding reference operation path, i.e., the path consistency error. It characterizes the systematic deviation of the actual operation trajectory relative to the reference trajectory of the sugarcane operation row caused by the changes in the external parameter relationship and the internal parameter drift of the multi-source sensor under continuous operation conditions in hilly terrain. Its short-term fluctuations mainly come from the terrain undulation and operation vibration, and its long-term variation trend is used to indicate the potential drift of the calibration parameters.

[0024] Job status consistency error term This is used to constrain the coordination between the sensing results and changes in the body's posture and operational status, i.e., the operational status consistency error term. Using pitch angle, roll angle and vibration intensity operation state parameters collected by inertial measurement unit as constraint variables, it is used to characterize the reasonable response relationship between the sensing results and changes in body attitude and operation state under hilly terrain undulation and harvesting operation vibration conditions, thereby suppressing the misjudgment of sensing changes caused by terrain undulation or operation vibration as sensor intrinsic and extrinsic parameter drift.

[0025] The specific formula is as follows:

[0026]

[0027] in, This represents the external parameter transformation relationship from the sensor coordinate system to the vehicle coordinate system. Indicates the first Sensors within a single work cycle Collected geometric feature points or point cloud sampling points;

[0028]

[0029] in, For the first The actual operation trajectory pose for each operation cycle is calculated by the fusion positioning of LiDAR, visual sensor, and IMU. This fusion positioning is based on the LiDAR ranging model, visual imaging model, and IMU error model, and is therefore related to the corresponding sensor intrinsic parameters. The reference pose for the corresponding operation path is generated based on the geometric structural features of the sugarcane operation row, that is, the reference path obtained by fitting the center line or row direction of the sugarcane operation row.

[0030]

[0031]

[0032]

[0033] in, For the first Path deviation per cycle These are mapping coefficients, used to characterize the impact of changes in operational status on changes in path deviation. This is the job state vector.

[0034] The path deviation is used to characterize the degree of deviation of the actual operation path from the reference operation path within the corresponding operation cycle. The path deviation includes at least one or more of lateral position deviation and heading deviation, and is used to reflect the systematic impact of changes in multi-sensor parameters on the execution effect of the operation path.

[0035] Among them, the path consistency error term As one of the feedback error items in the operation execution layer, it reflects the impact of environmental perception errors on the actual operation effect, and it is related to the geometric consistency error item. and the consistency error term of the operation status These together constitute the joint constraints for updating calibration parameters, rather than serving as a single calibration basis.

[0036] By constructing a fusion geometric consistency error Path consistency error Consistency error with operating status The multidimensional error model incorporates path execution deviation as a key feedback term, enabling a comprehensive and multidimensional quantitative evaluation of sensor parameter accuracy, from internal data consistency to the impact of external operational effects. This provides a joint constraint basis for the accurate identification and collaborative updating of parameter drift.

[0037] Preferably, in order to accurately capture the actual sensor parameter drift signal in a hilly environment filled with transient vibration interference and human operation, step S3 addresses the path consistency error. Multi-timescale analysis is performed to determine whether persistent drift occurs in the intrinsic and extrinsic parameters of multiple sensors, specifically including:

[0038] For path consistency error Short-term fluctuation filtering at the second-level timescale is performed to suppress transient disturbances caused by hilly terrain undulations and operational vibrations; the path consistency error after short-term fluctuation filtering is analyzed. Long-term trend analysis at the minute-level time scale is performed to characterize the persistent deviation caused by the drift of intrinsic and extrinsic parameters of multiple sensors; combined with operational status data, agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field, or adjusting across rows are identified, and the path deviation within the corresponding operation cycle is removed or its weight is reduced as agricultural operation deviation;

[0039] The path consistency error after the above processing If any of the following conditions are met within N consecutive work cycles, it is determined that there is a continuous drift in the multi-sensor parameters and the process proceeds to step S4:

[0040] The average increment is greater than the first threshold T1 or The fluctuation amplitude is greater than the second threshold T2, wherein N is 5~30, the first threshold T1 is 0.01~0.50, and the second threshold T2 is 0.05~1.00;

[0041] When path consistency error If a sudden increase occurs within a short period of time but falls back to the above threshold range within a preset recovery period, it is determined to be a transient disturbance and no parameter update is triggered.

[0042] By analyzing path consistency error By performing short-term fluctuation filtering at the second level and long-term trend analysis at the minute level, and combining it with the identification of specific operational behaviors to eliminate or reduce the weight of related deviations, it is possible to effectively separate transient or intentional disturbances such as terrain vibration and operational adjustments from the persistent and systematic drift trends of characterization calibration failures. This significantly improves the accuracy and reliability of parameter drift judgment and avoids false updates.

[0043] Preferably, in order to safely and robustly complete the multi-parameter collaborative optimization update under targetless conditions and adapt to the dynamic changes of external parameters in hilly operations, S4 specifically includes:

[0044] Without deploying manually calibrated targets, based on the discrimination results of step S3, a joint cost function for multi-source error collaborative constraints is constructed:

[0045]

[0046]

[0047]

[0048] in, For geometric consistency error term,

[0049] This is the path consistency error term.

[0050] This is the consistency error term for the work status.

[0051] For internal reference purposes.

[0052] As an external reference,

[0053] These are prior constraints, set based on the actual installation structure of the sensor on the sugarcane machinery in hilly terrain, the vibration characteristics of the entire machine, and the range of changes in its operating posture. They are used to suppress the excessive amplification of parameter updates caused by transient disturbances due to terrain undulations, ensuring that the calibration results conform to the physical feasibility under the operating conditions of the agricultural machinery.

[0054] These are the prior constraint weight coefficients, and their values ​​are greater than 0.

[0055] , For the physical reasonable upper and lower bounds of each parameter,

[0056] , , The corresponding adaptive weight coefficients are updated as follows: Calculate them separately within the sliding window. , , residual scaling parameters , , And update the weights according to the inverse relationship of the residual scale, specifically:

[0057] , ,

[0058] in, To prevent a preset positive number with a denominator of zero, the residual scaling parameter... The weighting coefficients are normalized to correspond to the root mean square value or standard deviation of the error term within the sliding window. ;

[0059] By minimizing the joint cost function J, the intrinsic and extrinsic parameters of the multi-sensor system are updated online in a coordinated manner. The extrinsic parameters are time-varying parameters that are equivalent to changes in the machine's attitude, structural elastic deformation, or vibration during operations in hilly terrain. The intrinsic parameters are parameters of the sensor's own imaging or ranging model, and include at least one or a combination of the following:

[0060] Visual sensor intrinsic parameters: focal length , Main point , and radial / tangential distortion coefficients , , , , ;

[0061] LiDAR intrinsic parameters: scan angle zero bias, range scale factor, range zero bias;

[0062] Inertial Measurement Unit (IMU) intrinsic parameters: accelerometer / gyroscope bias, scale factor, and inter-axis non-orthogonal parameters;

[0063] The intrinsic and extrinsic parameters together constitute a parameter vector. Minimize the joint cost function iteratively within the sliding window. Solving for parameter increments and according to Update the internal references online, and... Limiting the amplitude to suppress divergence;

[0064] The external parameter changes are implemented by introducing the external parameter as a variable to be estimated into the sliding window optimization framework. In each work cycle, the changes are based on the latest acquired work state vector. The extrinsic parameter variation model is calculated, and the corresponding time-varying extrinsic parameters are estimated and updated online by minimizing the joint cost function composed of the multi-source sensing consistency error.

[0065] By using the above error modeling and discrimination methods, calibration parameter updates are triggered only when systematic deviations caused by hilly terrain and sugarcane operating conditions occur, thereby avoiding unnecessary calibration updates caused by normal operating behavior or transient vibrations.

[0066] By constructing a joint cost function that integrates multi-source errors and physical prior constraints, adopting adaptive weights to balance the contributions of each error term, and modeling the extrinsic parameters as "time-varying parameters" driven by the operational state and then corrected by consistency errors, online collaborative estimation and updating of intrinsic and extrinsic parameters are achieved under strict physical boundaries and rate of change constraints. This effectively suppresses divergence and oscillation in the optimization process, ensures that the calibration results conform to the physical realizability of the mechanical structure, and avoids unnecessary calibration updates caused by normal operational behavior or transient vibrations.

[0067] Preferably, in order to timely and effectively transform the updated calibration parameters into more accurate sensing and positioning capabilities, thereby directly improving the quality of subsequent operations, S5 specifically includes:

[0068] The updated multi-sensor intrinsic and extrinsic parameters are used to update the coordinate transformation and fusion relationship of multi-source sensing data, thereby correcting the environmental modeling and positioning results. The corrected results are then provided to the operation path planning and path tracking process, ensuring that path generation and execution are based on the latest calibration parameters. The corrected environmental sensing results serve as the shared sensing base output of the entire machine's multiple modules. The coordinate transformation update process is as follows:

[0069] Reconstructing the coordinate transformation matrix after extrinsic parameter update, let the extrinsic parameters from sensor s to vehicle coordinate system b be... , Then in the first For each operation cycle, a homogeneous transformation matrix is ​​constructed based on the updated extrinsic parameters:

[0070]

[0071] Unified mapping of multi-source data coordinates for LiDAR point cloud points :

[0072]

[0073] For visual sensor image feature points: use the updated camera intrinsic parameter matrix and distortion coefficient Perform distortion correction and normalization:

[0074]

[0075] in, Indicates the visual sensor in the first The first detected in the first work cycle The original pixel coordinates of each image feature point are combined with extrinsic parameters to unify the corresponding 3D features into the vehicle coordinate system or the world coordinate system.

[0076] For IMU data: Correct the original measurements using updated IMU intrinsic parameters, namely the zero bias and scaling factor.

[0077]

[0078] in, The corrected angular velocity, With zero bias at angular velocity, is the scale factor for angular velocity;

[0079]

[0080] in, For the corrected acceleration, For zero bias of acceleration, is the scale factor for acceleration.

[0081] The fusion observation model is reparameterized as follows:

[0082] The fusion observation model is reparameterized by reparameterizing the observation models of each sensor in the fusion system with updated intrinsic and extrinsic parameters, and the observation function is expressed as:

[0083]

[0084] in, From external references and internal reference, i.e., camera Radar scale / angle zero offset and IMU offset / scale are jointly determined. This is the observation error term, representing the measurement noise or uncertainty of sensor S at time k;

[0085] Update the weights of each sensor based on the updated residual statistics:

[0086]

[0087] The environmental modeling and localization correction process is as follows:

[0088] Within a sliding window, the pose and map are recalculated. The fused data from the most recent M operation cycles are replayed, and the pose trajectory is re-estimated using the updated intrinsic and extrinsic parameters. Environmental Map Output the corrected positioning results:

[0089]

[0090] Where, r s Let m be the residual function. By minimizing the sum of squares of the residuals from the LiDAR, camera, and IMU, the corrected pose trajectory and environment map are obtained, and the corrected environment map m is output accordingly. new The revised environment map includes: a new 3D environment point cloud map; a new sugarcane row centerline, row direction, and row spacing structure; a new vehicle positioning trajectory; and new reference path generation base information.

[0091] By re-performing coordinate transformation, sensor data correction, and fusion model reparameterization using updated intrinsic and extrinsic parameters, and by performing pose and map re-estimation within a sliding window on recent historical data, the system achieves real-time correction and global consistency improvement of environmental modeling, vehicle positioning, and sugarcane row structure perception results. This provides the path planning and tracking module with perception base information based on the latest and most accurate calibration status, thereby directly improving the accuracy and stability of autonomous operations.

[0092] Preferably, to ensure the stability and security of the entire online self-calibration closed-loop system during long-term operation and to prevent system performance degradation due to erroneous updates, abnormal interference, or optimization failures, S6 specifically includes:

[0093] During the online iteration process, when step S3 determines that there is a persistent drift in the multi-sensor parameters, the joint cost function is used. The rate of change controls the timing of parameter updates;

[0094]

[0095] The trigger threshold E of the joint cost function change ΔJ th E is a preset positive value, and its value is determined based on the stability of the operating environment and the system noise level. th Take a value of 0.05 to 0.50.

[0096] when Exceeding the preset trigger threshold E within N consecutive work cycles th An update is triggered when N is an integer from 5 to 30; otherwise, the current internal and external parameters remain unchanged to avoid frequent updates and ensure real-time performance.

[0097] When any error term in the joint cost function is detected to have a sudden increase in a single or continuous work cycle, or when the updated parameter exceeds the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameter is rolled back to the most recent stable state, or the safe work mode of freezing parameter updates or degrading operation is entered.

[0098] By setting update trigger conditions based on the rate of change of the joint cost function, and defining anomaly detection and alarm mechanisms for increased error abruptness and parameters exceeding the physical reasonable range, coupled with safety control strategies using parameter rollback, update freezing, or mode degradation, real-time monitoring and autonomous fault tolerance of the calibration process itself are achieved, ensuring the robustness and physical controllability of the online calibration system in complex hilly operating environments.

[0099] A system for implementing online self-calibration of multiple sensors for sugarcane agricultural machinery in hilly terrain includes a lidar, a vision sensor, an inertial measurement unit (IMU), an operation status perception module, a perception consistency evaluation module based on operation path feedback, a dynamic error discrimination module, an internal and external parameter collaborative online update module, and a path perception correction module driven by calibration results.

[0100] The lidar is used to acquire environmental point cloud data that characterizes the spatial structure of the working environment.

[0101] The visual sensor is used to characterize environmental perception data that represents the geometric structural features of sugarcane work rows.

[0102] The inertial measurement unit (IMU) is used to acquire body attitude data and motion state data that characterize the undulation characteristics of hilly terrain.

[0103] The operation status sensing module is used to acquire operation status data that reflects the overall machine operation status;

[0104] The perception consistency evaluation module is used to construct a multi-source perception consistency error model by combining the execution deviation of the job path;

[0105] The dynamic error discrimination module is used to perform second-level fluctuation filtering and minute-level trend analysis on path consistency error, distinguish between vibration and operational behavior deviation and calibration drift, and then determine whether the external parameters of multiple sensors and the internal parameters of a single sensor have drifted.

[0106] The internal and external parameter collaborative online update module is used to collaboratively update the internal and external parameter parameters of multiple sensors online under the condition of no manual target calibration, combining the hilly operation status and operation row structure constraints and introducing prior amplitude and speed limits;

[0107] The calibration result-driven path perception correction module is used to apply the updated intrinsic and extrinsic parameters to correct the environmental perception results and provide corrected perception information for subsequent operation path planning and path tracking.

[0108] By integrating specific sensing hardware such as lidar, vision sensors, and IMU, and constructing a dedicated processing module that includes sensing consistency evaluation, dynamic error discrimination, online parameter updates, and path perception correction, a complete embedded system has been realized, from multi-source data acquisition to error modeling and analysis and online parameter calibration, and then to real-time correction of sensing results. This provides dedicated hardware and software support for the continuous, reliable, and high-precision autonomous operation of sugarcane agricultural machinery in hilly terrain.

[0109] Beneficial Effects: This invention constructs a multi-dimensional error model that integrates geometry, path, and operational status consistency during the autonomous operation of sugarcane harvesters in hilly terrain. Based on operational path deviations, it provides feedback and dynamic discrimination across multiple time scales, achieving online sensing and collaborative optimization of sensor parameter drift without relying on manual targets. This forms a closed-loop self-calibration system with deep fusion of operation, sensing, and calibration, effectively improving the long-term accuracy and adaptive maintenance capability of multi-sensor calibration parameters in dynamic hilly environments. It also mitigates the degradation in fusion sensing quality caused by sensor pose changes and parameter drift, directly enhancing the accuracy and stability of path tracking and autonomous operation. Furthermore, by introducing multi-source constraint joint optimization, physical prior amplitude limiting, and anomaly safety control mechanisms, it effectively suppresses erroneous updates and optimization divergence under complex disturbances, ensuring the reliability and robustness of the online calibration process. This meets the urgent requirements of sugarcane harvesters in hilly terrain for high-precision, high-reliability autonomous operation of multi-sensor fusion systems under continuous vibration, attitude changes, and long-term operation conditions. Attached Figure Description

[0110] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0111] Figure 1 This is a flowchart of the method of the present invention;

[0112] Figure 2 This is a schematic diagram illustrating the availability determination of the job path consistency error of the present invention and its introduction into perceived consistency evaluation.

[0113] Figure 3 This is a schematic diagram of the closed loop of online updating and sensing correction of internal and external parameters under the multi-source consistency error collaborative constraint of the present invention;

[0114] Figure 4This is a schematic diagram illustrating the extraction of the centerline of the sugarcane operation row from the lidar point cloud and visual image, as well as the geometric consistency error in the vehicle coordinate system, according to the present invention. Detailed Implementation

[0115] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0116] In the description of this invention, it should be understood that the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0117] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can include direct contact between the first and second features, or contact between the first and second features through another feature between them. Furthermore, "above," "over," and "on top" of the second feature includes the first feature directly above or diagonally above the second feature, or simply indicates that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature includes the first feature directly below or diagonally below the second feature, or simply indicates that the first feature is at a lower horizontal level than the second feature.

[0118] like Figures 1-4 As shown, the online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain includes the following steps:

[0119] S1. Multi-source operation perception data acquisition: Real-time acquisition of multi-source operation perception data during the autonomous operation of a small sugarcane harvester in hilly terrain;

[0120] The multi-source operation sensing data in S1 specifically includes:

[0121] Environmental point cloud data collected by lidar to characterize the spatial structure of the working environment;

[0122] Environmental perception data collected by visual sensors to characterize the geometric structure of sugarcane work rows;

[0123] The inertial measurement unit (IMU) collects body attitude data and motion state data to characterize the hilly terrain undulation characteristics. The body attitude data includes at least pitch angle and roll angle to characterize the hilly terrain undulation characteristics. The pitch angle and roll angle serve as direct quantitative indicators of hilly terrain undulation and are used to model and compensate for systematic errors caused by terrain undulation in subsequent consistency error modeling and parameter updates. The motion state data includes acceleration or angular velocity data to characterize the intensity of operational vibration.

[0124] The operation status data reflects the overall machine operating status during the operation. The multi-source operation sensing data is collected synchronously according to a fixed operation cycle, which is 50~200ms, preferably 100ms.

[0125] S2. Construction of a multi-source perception consistency error model based on operation path feedback: Based on the multi-source operation perception data collected in step S1, the observation results of different sensors are uniformly mapped to the coordinate system of the small sugarcane harvester. Constraints caused by attitude changes and vibration interference caused by hilly terrain undulations, as well as geometric constraints caused by the center line, row direction or row spacing of sugarcane operation rows, are introduced to construct a multi-source perception consistency error model based on operation path feedback, so as to reduce the impact of perception instability on calibration results in complex hilly environments.

[0126] Specifically, S2 is:

[0127] The multi-source sensing consistency error model includes at least one of the following error terms or a combination thereof:

[0128] Geometric consistency error term This is used to characterize the spatial consistency of observation results from different sensors within the same vehicle coordinate system. Specifically, the geometric consistency error is constructed based on high-level geometric features extracted from lidar point clouds and visual images, such as the centerline, direction, or spacing of sugarcane planting rows, to reduce sensitivity to local occlusion, vibration noise, and single-frame perception anomalies. Figure 4 As shown, the point cloud is fitted after ground filtering and clustering to obtain the center line of the LiDAR line; the image is detected, segmented, and 3D projected to obtain the visual center line; the alignment error of the two center lines in the vehicle coordinate system is characterized by the lateral distance error Δd and the orientation angle error Δθ. Where Δd: the lateral distance error (line spacing error) at the same forward position, and the line spacing is equal to the distance between the left and right center lines.

[0129] Path consistency error term This is used to characterize the deviation between the actual operation path and the corresponding reference operation path, i.e., the path consistency error. It characterizes the systematic deviation of the actual operation trajectory relative to the reference trajectory of the sugarcane operation row caused by the changes in the external parameter relationship and the internal parameter drift of the multi-source sensor under continuous operation conditions in hilly terrain. Its short-term fluctuations mainly come from the terrain undulation and operation vibration, and its long-term variation trend is used to indicate the potential drift of the calibration parameters.

[0130] Job status consistency error term This is used to constrain the coordination between the sensing results and changes in the body's posture and operational status, i.e., the operational status consistency error term. Using pitch angle, roll angle and vibration intensity operation state parameters collected by inertial measurement unit as constraint variables, it is used to characterize the reasonable response relationship between the sensing results and changes in body attitude and operation state under hilly terrain undulation and harvesting operation vibration conditions, thereby suppressing the misjudgment of sensing changes caused by terrain undulation or operation vibration as sensor intrinsic and extrinsic parameter drift.

[0131] The specific formula is as follows:

[0132]

[0133] in, This represents the external parameter transformation relationship from the sensor coordinate system to the vehicle coordinate system. Indicates the first Sensors within a single work cycle Collected geometric feature points or point cloud sampling points;

[0134]

[0135] in, For the first The actual operation trajectory pose for each operation cycle is calculated by the fusion positioning of LiDAR, visual sensor, and IMU. This fusion positioning is based on the LiDAR ranging model, visual imaging model, and IMU error model, and is therefore related to the corresponding sensor intrinsic parameters. The reference pose for the corresponding operation path is generated based on the geometric structural features of the sugarcane operation row, that is, the reference path obtained by fitting the center line or row direction of the sugarcane operation row.

[0136]

[0137]

[0138]

[0139] in, For the first Path deviation per cycle These are mapping coefficients, used to characterize the impact of changes in operational status on changes in path deviation. This is the job state vector.

[0140] The path deviation is used to characterize the degree of deviation of the actual operation path from the reference operation path within the corresponding operation cycle. The path deviation includes at least one or more of lateral position deviation and heading deviation, and is used to reflect the systematic impact of changes in multi-sensor parameters on the execution effect of the operation path.

[0141] The mapping coefficients This represents the mapping coefficient between changes in operational status and changes in path deviation. It is used to characterize the equivalent path offset effect produced by changes in vehicle attitude or vibration in a multi-source sensing system. Its physical meaning is that changes in pitch and roll angles caused by hilly terrain, or angular velocity and acceleration disturbances caused by high-frequency vibrations, will be projected into the vehicle's sensing coordinate system in different ways through differences in sensor installation position and angle, thus introducing offsets of varying magnitudes into the perceived operational path.

[0142] Among them, the path consistency error term As one of the feedback error items in the operation execution layer, it reflects the impact of environmental perception errors on the actual operation effect, and it is related to the geometric consistency error item. and the consistency error term of the operation status These together constitute the joint constraints for updating calibration parameters, rather than serving as a single calibration basis.

[0143] The method for introducing constraints based on attitude changes and vibration disturbances caused by hilly terrain undulations is as follows:

[0144] Attitude change constraint introduction method based on IMU attitude output: Utilizing the pitch and roll angles output in real-time by the IMU as quantitative descriptions of hilly terrain undulations, and establishing a functional relationship between the attitude angles and the spatial geometric features corresponding to the observation results of each sensor, this method introduces attitude change constraints, including:

[0145] In the geometric consistency error term Introducing attitude compensation terms:

[0146] In the process of projecting the LiDAR point cloud and visual feature points onto the vehicle coordinate system, a compensation matrix is ​​constructed using the IMU attitude angle:

[0147] ,

[0148] And Introduced into the coordinate transformation chain, this prevents instantaneous changes in the body's attitude caused by terrain undulations from being mistaken for sensor extrinsic drift.

[0149] Construct the attitude response term in the attitude dynamic consistency constraint Es:

[0150] Based on the IMU's attitude change rate and direction, the observation changes from multiple sensors within the same period should satisfy a reasonable attitude response relationship. For example, when the pitch angle increases, the changes in the horizon position in the visual image and the tilt angle of the radar ground point cloud should be consistent with the IMU trend. If these two changes are inconsistent, it will lead to an increase in Es, which will be identified as a potential calibration offset during optimization. This method ensures that the system will not misjudge real attitude changes caused by terrain as sensor installation relationship drift.

[0151] Dynamic disturbance suppression based on vibration intensity and introduction of noise-related constraints: In hilly areas, vibration frequencies are high and amplitudes are large. To avoid random offsets caused by vibration being mistaken for calibration drift, this invention introduces vibration disturbance constraints to achieve the following modeling method:

[0152] Vibration intensity index constructed using IMU acceleration / angular velocity :

[0153] ,

[0154] And add a vibration-related weighting function to the error model:

[0155] ,

[0156] The stronger the vibration, the lower the instantaneous residual weights of the geometric consistency error Eg and the path consistency error Ep;

[0157] Introduce vibration suppression filtering into the path consistency error Ep:

[0158] For path deviation fluctuations caused by high-frequency vibrations, an adaptive filter driven by vibration intensity is used to subtract transient fluctuations, so that the long-term trend of path deviation can reflect the changes in real external parameters.

[0159] Constructing the vibration-observation coupling relationship as an additional constraint:

[0160] For vibration-related features such as LiDAR scan line jitter and visual image blurring, the following coupling consistency is introduced:

[0161] If the vibration intensity surges, a short-term increase in observation noise (Eg, Ep rises briefly) is allowed, but a sustained shift is not permitted; if the observation error remains shifted after the vibration intensity stabilizes, it is determined to be extrinsic parameter drift rather than vibration noise.

[0162] This constraint guarantees that random disturbances caused by vibration will not trigger erroneous updates, but that persistent increases in error caused by drift can be reliably identified.

[0163] S3. Dynamic error discrimination based on path deviation characteristics: Based on the multi-source sensing consistency error model constructed in step S2, the path consistency error is analyzed at multiple time scales, and the path deviation caused by agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field, or adjusting across rows is distinguished from the potential calibration drift trend. Based on this, it is determined whether the external parameter relationship parameters of multiple sensors and the internal parameter parameters of individual sensors have drifted. If it is determined that a drift has occurred, proceed to step S4; if it is not determined that a drift has occurred, the current parameters are kept unchanged and proceed to step S6.

[0164] S3 addresses path consistency error. Multi-timescale analysis is performed to determine whether persistent drift occurs in the intrinsic and extrinsic parameters of multiple sensors, specifically including:

[0165] For path consistency error Short-term fluctuation filtering at the second-level timescale is performed to suppress transient disturbances caused by hilly terrain undulations and operational vibrations; the path consistency error after short-term fluctuation filtering is analyzed. Long-term trend analysis at the minute-level time scale is performed to characterize the persistent deviation caused by the drift of intrinsic and extrinsic parameters of multiple sensors; combined with operational status data, agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field, or adjusting across rows are identified, and the path deviation within the corresponding operation cycle is removed or its weight is reduced as agricultural operation deviation;

[0166] The path consistency error after the above processing If any of the following conditions are met within N consecutive work cycles, it is determined that there is a continuous drift in the multi-sensor parameters and the process proceeds to step S4:

[0167] The average increment is greater than the first threshold T1 or The fluctuation amplitude is greater than the second threshold T2, wherein N is 5~30, the first threshold T1 is 0.01~0.50, and the second threshold T2 is 0.05~1.00;

[0168] When path consistency error If a sudden increase occurs within a short period of time but falls back to the above threshold range within a preset recovery period, it is determined to be a transient disturbance and no parameter update is triggered.

[0169] The short-time window length is defined as Corresponding number of work cycles .

[0170] A sudden increase is defined as a situation where the amplitude of a jump significantly exceeds the normal fluctuation within a short time window. A sudden increase can be identified if the following conditions are met: ,in , .

[0171] The recovery period is defined as The corresponding number of work cycles is .

[0172] Path consistency error In the multi-timescale analysis, "short-term fluctuation filtering at the second-level timescale" and "long-term trend analysis at the minute-level timescale" are key steps designed by this invention to adapt to hilly operation scenarios. The specific implementation of short-term fluctuation filtering and the discrimination method of long-term trend analysis are not general filtering operations, but are specially constructed for the vibration characteristics of hilly terrain and the drift characteristics of multiple sensors of sugarcane processing machines. Therefore, they are specifically disclosed in the specification.

[0173] Short-term fluctuation filtering on a second-level timescale: caused by hilly terrain undulations and operational vibrations. High-frequency transient fluctuations may occur; therefore, this invention employs a first-order exponential moving average (EMA) filter as a means of suppressing short-term fluctuations. Its core calculation formula and parameters are as follows:

[0174]

[0175] in, This represents the path consistency error after filtering. The filtering smoothing coefficient is set to 0.05-0.25 (corresponding to a time constant of 1-5 seconds). Reason for selection: EMA is calculated recursively, suitable for agricultural machinery operating cycles of 50-200 ms; it has low sensitivity to high-frequency noise caused by mechanical vibration; it does not introduce excessive phase lag, thus not masking the true drift trend; and it is simple to implement in engineering, making it the mainstream method for mobile platforms.

[0176] To enhance the suppression of high-frequency vibrations, a three-point or five-point median filter can be optionally superimposed: The median filter window is set to 3-5 points to further remove isolated spikes caused by vibration and impact.

[0177] Long-term trend analysis on a minute-level timescale: The error sequence after short-time filtering is denoted as: In order to distinguish between transient terrain disturbances and continuous drift, this invention uses a sliding window linear trend estimation method for analysis.

[0178] Build length is Trend analysis window, window length: ,correspond One job cycle (50-200ms cycle).

[0179] Fit the trend slope using the least squares method: The slope is obtained from the window data. : Represents the trend of error over time, intercept : Indicates the starting value of this trend line when k=0.

[0180] Root mean square fluctuation : Represents the fluctuations around the trend.

[0181] Trend judgment rule: When ( )and ( ) is determined to be persistent drift; when or It was determined to be a non-drift type fluctuation caused by topographic relief or vibration.

[0182] Agricultural operational behaviors (detours, turns, and cross-rows) are excluded or downweighted: In long-term trend analysis, to avoid misjudging short-term trajectory deviations caused by behaviors such as detouring obstacles, turning at the edge of the field, and cross-row adjustments as drifts, this invention adopts the following logic:

[0183] Behavior recognition: Identify operational behavior based on one or a combination of the following features: rate of change of heading angle (Turning point); Lateral offset Row spacing (cross-row adjustment); LiDAR can detect breaks or obstacles in the crop row structure (obstacle avoidance).

[0184] Cycle-based behavior elimination or de-weighting: Cycles identified as operational behaviors meet the following criteria: ( ), i.e., elimination (weight=0): not used for drift detection. Reducing weight (weight) ): Reduce its impact on trend judgment.

[0185] The logic and threshold for distinguishing between agricultural operation behavior path deviation and calibration drift are as follows:

[0186] In the path consistency error analysis process, a multi-condition differentiation logic based on job behavior recognition, path deviation time sequence characteristics, and job status correlation is introduced:

[0187] The path deviation differentiation logic based on operation behavior recognition identifies agricultural operation behavior types based on operation status data and path geometric features. Specifically, this includes: obstacle avoidance behavior (marked as an obstacle avoidance segment when the path curvature suddenly increases within a short period and an obstacle or discontinuous inter-row structure is detected simultaneously); and field-end turning behavior (when the rate of change of the operation path heading angle exceeds a first heading threshold). And the path curvature is consistently greater than the set curvature threshold. The duration exceeds the preset minimum turning duration When the lateral deviation of the path exceeds the preset proportional threshold of the sugarcane row spacing (e.g., 0.5 to 1.5 times the row spacing) within a short period of time, and is accompanied by a decrease in operating speed or an increase in lateral control input, it is determined to be a cross-row adjustment behavior. Path deviations generated within the above-mentioned behavior sections are all marked as operation behavior deviation sections and are not directly used as calibration drift criteria.

[0188] Differentiation logic based on path deviation time characteristics: for path consistency error Evolutionary characteristics over time series are analyzed to distinguish between operational behavior deviations and calibration drift through short-term and long-term features: Short-term abrupt change characteristic determination (operational behavior deviation): If the path consistency error suddenly increases within a single or a few operational cycles, and one of the following conditions is met, it is determined to be an operational behavior deviation: Path consistency error increment Greater than the second threshold However, the subsequent period will not exceed Within each work cycle, the error returned to the baseline range; the path consistency error exhibited a "sudden rise-fall" pattern within a short window, without forming a monotonic cumulative trend. Among these, Take a value of 0.05-0.30. Take 3-10 operating cycles. Long-term cumulative characteristic determination (calibration drift trend): If in continuous... Within a single operation cycle, if the path consistency error meets any of the following conditions, it is considered a potential calibration drift: the mean path consistency error exhibits a continuous monotonically increasing trend, and its growth rate exceeds the first threshold. The fluctuation amplitude of the path consistency error consistently exceeds the third threshold. And there is no corresponding task behavior marker. Take 5-30, Take a value of 0.01-0.1 per cycle. Take a value between 0.05 and 1.00.

[0189] Auxiliary discrimination logic based on job status correlation: To further improve the reliability of differentiation, this invention introduces correlation analysis between path deviation and job status changes: Constructing a path consistency error sequence and a job status vector. Correlation coefficients between parameters including pitch angle, roll angle, vibration intensity, or operating load. When path deviation changes are highly correlated with pitch angle, roll angle, or vibration intensity. When the deviation is primarily caused by terrain undulations or operational behavior, it is considered not to be a basis for calibration drift. However, when the path deviation continues to accumulate even with small changes in operational status (below the operational status change threshold), it is considered difficult to explain by operational behavior and is judged as a calibration drift risk. Take a value of 0.6-0.9.

[0190] Based on the multi-dimensional discrimination logic described above, online updates of multi-sensor intrinsic or extrinsic parameters are triggered when the path consistency error simultaneously meets the following conditions: the path deviation does not belong to an identified agricultural operation segment; the path consistency error shows a continuous accumulation or high-amplitude fluctuation trend within a long-term window; the correlation between the path deviation and changes in operation status is lower than the correlation threshold; and the deviation recurs in multiple similar operation segments with consistent offset directions. Otherwise, the path deviation is determined to be a normal agricultural operation or a transient disturbance caused by hilly terrain, and calibration parameter updates are not triggered.

[0191] When the path consistency error Ep shows a monotonically increasing or high-frequency fluctuating state over multiple consecutive work cycles, and is not significantly correlated with changes in work status, it is determined that there is a continuous drift in the multi-sensor parameters. The correlation can be determined by threshold judgment, statistical feature analysis, or empirical rules. When the error suddenly increases in a short period of time but then recovers quickly, it is determined to be a transient disturbance and parameter updates are not triggered.

[0192] The statistical feature analysis and discrimination are as follows: within the sliding window time... Inside, definition For N consecutive periods, the path consistency error sequence Calculate its mean change rate and volatility characteristics, which are defined as follows:

[0193] , ,

[0194] Meanwhile, based on the job state vector (Including pitch angle, roll angle, vibration intensity, or work load parameters), calculate the correlation coefficient between path consistency error and work status:

[0195] ,

[0196] When one of the following conditions is met within N consecutive work cycles, it is determined that the change in path consistency error is mainly caused by the drift of multi-sensor intrinsic or extrinsic parameters: (1) the mean of path consistency error It shows a continuous monotonic increase, and its growth rate exceeds the first threshold; (2) Path consistency error fluctuation amplitude The correlation coefficient of changes in work status exceeding the second threshold. Less than the preset correlation threshold.

[0197] If the above statistical characteristics increase abnormally within a short window but recover to the threshold range in subsequent windows, it is determined to be a transient disturbance caused by hilly terrain undulations or operational vibrations.

[0198] The empirical rule judgment is as follows: Based on the engineering experience of sugarcane harvesting operations in hilly terrain, the evolution of path consistency error and changes in operation status are jointly judged, specifically including: (1) When the path consistency error increases synchronously in the operation section where the pitch angle or roll angle of the machine changes significantly, and decreases after the machine attitude returns to stability, it is determined that the error change is mainly caused by the undulation of hilly terrain and does not trigger parameter updates; (2) When the path consistency error changes significantly during agricultural operations such as turning at the edge of the field, bypassing obstacles, or adjusting across rows, and falls back quickly within the preset recovery period after the operation ends, it is determined to be a path deviation caused by the operation and is not used as a basis for calibration drift; (3) When the path consistency error gradually accumulates and increases in the continuous straight operation section, and still maintains an increasing trend when the change in operation status parameters is small, it is determined that the path deviation is difficult to explain by normal operation behavior and is considered to have the risk of multi-sensor parameter drift; (4) When the path consistency error repeatedly shows a consistent offset trend under multiple similar operation postures and operation status conditions, it is determined to be a systematic deviation and triggers online updates of multi-sensor intrinsic or extrinsic parameters first.

[0199] S4. Collaborative Online Update of Internal and External Parameters: Without the deployment of artificial calibration targets, the internal and external parameters of multiple sensors are collaboratively updated online, taking into account the constraints of hilly operation status and sugarcane row structure. Constraints based on prior knowledge are introduced during the parameter update process to limit the range and rate of change of parameter updates and prevent the optimization results from deviating from the physical reality of sensor installation relationships and mechanical structures.

[0200] Specifically, S4 is:

[0201] Without deploying manually calibrated targets, based on the discrimination results of step S3, a joint cost function for multi-source error collaborative constraints is constructed:

[0202]

[0203]

[0204]

[0205] in, For geometric consistency error term,

[0206] This is the path consistency error term.

[0207] This is the consistency error term for the work status.

[0208] For internal reference purposes.

[0209] As an external reference,

[0210] These are prior constraints, set based on the actual installation structure of the sensor on the sugarcane machinery in hilly terrain, the vibration characteristics of the entire machine, and the range of changes in its operating posture. They are used to suppress the excessive amplification of parameter updates caused by transient disturbances due to terrain undulations, ensuring that the calibration results conform to the physical feasibility under the operating conditions of the agricultural machinery.

[0211] The values ​​of the prior constraints are determined by a combination of three types of prior knowledge: First, based on the actual installation structure of the sensor on a sugarcane harvester in hilly terrain, the allowable offset range of external parameters translation and rotation is obtained through actual measurements of the sensor bracket mounting holes, mounting surface normal deviation, assembly reference surface, and positioning pins. The typical translation offset range is set to ±5 mm to ±30 mm, and the typical rotation offset range is set to ±0.2° to ±3°. Second, based on the attitude changes and vibration characteristics of the entire machine in a hilly operating environment, the pitch angle, roll angle, and acceleration collected by the IMU in the stable driving section are statistically analyzed to obtain the attitude change amplitude and vibration intensity within the operating cycle. This determines the upper bound of the equivalent change in external parameters and the speed limit threshold for parameter updates, where the maximum translational update amount of external parameters in a single cycle is limited to 0.1 mm to 3 mm. The maximum update amount for a single-cycle rotation is limited to 0.01° to 0.3°. Thirdly, based on the nominal specifications and factory calibration parameters of various sensors, the feasible value range of the intrinsic parameters is determined. Among them, the allowable offset of intrinsic parameters such as camera focal length and principal point position is typically set to 0.1% to 3% of the nominal value, and the distortion coefficient drift is limited to ±0.0005 to ±0.01. The allowable offset of the distance zero bias of the lidar is limited to ±2 mm to ±30 mm, and the allowable offset of the scanning angle zero bias is limited to ±0.05° to ±1°. The allowable variation range of the zero bias of the IMU accelerometer and gyroscope is set to ±2 to ±20 times the noise density corresponding to the factory nominal zero bias. The aforementioned prior knowledge forms the physical range of the parameters that can be realized. Through amplitude and speed constraints, the parameter update amount is strictly limited to the above range, so that the update process will not be excessively amplified by transient attitude disturbances or high-frequency vibrations caused by hilly undulations. This ensures that the entire online self-calibration process maintains stability, controllability, and physical rationality in engineering.

[0212] These are the prior constraint weight coefficients, and their values ​​are greater than 0.

[0213] , For the physical reasonable upper and lower bounds of each parameter,

[0214] , , The corresponding adaptive weight coefficients are updated as follows: Calculate them separately within the sliding window. , , residual scaling parameters , , And update the weights according to the inverse relationship of the residual scale, specifically:

[0215] , ,

[0216] in, To prevent a preset positive number with a denominator of zero, the residual scaling parameter... The weighting coefficients are normalized to correspond to the root mean square value or standard deviation of the error term within the sliding window. ;

[0217] By minimizing the joint cost function J, the intrinsic and extrinsic parameters of the multi-sensor system are updated online in a coordinated manner. The extrinsic parameters are time-varying parameters that are equivalent to changes in the machine's attitude, structural elastic deformation, or vibration during operations in hilly terrain. The intrinsic parameters are parameters of the sensor's own imaging or ranging model, and include at least one or a combination of the following:

[0218] Visual sensor intrinsic parameters: focal length , Main point , and radial / tangential distortion coefficients , , , , ;

[0219] LiDAR intrinsic parameters: scan angle zero bias, range scale factor, range zero bias;

[0220] Inertial Measurement Unit (IMU) intrinsic parameters: accelerometer / gyroscope bias, scale factor, and inter-axis non-orthogonal parameters;

[0221] The intrinsic and extrinsic parameters together constitute a parameter vector. Minimize the joint cost function iteratively within the sliding window. Solving for parameter increments and according to Update the internal references online, and... Limiting the amplitude to suppress divergence;

[0222] The minimization of the joint cost function J is solved using a nonlinear least squares iterative optimization method based on the Gauss-Newton method. The Gauss-Newton method uses the Jacobian matrix of each residual term in the cost function as approximate second-order information, and iteratively updates the parameters by constructing the following incremental equation:

[0223]

[0224] in, The Jacobian matrix of the joint residual vector with respect to the parameter vector. The residual vector for all error terms. This represents the parameter increment to be determined.

[0225] The reasons for choosing the Gauss-Newton method are as follows: The joint cost function J constructed in this invention belongs to the standard nonlinear least squares form, consisting of geometric consistency residuals, path consistency residuals, and operational state consistency residuals. The residual terms can all be used to calculate the first derivative with respect to the parameter vector, and its structure fully meets the applicable conditions of the Gauss-Newton method. The Gauss-Newton method has a fast local convergence speed and low computational complexity, suitable for the 50–200 ms operation cycle requirements of sugarcane harvesters, ensuring that online optimization can complete iterative calculations within each cycle. The Gauss-Newton method performs best under the condition that the noise approximately follows a Gaussian distribution, highly matching the statistical characteristics of lidar ranging noise, IMU noise, and visual imaging errors, thus ensuring update accuracy and convergence stability. Because the intrinsic and extrinsic parameters of this invention have low dimensions (generally in the range of 6–20 dimensions), the Gauss-Newton method does not require constructing an accurate Hessian during solution; high-precision results can be obtained only through an approximate second-order structure, achieving efficient computation and low resource consumption, making it very suitable for embedded or vehicle-mounted controller deployment.

[0226] The intrinsic and extrinsic parameters together constitute the parameter vector θ = [θ in ,θ ex The parameter increment ∆θ is solved by iteratively minimizing the joint cost function J(θ) within a sliding window. The parameter vector is solved using a sliding window optimization framework, with the window size set to the nearest... Data from one job cycle. Considering the actual sampling frequency of 50–200 ms for this invention's job cycle, this window length corresponds to a data time span of approximately 1–12 seconds.

[0227] The reasons for choosing a window size of 20-60 are as follows:

[0228] Sufficient to cover the typical periodic changes of hilly terrain undulations and operational vibrations: Through field statistics of IMU pitch angle, roll angle and vibration intensity, it can be seen that the dominant frequency components of hilly undulations and operational vibrations generally fall around 0.1 to 1 Hz; using a window of 1 to 12 seconds can completely include one or more attitude change cycles, so that the equivalent change trend of external parameters can be accurately identified.

[0229] It can capture the cross-cycle consistency relationship between multiple source sensors: the refresh cycle of lidar point cloud is usually 5-20 Hz; the feature extraction cycle of visual sensor is 10-30 Hz; and IMU data can reach 100-200 Hz. By using the most recent 20-60 operation cycles (1-12 s) as a sliding window, it can ensure that the window contains at least: multiple frames of lidar point cloud; a stable and continuous visual feature sequence; and sufficient density of IMU attitude integral data, so that the multi-sensor coupling residual can be fully optimized.

[0230] The balance between computational complexity and the real-time performance of the vehicle controller: Too small a window (e.g., < 10 cycles) will lead to insufficient information and unstable optimization solutions; too large a window (e.g., > 100 cycles) will significantly increase computational complexity and make it impossible to complete an iteration within the 50-200ms real-time cycle of agricultural operations.

[0231] Matching the timescale of parameter drift: The equivalent changes in multi-sensor extrinsic parameters are usually caused by attitude changes, structural elastic deformation, or vibration averaging effects, and their rate of change is much lower than the sensor sampling frequency; using a 1-12 second window can effectively capture the "slow drift" trend without being overly affected by transient vibrations.

[0232] The external parameter changes are implemented by introducing the external parameter as a variable to be estimated into the sliding window optimization framework. In each work cycle, the changes are based on the latest acquired work state vector. The extrinsic parameter variation model is calculated, and the corresponding time-varying extrinsic parameters are estimated and updated online by minimizing the joint cost function composed of the multi-source sensing consistency error.

[0233] The extrinsic parameter variation model is based on the physical fact that attitude changes, vibrations, and loads during hilly operations cause small equivalent pose offsets between the sensor bracket and the vehicle body. Therefore, the extrinsic parameters are modeled as incremental parameters driven by the operational state.

[0234] In the Read the job status vector in each job cycle. (Pitch, roll, vibration intensity, load, etc.), using a linear sensitivity matrix Mapping it to an extrinsic parameter increment and limiting its amplitude and rate, we obtain the prediction update. ,in Subsequently, within the sliding window, using this prediction as a priori, the multi-source consensus joint cost function is minimized. Solve for the correction amount The final external parameters were updated to This enables an online estimation process that combines "state-driven prediction with consistency error correction".

[0235] The time-varying extrinsic parameters refer to the fact that the sensor's mounting posture relative to the vehicle body is not strictly constant during actual operation, but rather undergoes small, continuous changes over time due to factors such as vehicle pitch, roll, vibration, structural elastic deformation, and changes in workload. Therefore, the extrinsic parameters are no longer considered fixed constants, but are modeled as time-dependent parameters that are continuously updated with the operation cycle and can reflect changes in the sensor's equivalent mounting posture in real time, in order to more accurately describe the dynamic geometric relationship between the sensor and the vehicle body during operation.

[0236] By using the above error modeling and discrimination methods, calibration parameter updates are triggered only when systematic deviations caused by hilly terrain and sugarcane operating conditions occur, thereby avoiding unnecessary calibration updates caused by normal operating behavior or transient vibrations.

[0237] S5. Calibration result-driven path awareness correction: The multi-sensor intrinsic and extrinsic parameters updated in step S4 are used to update the coordinate transformation and fusion relationship of the multi-source sensing data, correct the environmental modeling results and positioning results, and provide the corrected results to the operation path planning and path tracking process. Based on the corrected operation path feedback information, the calibration parameters are evaluated to see if they have reached a stable state. If the evaluation result shows that the calibration parameters have not reached a stable state, the process returns to step S3 to continue parameter drift discrimination. If the evaluation result shows that the calibration parameters have reached a stable state, the process proceeds to step S6.

[0238] Specifically, S5 is:

[0239] The updated multi-sensor intrinsic and extrinsic parameters are used to update the coordinate transformation and fusion relationship of multi-source sensing data, thereby correcting the environmental modeling and positioning results. The corrected results are then provided to the operation path planning and path tracking process, ensuring that path generation and execution are based on the latest calibration parameters. The corrected environmental sensing results serve as the shared sensing base output of the entire machine's multiple modules. The coordinate transformation update process is as follows:

[0240] Reconstructing the coordinate transformation matrix after extrinsic parameter update, let the extrinsic parameters from sensor s to vehicle coordinate system b be... , Then in the first For each operation cycle, a homogeneous transformation matrix is ​​constructed based on the updated extrinsic parameters:

[0241]

[0242] Unified mapping of multi-source data coordinates for LiDAR point cloud points :

[0243]

[0244] For visual sensor image feature points: use the updated camera intrinsic parameter matrix and distortion coefficient Perform distortion correction and normalization:

[0245]

[0246] in, Indicates the visual sensor in the first The first detected in the first work cycle The original pixel coordinates of each image feature point are used, and the corresponding 3D features are unified to the vehicle coordinate system or the world coordinate system by combining extrinsic parameters.

[0247] For IMU data: Correct the original measurements using updated IMU intrinsic parameters, i.e., zero bias or scale.

[0248]

[0249] in, The corrected angular velocity, With zero bias at angular velocity, is the scale factor for angular velocity;

[0250]

[0251] in, For the corrected acceleration, For zero bias of acceleration, is the scale factor for acceleration.

[0252] The fusion observation model is reparameterized as follows:

[0253] The fusion observation model is reparameterized by reparameterizing the observation models of each sensor in the fusion system with updated intrinsic and extrinsic parameters, and the observation function is expressed as:

[0254]

[0255] in, From external references and internal reference, i.e., camera Radar scale / angle zero offset and IMU offset / scale are jointly determined. This is the observation error term, representing the measurement noise or uncertainty of sensor S at time k;

[0256] Update the weights of each sensor based on the updated residual statistics:

[0257]

[0258] The environmental modeling and localization correction process is as follows:

[0259] Within a sliding window, the pose and map are recalculated. The fused data from the most recent M operation cycles are replayed, and the pose trajectory is re-estimated using the updated intrinsic and extrinsic parameters. Environmental Map Output the corrected positioning results:

[0260]

[0261] Where, r s Let m be the residual function. By minimizing the sum of squares of the residuals from the LiDAR, camera, and IMU, the corrected pose trajectory and environment map are obtained, and the corrected environment map m is output accordingly. new The revised environment map includes: a new 3D environment point cloud map; a new sugarcane row centerline, row direction, and row spacing structure; a new vehicle positioning trajectory; and new reference path generation base information.

[0262] S6. Online closed-loop iterative operation and safety control: During continuous operation, steps S1 to S5 are executed cyclically. When a parameter update is detected that causes a sudden increase in any consistency error item, or when the updated internal and external parameters exceed the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameters are rolled back to the most recent stable state, or a safe operation mode of freezing parameter updates or degrading operation is entered, forming a multi-sensor online self-calibration closed loop adapted to hilly terrain and sugarcane operation characteristics based on operation path feedback.

[0263] Specifically, S6 is:

[0264] During the online iteration process, when step S3 determines that there is a persistent drift in the multi-sensor parameters, the joint cost function is used. The rate of change controls the timing of parameter updates;

[0265]

[0266] The trigger threshold E of the joint cost function change ΔJ th E is a preset positive value, and its value is determined based on the stability of the operating environment and the system noise level. th Take a value of 0.05 to 0.50.

[0267] when Exceeding the preset trigger threshold E within N consecutive work cycles th An update is triggered when N is an integer from 5 to 30; otherwise, the current internal and external parameters remain unchanged to avoid frequent updates and ensure real-time performance.

[0268] When any error term in the joint cost function is detected to have a sudden increase in a single or continuous work cycle, or when the updated parameter exceeds the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameter is rolled back to the most recent stable state, or the safe work mode of freezing parameter updates or degrading operation is entered.

[0269] The abrupt increase refers to the instantaneous change in the error term exceeding its statistical fluctuation range under normal hilly working conditions within a single or a few consecutive work cycles. Specifically, it is measured by the mean error within the sliding window. with standard deviation As a baseline, when a certain consistency error term is in the th... When a work cycle satisfies the following formula, it is considered a sudden increase:

[0270]

[0271]

[0272] in, This is the error value for the current work cycle. This represents the sudden increase in value relative to the previous cycle. The mutation detection threshold is represented by the coefficient. Based on the system noise level and hilly vibration characteristics, a value of 2-5 is selected to cover approximately 95%-99% of error fluctuations under normal operating conditions. When a sudden increase continuously occurs... One cycle If the sudden increase exceeds a preset proportion of the upper limit of the error allowable (e.g., 1.5 to 3 times the normal range), it is determined that the error has suddenly increased.

[0273] The preset physical reasonable range includes the feasible value range of intrinsic and extrinsic parameters and their maximum update amplitude per cycle. Specifically: the extrinsic parameter translation parameter is limited to ±5 mm to ±30 mm, and the extrinsic parameter rotation parameter is limited to ±0.2° to ±3°; the maximum update step size of the extrinsic parameter translation in a single operation cycle is limited to 0.1 mm to 3 mm, and the maximum update step size of the extrinsic parameter rotation is limited to 0.01° to 0.3°. The allowable variation range of camera intrinsic parameters (including focal length, principal point coordinates, and distortion coefficient) is limited to 0.1% to 3% of the nominal value; the allowable variation ranges of lidar range offset and scale factor are limited to ±2 mm to ±30 mm and 10⁻ mm, respectively. 5~10⁻²; The allowable variation range of IMU zero bias and scale factor is limited to ±2 to ±20 times the noise density corresponding to the factory-specified zero bias. When any internal or external parameter after online update exceeds the above-mentioned physically reasonable range, or its single-cycle change exceeds the corresponding maximum update step size, it is determined to be an abnormal parameter update, triggering an abnormal alarm and parameter rollback mechanism, or entering a safe mode of frozen update / degraded operation, to ensure the physical controllability and stability of the online self-calibration process under hilly operating conditions.

[0274] A system for implementing online self-calibration of multiple sensors for sugarcane agricultural machinery in hilly terrain includes a lidar, a vision sensor, an inertial measurement unit (IMU), an operation status perception module, a perception consistency evaluation module based on operation path feedback, a dynamic error discrimination module, an internal and external parameter collaborative online update module, and a path perception correction module driven by calibration results.

[0275] The lidar is used to acquire environmental point cloud data that characterizes the spatial structure of the working environment.

[0276] The visual sensor is used to characterize environmental perception data that represents the geometric structural features of sugarcane work rows.

[0277] The inertial measurement unit (IMU) is used to acquire body attitude data and motion state data that characterize the undulation characteristics of hilly terrain.

[0278] The operation status sensing module is used to acquire operation status data that reflects the overall machine operation status;

[0279] The perception consistency evaluation module is used to construct a multi-source perception consistency error model by combining the execution deviation of the job path;

[0280] The dynamic error discrimination module is used to perform second-level fluctuation filtering and minute-level trend analysis on path consistency error, distinguish between vibration and operational behavior deviation and calibration drift, and then determine whether the external parameters of multiple sensors and the internal parameters of a single sensor have drifted.

[0281] The internal and external parameter collaborative online update module is used to collaboratively update the internal and external parameter parameters of multiple sensors online under the condition of no manual target calibration, combining the hilly operation status and operation row structure constraints and introducing prior amplitude and speed limits;

[0282] The calibration result-driven path perception correction module is used to apply the updated intrinsic and extrinsic parameters to correct the environmental perception results and provide corrected perception information for subsequent operation path planning and path tracking.

[0283] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0284] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A multi-sensor online self-calibration method for sugarcane agricultural machinery in hilly terrain, characterized by: Includes the following steps: S1. Multi-source operation perception data acquisition: Real-time acquisition of multi-source operation perception data during the autonomous operation of a small sugarcane harvester in hilly terrain; S2. Construction of a multi-source perception consistency error model based on operation path feedback: Based on the multi-source operation perception data collected in step S1, the observation results of different sensors are uniformly mapped to the coordinate system of the small sugarcane harvester. Constraints caused by attitude changes and vibration interference caused by hilly terrain undulations, as well as geometric constraints caused by the center line, row direction or row spacing of sugarcane operation rows, are introduced to construct a multi-source perception consistency error model based on operation path feedback, so as to reduce the impact of perception instability on calibration results in complex hilly environments. S3. Dynamic error discrimination based on path deviation characteristics: Based on the multi-source sensing consistency error model constructed in step S2, the path consistency error is analyzed at multiple time scales, and the path deviation caused by agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field or adjusting across rows is distinguished from the potential calibration drift trend. Based on this, it is determined whether the external parameter relationship parameters of multiple sensors and the internal parameter parameters of a single sensor have drifted. If drift is detected, proceed to step S4; if drift is not detected, maintain the current parameters and proceed to step S6. S4. Collaborative Online Update of Internal and External Parameters: Without the deployment of artificial calibration targets, the internal and external parameters of multiple sensors are collaboratively updated online, taking into account the constraints of hilly operation status and sugarcane row structure. Constraints based on prior knowledge are introduced during the parameter update process to limit the range and rate of change of parameter updates and prevent the optimization results from deviating from the physical reality of sensor installation relationships and mechanical structures. S5. Calibration result-driven path awareness correction: The multi-sensor intrinsic and extrinsic parameters updated in step S4 are used to update the coordinate transformation and fusion relationship of the multi-source sensing data, correct the environmental modeling results and positioning results, and provide the corrected results to the operation path planning and path tracking process. Based on the corrected operation path feedback information, the calibration parameters are evaluated to see if they have reached a stable state. If the evaluation result shows that the calibration parameters have not reached a stable state, return to step S3 to continue parameter drift discrimination. If the evaluation result shows that the calibration parameters have reached a stable state, proceed to step S6. S6. Online closed-loop iterative operation and safety control: During continuous operation, steps S1 to S5 are executed cyclically. When a parameter update is detected that causes a sudden increase in any consistency error item, or when the updated internal and external parameters exceed the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameters are rolled back to the most recent stable state, or a safe operation mode of freezing parameter updates or degrading operation is entered, forming a multi-sensor online self-calibration closed loop adapted to hilly terrain and sugarcane operation characteristics based on operation path feedback.

2. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: The multi-source operation sensing data in S1 specifically includes: Environmental point cloud data collected by lidar to characterize the spatial structure of the working environment; Environmental perception data collected by visual sensors to characterize the geometric structure of sugarcane work rows; The inertial measurement unit (IMU) collects body attitude data and motion state data to characterize the hilly terrain undulation characteristics. The body attitude data includes at least pitch angle and roll angle to characterize the hilly terrain undulation characteristics. The pitch angle and roll angle serve as direct quantitative indicators of hilly terrain undulation and are used to model and compensate for systematic errors caused by terrain undulation in subsequent consistency error modeling and parameter updates. The motion state data includes acceleration or angular velocity data to characterize the intensity of operational vibration. The operation status data reflects the overall machine operating status during the operation. The multi-source operation sensing data is collected synchronously according to a fixed operation cycle, which is 50~200ms.

3. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: Specifically, S2 is: The multi-source sensing consistency error model includes at least one of the following error terms or a combination thereof: Geometric consistency error term The geometric consistency error is used to characterize the spatial consistency of observation results from different sensors in the same vehicle coordinate system. That is, the geometric consistency error is constructed based on the high-level geometric features of the sugarcane operation row centerline, row direction or row spacing extracted from the lidar point cloud and visual image, respectively, in order to reduce the sensitivity to local occlusion, vibration noise and single-frame perception anomalies. Path consistency error term This is used to characterize the deviation between the actual operation path and the corresponding reference operation path, i.e., the path consistency error. It characterizes the systematic deviation of the actual operation trajectory relative to the reference trajectory of the sugarcane operation row caused by the changes in the external parameter relationship and the internal parameter drift of the multi-source sensor under continuous operation conditions in hilly terrain. Its short-term fluctuations mainly come from the terrain undulation and operation vibration, and its long-term variation trend is used to indicate the potential drift of the calibration parameters. Job status consistency error term This is used to constrain the coordination between the sensing results and changes in the body's posture and operational status, i.e., the operational status consistency error term. Using pitch angle, roll angle and vibration intensity operation state parameters collected by inertial measurement unit as constraint variables, it is used to characterize the reasonable response relationship between the sensing results and changes in body attitude and operation state under hilly terrain undulation and harvesting operation vibration conditions, thereby suppressing the misjudgment of sensing changes caused by terrain undulation or operation vibration as sensor intrinsic and extrinsic parameter drift. The specific formula is as follows: ； in, This represents the external parameter transformation relationship from the sensor coordinate system to the vehicle coordinate system. Indicates the first Sensors within a single work cycle Collected geometric feature points or point cloud sampling points; ； in, For the first The actual operation trajectory pose for each operation cycle is calculated by the fusion positioning of LiDAR, visual sensor, and IMU. This fusion positioning is based on the LiDAR ranging model, visual imaging model, and IMU error model, and is therefore related to the corresponding sensor intrinsic parameters. The reference pose for the corresponding operation path is generated based on the geometric structural features of the sugarcane operation row, that is, the reference path obtained by fitting the center line or row direction of the sugarcane operation row. ； ； ； in, For the first Path deviation per cycle These are mapping coefficients, used to characterize the impact of changes in operational status on changes in path deviation. This is the job state vector; The path deviation is used to characterize the degree of deviation of the actual operation path from the reference operation path within the corresponding operation cycle. The path deviation includes at least one or more of lateral position deviation and heading deviation, and is used to reflect the systematic impact of changes in multi-sensor parameters on the execution effect of the operation path. Among them, the path consistency error term As one of the feedback error items in the operation execution layer, it reflects the impact of environmental perception errors on the actual operation effect, and it is related to the geometric consistency error item. and the consistency error term of the operation status These together constitute the joint constraints for updating calibration parameters, rather than serving as a single calibration basis.

4. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: S3 addresses path consistency error. Multi-timescale analysis is performed to determine whether persistent drift occurs in the intrinsic and extrinsic parameters of multiple sensors, specifically including: For path consistency error Short-term fluctuation filtering at the second-level timescale is performed to suppress transient disturbances caused by hilly terrain undulations and operational vibrations; the path consistency error after short-term fluctuation filtering is analyzed. Long-term trend analysis at the minute-level time scale is performed to characterize the persistent deviation caused by the drift of intrinsic and extrinsic parameters of multiple sensors; combined with operational status data, agricultural operation behaviors such as bypassing obstacles, turning at the edge of the field, or adjusting across rows are identified, and the path deviation within the corresponding operation cycle is removed or its weight is reduced as agricultural operation deviation; The path consistency error after the above processing If any of the following conditions are met within N consecutive work cycles, it is determined that there is a continuous drift in the multi-sensor parameters and the process proceeds to step S4: The average increment is greater than the first threshold T1 or The fluctuation amplitude is greater than the second threshold T2, wherein N is 5~30, the first threshold T1 is 0.01~0.50, and the second threshold T2 is 0.05~1.00; When path consistency error If a sudden increase occurs within a short period of time but falls back to the above threshold range within a preset recovery period, it is determined to be a transient disturbance and no parameter update is triggered.

5. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: Specifically, S4 is: Without deploying manually calibrated targets, based on the discrimination results of step S3, a joint cost function for multi-source error collaborative constraints is constructed: ； ； ； in, For geometric consistency error term, This is the path consistency error term. This is the consistency error term for the work status. For internal reference purposes. As an external reference, These are prior constraints, set based on the actual installation structure of the sensor on the sugarcane machinery in hilly terrain, the vibration characteristics of the entire machine, and the range of changes in its operating posture. They are used to suppress the excessive amplification of parameter updates caused by transient disturbances due to terrain undulations, ensuring that the calibration results conform to the physical feasibility under the operating conditions of the agricultural machinery. These are the prior constraint weight coefficients, and their values ​​are greater than 0. , For the physical reasonable upper and lower bounds of each parameter, , , The corresponding adaptive weight coefficients are updated as follows: Calculate them separately within the sliding window. , , residual scaling parameters , , And update the weights according to the inverse relationship of the residual scale, specifically: ， ， ； in, To prevent a preset positive number with a denominator of zero, the residual scaling parameter... The weighting coefficients are normalized to correspond to the root mean square value or standard deviation of the error term within the sliding window. ; By minimizing the joint cost function J, the intrinsic and extrinsic parameters of the multi-sensor system are updated online in a coordinated manner. The extrinsic parameters are time-varying parameters that are equivalent to changes in the machine's attitude, structural elastic deformation, or vibration during operations in hilly terrain. The intrinsic parameters are parameters of the sensor's own imaging or ranging model, and include at least one or a combination of the following: Visual sensor intrinsic parameters: focal length , Main point , and radial / tangential distortion coefficients , , , , ; LiDAR intrinsic parameters: scan angle zero bias, range scale factor, range zero bias; Inertial Measurement Unit (IMU) intrinsic parameters: accelerometer / gyroscope bias, scale factor, and inter-axis non-orthogonal parameters; The intrinsic and extrinsic parameters together constitute a parameter vector. Minimize the joint cost function iteratively within the sliding window. Solving for parameter increments and according to Update the internal references online, and... Limiting the amplitude to suppress divergence; The external parameter changes are implemented by introducing the external parameter as a variable to be estimated into the sliding window optimization framework. In each work cycle, the changes are based on the latest acquired work state vector. The extrinsic parameter variation model is calculated, and the corresponding time-varying extrinsic parameters are estimated and updated online by minimizing the joint cost function composed of the multi-source sensing consistency error. By using the above error modeling and discrimination methods, calibration parameter updates are triggered only when systematic deviations caused by hilly terrain and sugarcane operating conditions occur, thereby avoiding unnecessary calibration updates caused by normal operating behavior or transient vibrations.

6. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: Specifically, S5 is: The updated multi-sensor intrinsic and extrinsic parameters are used to update the coordinate transformation and fusion relationship of multi-source sensing data, thereby correcting the environmental modeling and positioning results. The corrected results are then provided to the operation path planning and path tracking process, ensuring that path generation and execution are based on the latest calibration parameters. The corrected environmental sensing results serve as the shared sensing base output of the entire machine's multiple modules. The coordinate transformation update process is as follows: Reconstructing the coordinate transformation matrix after extrinsic parameter update, let the extrinsic parameters from sensor s to vehicle coordinate system b be... , Then in the first For each operation cycle, a homogeneous transformation matrix is ​​constructed based on the updated extrinsic parameters: ； Unified mapping of multi-source data coordinates for LiDAR point cloud points : ； For visual sensor image feature points: use the updated camera intrinsic parameter matrix and distortion coefficient Perform distortion correction and normalization: ； in, Indicates the visual sensor in the first The first detected in the first work cycle The original pixel coordinates of each image feature point are combined with extrinsic parameters to unify the corresponding 3D features into the vehicle coordinate system or the world coordinate system; For IMU data: Correct the original measurements using updated IMU intrinsic parameters, i.e., zero bias or scale. ； in, The corrected angular velocity, With zero bias at angular velocity, is the scale factor for angular velocity; ； in, For the corrected acceleration, For zero bias of acceleration, The scale factor for acceleration; The fusion observation model is reparameterized as follows: The fusion observation model is reparameterized by reparameterizing the observation models of each sensor in the fusion system with updated intrinsic and extrinsic parameters, and the observation function is expressed as: ； in, From external references and internal reference, i.e., camera Radar scale / angle zero offset and IMU offset / scale are jointly determined. This is the observation error term, representing the measurement noise or uncertainty of sensor S at time k; Update the weights of each sensor based on the updated residual statistics: ； The environmental modeling and localization correction process is as follows: Within a sliding window, the pose and map are recalculated. The fused data from the most recent M operation cycles are replayed, and the pose trajectory is re-estimated using the updated intrinsic and extrinsic parameters. Environmental Map Output the corrected positioning results: ； in, Given the residual function, the corrected pose trajectory and environment map are obtained by minimizing the sum of squares of the residuals from the LiDAR, camera, and IMU, and the corrected environment map is output accordingly. The revised environment map includes: a new 3D environment point cloud map; a new sugarcane row centerline, row direction, and row spacing structure; a new vehicle positioning trajectory; and new reference path generation base information.

7. The online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain according to claim 1, characterized in that: Specifically, S6 is: During the online iteration process, when step S3 determines that there is a persistent drift in the multi-sensor parameters, the joint cost function is used. The rate of change controls the timing of parameter updates; ； The trigger threshold E of the joint cost function change ΔJ th E is a preset positive value, and its value is determined based on the stability of the operating environment and the system noise level. th Take a value of 0.05 to 0.

50. when Exceeding the preset trigger threshold E within N consecutive work cycles th An update is triggered when N is an integer from 5 to 30; otherwise, the current internal and external parameters remain unchanged to avoid frequent updates and ensure real-time performance. When any error term in the joint cost function is detected to have a sudden increase in a single or continuous work cycle, or when the updated parameter exceeds the preset physical reasonable range, an abnormal alarm mechanism is triggered, and the parameter is rolled back to the most recent stable state, or the safe work mode of freezing parameter updates or degrading operation is entered.

8. A system for implementing the online self-calibration method for multiple sensors in sugarcane farming machinery in hilly terrain as described in any one of claims 1-7, characterized in that, It includes lidar, vision sensor, inertial measurement unit (IMU), operation status perception module, perception consistency evaluation module based on operation path feedback, dynamic error discrimination module, internal and external parameter collaborative online update module, and calibration result-driven path perception correction module; The lidar is used to acquire environmental point cloud data that characterizes the spatial structure of the working environment. The visual sensor is used to characterize environmental perception data that represents the geometric structural features of sugarcane work rows. The inertial measurement unit (IMU) is used to acquire body attitude data and motion state data that characterize the undulation characteristics of hilly terrain. The operation status sensing module is used to acquire operation status data that reflects the overall machine operation status; The perception consistency evaluation module is used to construct a multi-source perception consistency error model by combining the execution deviation of the job path; The dynamic error discrimination module is used to perform second-level fluctuation filtering and minute-level trend analysis on path consistency error, distinguish between vibration and operational behavior deviation and calibration drift, and then determine whether the external parameters of multiple sensors and the internal parameters of a single sensor have drifted. The internal and external parameter collaborative online update module is used to collaboratively update the internal and external parameter parameters of multiple sensors online under the condition of no manual target calibration, combining the hilly operation status and operation row structure constraints and introducing prior amplitude and speed limits; The calibration result-driven path perception correction module is used to apply the updated intrinsic and extrinsic parameters to correct the environmental perception results and provide corrected perception information for subsequent operation path planning and path tracking.

Images