Track tracking dynamic correction method and system for tracked electric chassis

Abstract

The application discloses a track following dynamic correction method and system for a caterpillar electric chassis, and belongs to the field of intelligent control of agricultural robots. The method is aimed at the yellow sand substrate environment of a sunlight greenhouse. Firstly, a reference trajectory for inter-ridge operation is constructed, and a turning area is calibrated. Multi-source data are collected through an encoder, an IMU, a laser radar and a visual sensor, and a chassis pose, a deviation and a caterpillar slip state are fused and estimated. A prediction control model with time sequence embedding and dimension expansion is constructed, control weights and constraint conditions are adjusted online in combination with working condition identification. A caterpillar speed correction amount is solved through rolling optimization, and is output after being limited in amplitude. Meanwhile, disturbance estimation is updated according to an execution result, so that closed-loop control is realized. The application effectively improves the trajectory tracking precision and robustness under a soft ground, suppresses deviation accumulation during turning, and guarantees stable operation of the chassis under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of agricultural robots and intelligent control technology, and in particular to a method and system for dynamic correction of track tracking of a tracked electric chassis. Background Technology

[0002] As facility agriculture develops towards precision, intelligence, and automation, the demand for mobile operating equipment in confined environments such as greenhouses continues to increase in areas such as spraying, inspection, transportation, and auxiliary management. For example, in greenhouses cultivating tall crops such as tomatoes and peppers, the narrow passageways between rows and limited operating space require mobile chassis to not only have high passability and mobility but also maintain stable trajectory tracking capabilities when close to crop rows. Otherwise, problems such as uneven spray coverage, crop collisions, repeated operations, or missed operations can easily occur.

[0003] Existing mobile greenhouse chassis mostly employ wheeled structures or conventional differential control methods, which can meet basic navigation needs in flat, relatively stable ground environments. However, in sandy substrate cultivation environments, the surface is characterized by loose particles, uneven bearing capacity, local undulations, susceptibility to slippage, and localized subsidence. Traditional control methods are prone to problems such as straight-line yaw, turning radius mismatch, overshooting during U-turns, and inaccurate attitude estimation. Especially during U-turns at the ridge ends, significant deviations often exist between the control input and the actual attitude changes of the chassis due to the different adhesion conditions of the left and right tracks.

[0004] Furthermore, it is typically difficult to reliably acquire satellite positioning signals inside a solar greenhouse, and crop canopies, pillars, and the greenhouse frame can obstruct visual and laser sensing. Therefore, relying solely on static path planning or fixed-parameter controllers is insufficient to simultaneously ensure stability in straight sections and rapid correction capabilities during turning sections. If slippage, drift, and time-varying disturbances in the yellow sand substrate cannot be estimated and compensated for online, the chassis will easily accumulate trajectory deviations during repeated back-and-forth operations, affecting operational quality and row safety.

[0005] Based on the above problems, there is an urgent need to propose a dynamic correction method and system for tracked electric chassis trajectory tracking suitable for cultivation environments with confined working spaces and yellow sand substrate, so as to achieve high-precision trajectory tracking and dynamic correction, especially in the case of turning around, in the inter-row environment of yellow sand substrate. Summary of the Invention

[0006] To address the problems of significant slippage, inaccurate position estimation, large turning deviation, and poor trajectory tracking stability of existing tracked or differential mobile chassis in yellow sand substrate environments, this invention provides a dynamic correction method and system for tracked electric chassis trajectory tracking, in order to improve the path tracking accuracy, turning dynamic correction capability, and operational safety of the chassis in narrow spaces between ridges.

[0007] To achieve the above objectives, the present invention adopts the following technical solution.

[0008] This invention provides a dynamic correction method for trajectory tracking of a tracked electric chassis. The tracked electric chassis adopts a dual-track independent drive structure and is equipped with an encoder, an inertial measurement unit, a lidar, and a vision sensor. The method includes: Based on the boundary information of the working environment, the direction of travel, and the start and end points of the operation, a reference trajectory is constructed during the chassis operation, and the turning-off area at the ridge end is marked. During chassis operation, observation data of the current operating status of the chassis are collected in real time. The observation data includes encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data. Extract the centerline of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted centerline of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; combine the observation data to perform multi-source pose fusion estimation to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis. Based on the error state, velocity state, control state, and disturbance estimate at the current and historical moments, a time-series embedded extended-dimensional state vector is constructed, and a trajectory tracking predictive control model containing objective function and constraints is established based on the time-series embedded extended-dimensional state vector. Based on the current working condition of the chassis, the relevant control weights of the trajectory tracking predictive control model are adjusted online, and the constraints of the trajectory tracking predictive control model are updated. Based on the rolling optimization solution of the future control increment sequence of the left and right tracks, the target speed correction is obtained, and after amplitude limiting, the final control command is generated and output. The system receives the final control command and drives the left and right tracks to perform trajectory correction actions. At the same time, it updates the disturbance estimate, slip state parameters and predicted state according to the control command execution results. The process of data acquisition to trajectory correction is repeated until the inter-row operation trajectory tracking is completed.

[0009] Further, obtaining the lateral and heading deviations of the chassis relative to the reference trajectory includes: Select the reference trajectory point that is closest to the current position of the chassis on the reference trajectory, and determine the coordinates and tangential direction angle of the reference trajectory point; Based on the center features of the work channel, and combined with the coordinates and tangential direction angle of the reference trajectory point, the lateral deviation and heading deviation of the chassis relative to the reference trajectory are calculated. The acquisition of the chassis's current pose, lateral deviation, heading deviation, and track slippage parameters includes: Construct a multi-source fusion state vector that includes chassis pose, motion state, slip parameters and disturbance correction terms, and establish the corresponding system state equation and measurement equation to predict the chassis pose, motion parameters and slip disturbance state at the next moment. An extended Kalman filter algorithm is used to perform multi-source pose fusion estimation based on observation data, and the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis are output simultaneously.

[0010] Furthermore, the track slip state parameters include at least one or more of the following: left track slip ratio, right track slip ratio, instantaneous angular velocity deviation, attitude drift during the turning phase, and speed mismatch caused by local sinking; wherein, the speed mismatch caused by local sinking is the difference between the linear velocity calculated by the encoder and the actual velocity estimated by fusion, and the attitude drift during the turning phase is the cumulative or average amount of heading deviation within a preset turning window.

[0011] Furthermore, the construction of the temporal embedded extended-dimensional state vector includes: Determine the basic state vector, control input vector, and timing embedding parameters. The basic state vector corresponds to the core state of the chassis, the control input vector is the left and right track control input, and the timing embedding parameters include the state embedding order and the control input backtracking order. Based on the basic state vector, control input vector, and time-series embedding parameters, a time-series embedded extended-dimensional state vector is constructed. The extended-dimensional state vector includes the current error, historical error, historical control input, reference trajectory curvature, and reference curvature change rate.

[0012] Furthermore, the trajectory tracking predictive control model, which is constructed based on temporal embedded extended-dimensional state vectors and combined with the chassis kinematics model, and includes an objective function and constraints, comprises: Prediction time domain setting: Set the prediction time domain length to N. p The control time domain length is N c Used to predict the future N p The chassis condition and trajectory deviation at each moment are used to optimize the future N. c Control quantity at any given moment; Predictive Model Construction: Based on the constructed temporal embedded extended-dimensional state vector and combined with the chassis discrete kinematics update equation, a discrete predictive model related to operating conditions is established to predict the future N. p Chassis status and trajectory deviation at any given moment; Objective function construction: The objective is to achieve optimal trajectory tracking accuracy and optimal control smoothness in the prediction time domain N. p Internally, establish a rolling optimization objective function; Constraint setting: Taking into account the hardware performance limitations of the tracked electric chassis and the working space constraints of the greenhouse, the constraints of predictive control are set, including control input constraints and state constraints.

[0013] Furthermore, the online adjustment of the relevant control weights of the trajectory tracking predictive control model includes: adjusting the control weights online based on lateral deviation, heading deviation, reference trajectory curvature change rate, and working condition identification results; when the chassis is in a turning condition or a large curvature turn condition, increasing the heading deviation weight, slip compensation weight, and disturbance compensation weight, and reducing the constraint threshold corresponding to the track speed change smoothing term to suppress overshoot and yaw accumulation during the turning phase.

[0014] Furthermore, updating the constraints of the trajectory tracking predictive control model includes: Speed ​​constraint update: Adjust the upper and lower limits of the left and right track control inputs according to the working conditions. and When traveling straight, the upper speed limit is relaxed and the lower speed limit is tightened to ensure smooth operation; when turning around, the upper speed limit is tightened and the lower speed limit is relaxed to avoid excessive speed causing skidding.

[0015] Speed ​​Increment Constraint Update: Synchronously Adjust Upper and Lower Limits of Control Increment and This adapts to speed constraints and avoids sudden changes in control quantities. Steering safety constraints updated: The upper limit of lateral acceleration constraints is adjusted, with the upper limit lowered for U-turns to suppress sideslip, and the upper limit appropriately increased for straight-line operations to ensure operational efficiency.

[0016] Furthermore, obtaining the final control commands for the left and right tracks includes: The future control increment sequence is obtained by using an iterative optimization method with constrained projection. Once the optimization iteration converges, the first set of control increments in the optimal control sequence is taken as the execution quantity at the current moment. The left and right track feedforward velocities are obtained based on reference velocity and reference curvature; The current execution quantity is superimposed with the feedforward speed to obtain the final control commands for the left and right tracks.

[0017] Furthermore, the trajectory tracking prediction control model also introduces an online disturbance compensation term to characterize unmodeled disturbances caused by changes in the ground state of the yellow sand matrix, local subsidence, and crop interference.

[0018] This invention provides a dynamic correction method for track tracking of a tracked electric chassis, and correspondingly, a dynamic correction system for track tracking of a tracked electric chassis, comprising: a reference trajectory construction module, a data acquisition module, a state fusion estimation module, a predictive control modeling module, a working condition identification and parameter update module, a dynamic correction control module, and an execution drive and closed-loop update module; wherein... The reference trajectory construction module is used to construct the reference trajectory of the chassis during the operation process based on the boundary information of the working environment, the direction of travel information, and the start and end information of the operation, and to mark the U-turn area at the end of the ridge. The data acquisition and preprocessing module is used to collect observation data of the current operating status of the chassis in real time during chassis operation. The observation data includes encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data. The state fusion estimation module is used to extract the center line of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted center line of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; and to perform multi-source pose fusion estimation in combination with the observation data to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis. The predictive control modeling module is used to construct a time-series embedded extended-dimensional state vector based on the error state, velocity state, control state and disturbance estimate at the current and historical times, and to establish a trajectory tracking predictive control model containing objective function and constraints based on the time-series embedded extended-dimensional state vector. The working condition identification and parameter update module is used to adjust the relevant control weights of the trajectory tracking predictive control model online according to the current working condition of the chassis, and update the constraints of the trajectory tracking predictive control model. The dynamic correction control module is used to solve the future control increment sequence of the left and right tracks based on rolling optimization, obtain the target speed correction amount, and generate and output the final control command after amplitude limiting processing. The execution drive and closed-loop update module is used to receive the final control command and drive the left and right tracks to perform trajectory correction actions. At the same time, it updates the disturbance estimate, slip state parameters and predicted state according to the control command execution results, and repeats the process from data acquisition to trajectory correction until the inter-row operation trajectory tracking is completed.

[0019] Compared with the prior art, the present invention has at least the following beneficial effects: This invention addresses the unique challenges of cultivation in confined environments using yellow sand substrates by incorporating dynamic trajectory tracking correction into a unified control framework. It effectively addresses complex disturbances such as soft ground, track slippage, localized subsidence, and drifting at the ridge ends, demonstrating strong scenario specificity and high environmental adaptability. Employing a multi-source sensing fusion approach using encoders, inertial measurement units, lidar, and vision sensors, it accurately estimates the chassis's actual pose and track slippage state, significantly improving pose estimation accuracy and system robustness. Through temporal embedding and extended-dimensional state modeling, the current and historical states are jointly applied to predictive control, effectively characterizing motion lag and uncertainty under yellow sand substrate conditions, enhancing trajectory prediction accuracy and dynamic correction capabilities during turning. The introduction of condition identification and online weight scheduling mechanisms into predictive control ensures motion stability during straight-line travel and strengthens heading correction and slippage compensation during turning, effectively suppressing the accumulation of turning deviations. Roll optimization enables online correction of left and right track speeds, improving trajectory tracking performance while meeting speed constraints, speed increment constraints, and steering safety boundary requirements, resulting in strong engineering practicality and high control reliability. Attached Figure Description

[0020] Figure 1 This is a flowchart of a dynamic correction method for track tracking of a tracked electric chassis according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the tracked electric chassis and multi-source sensor arrangement structure in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the reference trajectory between ridges and the calibration of the turning-off area at the ridge end in an embodiment of the present invention. Figure 4 This is a flowchart of the trajectory tracking dynamic correction closed-loop control process in an embodiment of the present invention; Figure 5 This is a structural block diagram of the track-type electric chassis trajectory tracking dynamic correction system in an embodiment of the present invention. Detailed Implementation

[0021] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that the following embodiments are for illustrative purposes only and are not intended to limit the scope of protection of this invention. All equivalent substitutions, parameter adjustments, model transformations, or module substitutions made within the spirit and principles of this invention should fall within the scope of protection of this invention.

[0022] To accurately describe the motion characteristics of the tracked electric chassis in confined working environments, we first combine... Figure 2 and Figure 3 Taking a confined working environment, such as a solar greenhouse, as an example, this paper introduces the tracked electric chassis structure of the present invention and its positional relationship with the working scene, so as to facilitate the description and understanding of the subsequent solutions.

[0023] like Figure 2 As shown, the tracked electric chassis 6 adopts a dual-track independent electric drive structure. The left and right tracks are independently driven by the left drive motor 64 and the right drive motor 65, respectively. The chassis body consists of a chassis frame 61, a track assembly 62, and track support wheels 63, forming a symmetrically arranged tracked walking mechanism to meet the passage requirements of soft yellow sand substrate. The chassis integrates a multi-source sensing and control unit: an inertial measurement unit 8, a lidar 9, and a vision sensor 10 are used to acquire attitude, environmental contour, and inter-row visual information, respectively; a track speed encoder 7 is used to measure the track speed in real time; and a main controller 11 is used to receive multi-source sensing data and complete the execution process of the trajectory tracking dynamic correction method of the tracked electric chassis 6 of this invention, including reference trajectory construction, sensor data synchronization and preprocessing, pose fusion estimation, slip state recognition, working condition recognition, predictive control solution, and output of left and right track control commands. The left and right drive motors are controlled by the motor driver 12 to realize trajectory tracking and dynamic correction. The inter-row passage boundary 13 is used to construct the reference trajectory of the chassis, and the power module 15 is used to supply power to each unit.

[0024] like Figure 3 As shown, in the inter-row cultivation environment of the solar greenhouse 1, the planting ridge 2 and the inter-row operation channel 3 constitute the working area of ​​the chassis. Under ideal conditions, the tracked electric chassis 6 should travel in a straight line along the reference trajectory 4 in the inter-row operation channel 3 and complete the turning action in the turning area 5 at the end of the ridge to achieve reciprocating operation.

[0025] However, in confined working environments such as greenhouses with loose yellow sand substrate, variable adhesion conditions, significant local subsidence, and limited turning space at the ridge end, the tracked electric chassis 6 is prone to track slippage, local subsidence, angular velocity mismatch, and attitude drift during turning, resulting in different adhesion conditions on the left and right tracks, thus causing a significant deviation between the control input and the actual attitude change of the chassis.

[0026] To address the aforementioned issues, this embodiment provides a dynamic correction method and system for tracked electric chassis trajectory tracking, applicable to mobile operations such as spraying, inspection, and transportation of tomatoes, peppers, and other crops in confined working environments like greenhouses. Compared to differential chassis control in ordinary hard-surface environments, this embodiment integrates track slippage, local sinking, angular velocity mismatch, and attitude drift during turning into a unified predictive control framework to improve the chassis's trajectory tracking accuracy and turning correction capability under narrow inter-row conditions.

[0027] To achieve the chassis trajectory tracking and dynamic correction process, it is first necessary to build a basic kinematic model of the chassis trajectory tracking and dynamic correction in the yellow sand substrate cultivation environment of the solar greenhouse, so as to provide theoretical support for the subsequent chassis trajectory tracking and dynamic correction.

[0028] Therefore, this embodiment requires first establishing a ground fixed coordinate system and a vehicle coordinate system, setting a unified calculation benchmark for all positions, deviations, and headings, and defining the chassis pose (i.e., the position and heading angle of the chassis in the ground fixed coordinate system), track nominal linear velocity, slip ratio, and overall motion parameters (such as the chassis's equivalent linear velocity and equivalent angular velocity).

[0029] Specifically, the established ground-based fixed coordinate system is Ow−XwYw, and the vehicle-body coordinate system is Ob−XbYb. Here, Ob is the chassis geometric center, Xb is the chassis forward direction, and Yb is the chassis lateral direction. Let the chassis pose at the k-th sampling time be: , In the formula, and These are the coordinates of the chassis geometric center in a fixed ground coordinate system. This is the chassis heading angle.

[0030] Next, we define the nominal linear velocity of the track, and let the equivalent radii of the left and right track drive wheels be respectively... and The encoder measured the angular velocities of the left and right drive shafts as follows: and The nominal linear velocities of the left and right tracks obtained from the encoder are as follows: , , In the formula, the superscript (m) represents the nominal linear velocity measured by the encoder.

[0031] Based on a dual-track independent drive structure, a mapping relationship between the nominal linear velocity of the encoder and the actual speed of the tracks is established. Considering the influence of track slippage in a sandy substrate environment, left and right track slip ratios are introduced. and To characterize the track slippage characteristics in a sandy substrate environment, the actual linear velocities of the left and right tracks relative to the ground are as follows: , , In the formula, and These represent the slip ratios of the left and right tracks at the k-th sampling time, respectively. When there is no slippage between the track and the ground, the slip ratio is zero; as slippage increases, the slip ratio increases.

[0032] Let the center distance between the left and right tracks be... Then the equivalent linear velocity of the chassis and equivalent angular velocity for: , , In the formula, This represents the equivalent linear velocity of the chassis center of gravity along the longitudinal direction of the vehicle body. This represents the equivalent angular velocity of the chassis about a point perpendicular to the ground.

[0033] Furthermore, based on this, the discrete kinematics update equations for the chassis are constructed, and a linear velocity perturbation term is introduced into the velocity layer. and angular velocity disturbance term This model is used to characterize unmodeled dynamics such as the softness of the yellow sand matrix, local subsidence, surface undulation, and crop interference, providing a unified basic kinematic model for subsequent pose fusion estimation, temporal embedding extended-dimensional state prediction, and rolling optimization control. The constructed discrete kinematic update equations for the chassis are: , , , In the formula, The sampling period; This is the combined disturbance term in the direction of linear velocity; This represents the combined perturbation term in the angular velocity direction. By explicitly introducing the perturbation term into the kinematic model, the subsequent controller no longer relies on the "ideal no-slip" assumption, thereby enhancing its adaptability to soft yellow sand matrix environments.

[0034] See Figure 1 This figure is a flowchart of the dynamic correction method for track tracking of a tracked electric chassis provided in an embodiment of this application. Figure 1 As shown, the method includes the following steps: S1. Based on the boundary information of the working environment, the direction of travel, and the starting and ending points of the operation, construct the reference trajectory of the chassis during the operation and mark the turning area at the end of the ridge.

[0035] In this embodiment, the reference trajectory is the ideal operating path obtained through planning, which is the benchmark path for chassis trajectory tracking; the ridge-end turning area refers to the specific area where the chassis completes the row change and turning at the end of the inter-row operating row. This area has a narrow space, a small turning radius, and the tracks are prone to slippage and yaw due to the soft yellow sand matrix. It needs to be calibrated separately and a dedicated control strategy is adopted.

[0036] During the chassis reference trajectory construction phase, several discrete reference points are first determined based on boundary information, travel direction information, and the start and end point information of the operation: , In the formula, Let be the i-th reference point, and N be the total number of reference points. Reference points for straight sections are arranged near the center line between rows, while reference points for the U-turn area at the ridge end are set according to the turning space at the ridge end, the minimum turning radius of the chassis, and the location of the target turnaround lane.

[0037] To ensure the continuity of the reference trajectory in position, orientation, and curvature, a cubic spline curve is used to generate the reference trajectory. For any trajectory segment, the arc length parameter is used... This can be expressed as: , , In the formula, and These are the reference trajectory with arc length parameters. The horizontal and vertical coordinates below; , , , as well as For the first Segment spline coefficients.

[0038] From the reference trajectory equation, the tangential direction angle and curvature of the reference trajectory can be further obtained: , , In the formula, The tangential direction angle is the reference trajectory. The curvature of the reference trajectory; , It is the first derivative; , This is the second derivative. The curvature of the reference trajectory characterizes the degree of bending of the reference trajectory. The magnitude of the curvature directly reflects whether the current path is in a straight, turning, or U-turn condition, and is an important basis for subsequent condition identification and adaptive adjustment of control parameters.

[0039] Furthermore, the turning segment of the ridge-end U-turn area is calibrated. Based on the ridge-end turning space dimensions, the minimum turning radius of the chassis, the target lane change location, and the curvature of the reference trajectory, the ridge-end U-turn area is calibrated on the reference trajectory, determining the U-turn start position, U-turn path range, and U-turn end position. Specifically, trajectory segments with a reference trajectory curvature greater than a preset curvature threshold and satisfying the ridge-end turning space constraints can be marked as U-turn areas for subsequent chassis condition identification and adaptive switching of control strategies.

[0040] S2. During chassis operation, real-time observation data of the chassis's current operating status is collected. The observation data includes encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data.

[0041] In this embodiment, the chassis is equipped with multi-source sensing sensors, including a track speed encoder, an inertial measurement unit, a lidar, and a vision sensor. Among them: The encoder is used to collect the rotational speed information of the left and right tracks in order to obtain track speed data; The inertial measurement unit is used to collect the chassis's heading angle, angular velocity, and acceleration information to obtain chassis attitude data; Visual sensors are used to acquire field images in order to extract the center line of the furrow. When the center line of the furrow is difficult to extract stably due to shading, changes in light, or unclear surface texture, boundary features such as crop row boundaries, inter-row passage boundaries, or canopy edges can also be extracted, and the center line of the operation passage, the sequence of center points, or the center direction angle can be calculated from the left and right boundaries.

[0042] LiDAR is used to collect environmental point cloud information to extract information such as the boundaries of inter-row passages, the outline of ridges, the outline of posts, and the distance to obstacles.

[0043] Furthermore, the multi-source sensor data can be processed by time synchronization, filtering and noise reduction, and outlier data removal to remove noise, abrupt changes, and invalid data, thereby obtaining time-aligned, stable, and reliable observation data, which can be used as observations for subsequent pose fusion estimation.

[0044] S3. Extract the center line of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted center line of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; combine the observation data to perform multi-source pose fusion estimation to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis.

[0045] The boundary features include crop row boundaries, inter-row passage boundaries, or canopy edges; the operation passage center features refer to the geometric features representing the center position and extension direction of the passage area obtained by center point calculation, centerline fitting, or direction estimation of the furrow centerline, left and right crop row boundaries, inter-row passage boundaries, or canopy edges identified in the visual image, including one or more of the following: operation passage center point, center point sequence, fitted centerline, or center direction angle.

[0046] In one feasible implementation, the lateral and heading deviations of the chassis relative to the reference trajectory are obtained, specifically including: S31. Select the reference trajectory point that is closest to the current position of the chassis on the reference trajectory, and determine the coordinates and tangential direction angle of the reference trajectory point.

[0047] Specifically, reference trajectory points of the chassis can be selected ( , The reference trajectory point is the point on the reference trajectory that is closest to the current position of the chassis, and its tangential direction angle is... That is, the tangent direction of the reference trajectory at the reference trajectory point, corresponding to the chassis's desired heading.

[0048] S32. Based on the center characteristics of the work channel, and combined with the coordinates of the reference trajectory point and the tangential direction angle, calculate the lateral deviation and heading deviation of the chassis relative to the reference trajectory.

[0049] Specifically, the lateral deviation and heading deviation of the chassis relative to the reference trajectory are defined as follows: , , In the formula, Lateral deviation reflects the degree to which the chassis deviates from the center line between rows. The larger the value, the more serious the deviation of the chassis from the target working trajectory. The heading deviation reflects the angle difference between the current orientation of the chassis and the tangential direction of the reference trajectory. The larger the value, the more obvious the deviation of the chassis heading from the desired direction.

[0050] To more comprehensively describe the chassis's motion along the reference trajectory and compensate for the limitations of lateral and heading deviations, which only reflect lateral and steering deviations, a longitudinal deviation can be defined to assist in determining the chassis's propulsion position on the reference trajectory. Its definition is as follows: , In the formula, For longitudinal deviation, it is mainly used to help determine the chassis's propulsion position on the reference trajectory, so as to facilitate subsequent combination with lateral and heading deviations to fully understand the chassis's trajectory deviation status.

[0051] In one feasible implementation, the current chassis pose, lateral deviation, heading deviation, and track slippage parameters are obtained, including: S33. Construct a multi-source fusion state vector containing chassis pose, motion state, slip parameters and disturbance correction terms, and establish the corresponding system state equation and measurement equation to predict the chassis pose, motion parameters and slip disturbance state at the next moment.

[0052] To improve the accuracy of chassis state estimation and adapt to scenarios in greenhouse cultivation with sandy substrate where tracks are prone to slippage, ground subsidence, and yaw during turns, a multi-source fusion state vector can be constructed, incorporating chassis pose, motion state, and error correction terms. This vector simultaneously estimates slippage and disturbance factors affecting chassis state, achieving unified fusion and global optimal estimation of multi-sensor information, providing high-precision state input for subsequent dynamic trajectory correction. The constructed multi-source fusion state vector is as follows: , In the formula, in the formula, This is the instantaneous angular velocity deviation, used to characterize the measurement and calculation deviation of the chassis angular velocity; This represents the velocity mismatch caused by local subsidence, used to characterize the velocity measurement error caused by the softness and local subsidence of the yellow sand matrix. The attitude drift during the turning phase is used to characterize the cumulative effect of heading deviation of the chassis during the turning process at the end of the ridge.

[0053] Next, the system state equation is constructed. Based on the current motion state of the chassis and the control input, and combined with the chassis kinematic model with slip and disturbance established above, the equation predicts the chassis pose, motion parameters and slip disturbance state at the next moment, providing a priori inference basis for state estimation and ensuring that the estimation process has clear physical model support.

[0054] The constructed system state equations are as follows: , Then, a measurement equation is constructed to map the raw observation information from the encoder, inertial measurement unit, lidar, and vision sensor into observations corresponding to the system state vector. This enables the matching and correction of multi-source observation information with the predicted state, allowing sensor observation data to effectively correct model prediction biases and improve the accuracy of state estimation.

[0055] The constructed measurement equation is as follows: , In the formula, The control input vector corresponds to the control commands for the left and right tracks; The fused observation vector corresponds to the observations mapped from multiple sensor sources; This is process noise, used to characterize the errors caused by unmodeled dynamics (such as surface undulations and crop interference) during model extrapolation; For measuring noise, it is used to characterize the random measurement error of the sensor itself.

[0056] S34. The extended Kalman filter algorithm is adopted to perform multi-source pose fusion estimation based on the observation data, and the current pose of the chassis, lateral deviation, heading deviation and left and right track slip rates are output synchronously.

[0057] In this embodiment, the extended Kalman filter algorithm can be used for pose fusion estimation. This algorithm can effectively handle the state estimation problem of nonlinear systems, adapt to the nonlinear characteristics of chassis motion, and achieve accurate fusion of prediction and observation. The specific steps are as follows: 1. The prediction steps are as follows: , , In the formula, This is the prior state estimate, which is the predicted state value at the next moment obtained based on the current state and model deduction; is the prior covariance matrix, used to characterize the uncertainty of the prior state estimation; This is the Jacobian matrix of the state equation, used to linearize the nonlinear state equation; This is the process noise covariance matrix, used to quantify the impact of process noise.

[0058] 2. The update steps are as follows: , , , In the formula, This is the filter gain matrix, used to balance the reliability of prior state estimates and sensor observations, and to determine the fusion weight between the two. The Jacobian matrix for the measurement equation is used to linearize the nonlinear measurement equation. To measure the noise covariance matrix, which is used to quantify the degree of influence of the measurement noise; It is the identity matrix, used to ensure the normalization of matrix operations.

[0059] After obtaining the chassis linear velocity and angular velocity through fusion estimation using extended Kalman filtering, and considering the motion characteristics of the left and right tracks, the slip ratios of the left and right tracks are calculated as follows: , , In the formula, and These are the chassis linear velocity and angular velocity obtained from the fusion estimation, respectively; To prevent extremely small positive numbers with a denominator of zero, avoid numerical anomalies during the calculation process, and ensure the stability of the slip ratio calculation.

[0060] To improve pose estimation accuracy and comprehensively characterize chassis motion distortion under soft yellow sand, instantaneous angular velocity deviation, local sinking velocity mismatch, and attitude drift during the turning phase are all incorporated into the multi-source fusion state vector, and synchronously estimated using extended Kalman filtering. The specific definitions of each error correction term, based on multi-source sensor data, are as follows: 1. The instantaneous angular velocity deviation is defined as: , In the formula, The angular velocity measured by the inertial measurement unit; This reflects the difference between the angular velocity calculated by the encoder and the actual angular velocity measured by inertia. This difference mainly comes from factors such as track slippage and sensor errors.

[0061] 2. The velocity mismatch caused by localized subsidence is defined as: , In the formula, Used to characterize the difference between the encoder's nominal linear velocity and the fused actual chassis linear velocity, this difference is usually related to a soft matrix, local depression, or abrupt changes in grounding conditions, and is a key error correction term for adapting to yellow sand matrix environments.

[0062] 3. The attitude drift during the turn-around phase is defined as follows: , In the formula, The statistical window length for the U-turn phase can be reasonably set according to the size of the U-turn area at the ridge end and the chassis turning speed. This is used to characterize the cumulative effect of heading deviation during a turnaround. When this amount is large, it indicates that the chassis has significant yaw accumulation during the turnaround at the end of the ridge, requiring targeted correction through subsequent control strategies.

[0063] The three parameters mentioned above are not used to directly calculate the chassis pose, but rather participate in the state fusion process as system disturbance state variables. They are used to characterize the model mismatch caused by slip, sag, and attitude drift, thereby suppressing the impact of disturbances on the system. The impact of estimation accuracy is reduced, ultimately leading to more robust pose estimation.

[0064] Through the above steps, multi-source pose fusion estimation is completed, and the current pose of the chassis is finally output. ), lateral deviation ( ), heading deviation ( ) and track slippage state parameters ( , This provides high-precision and reliable state input for the construction of the S4 predictive control model and the adjustment of control parameters.

[0065] S4. Based on the error state, velocity state, control state, and disturbance estimate at the current and historical times, construct a time-series embedded extended-dimensional state vector and establish a trajectory tracking predictive control model that includes the objective function and constraints.

[0066] In one implementation, the construction of the temporal embedded extended-dimensional state vector specifically includes: S411. Determine the basic state vector, control input vector, and timing embedding parameters. The basic state vector corresponds to the core state of the chassis, the control input vector is the left and right track control input, and the timing embedding parameters include the state embedding order and the control input backtracking order.

[0067] To enhance the controller's ability to predict slip hysteresis and attitude drift, this embodiment is not limited to using the current state, but introduces a timing embedding method. The basic state vector, control input vector, and timing embedding parameters are first defined, as follows: First, let the basic state vector be defined as follows: , Secondly, the control input vector is set as follows: , In the formula, and These are the left and right track control inputs, which can be represented as target speed or equivalent torque commands.

[0068] Finally, the relevant parameters for temporal embedding are set: the state embedding order is p, and the control input backtracking order is q. These can be flexibly adjusted according to the chassis operating speed and trajectory complexity. When the operating speed is slow, p and q can be increased to capture more temporal features; when the operating speed is fast, p and q can be decreased to reduce the computational load and ensure real-time control. These parameters are used to construct the temporal embedding extended-dimensional state vector to capture the temporal features of chassis slip lag and attitude drift.

[0069] S412. Based on the basic state vector, control input vector, and time-series embedding parameters, construct a time-series embedded extended-dimensional state vector. The extended-dimensional state vector includes the current error, historical error, historical control input, and reference trajectory curvature and curvature change rate.

[0070] Based on the basic state vector, control input vector, and time-series embedding parameters determined in step S411, a time-series embedded extended-dimensional state vector is constructed, as shown in the following formula: , In the formula, The curvature of the current reference trajectory; The rate of change of curvature is the geometric information of the reference trajectory, derived from the coordinate data of the reference trajectory, ensuring that the extended dimension vector can capture the dynamic changes of the trajectory.

[0071] Since the extended state vector simultaneously includes current error, historical error, historical control input, and trajectory geometry information (specifically lateral deviation, heading deviation, pose information, track speed information, control input information, and disturbance estimate), it can more completely reflect the temporal characteristics of track slippage, attitude lag, and turning drift under yellow sand matrix conditions, providing a more comprehensive input basis for subsequent predictive control models and improving the robustness of the models.

[0072] Furthermore, the temporal embedded extended-dimensional state vector can be normalized to eliminate the dimensional differences of different state parameters and avoid the influence of dimensions on model training and control decision accuracy.

[0073] In another implementation, based on the temporally embedded extended-dimensional state vector and combined with the chassis kinematics model (i.e., the system state equation in S3), a trajectory tracking predictive control model containing the objective function and constraints is constructed, specifically including: S421, Prediction Time Domain Setting: Set the prediction time domain length to Np and the control time domain length to Nc, which is used to predict the chassis state and trajectory deviation at Np future moments and optimize the control quantity at Nc future moments.

[0074] S422, Prediction Model Construction: Based on the constructed temporal embedding extended-dimensional state vector By combining the discrete kinematics update equations of the chassis, a discrete prediction model related to the working conditions is established to predict the chassis state and trajectory deviation at the next Np time points: , In the formula, To control the increment; The perturbation vector; The reference input vector; , , and This is the state matrix related to the operating conditions.

[0075] The perturbation vector and the reference input vector are defined as follows: , , In the formula, The target's forward velocity corresponds to the reference trajectory.

[0076] This prediction model fully considers the linear velocity and angular velocity disturbance terms under the yellow sand matrix, which can accurately characterize the impact of unmodeled disturbances on the chassis motion state, improve prediction accuracy, and provide a foundation for subsequent rolling optimization.

[0077] S423. Objective Function Construction: With the objectives of achieving optimal trajectory tracking accuracy and optimal control smoothness in the prediction time domain... Within this framework, a rolling optimization objective function is established, with the following formula: , In the formula, The rolling optimization objective function is constructed for the k-th sampling time. To predict the length of the time domain, To control the time domain length, it represents the number of state steps the controller predicts forward, and typically satisfies... ≤ ; The lateral deviation at time k+i, predicted at the kth sampling time, characterizes the degree of lateral deviation of the chassis relative to the center line of the reference trajectory. To predict heading deviation, it characterizes the deviation between the chassis heading angle and the tangential direction of the reference trajectory; , These represent the left and right track slip rates in the predicted time domain, characterizing the degree of slippage between the left and right tracks and the ground under yellow sand matrix conditions. and These represent the linear velocity and angular velocity disturbances within the predicted time domain, respectively, characterizing the unmodeled disturbances caused by factors such as the softness of the yellow sand matrix, local subsidence, surface undulations, and crop interference.

[0078] For the lateral deviation weight, The heading deviation weight is used to adjust the penalty intensity of trajectory tracking accuracy in the objective function; This is the track slip penalty weight, used to suppress excessive slip rates on the left and right tracks; For the first The disturbance compensation weights at each sampling time are used to suppress the impact of linear velocity and angular velocity disturbances on trajectory tracking. and These are the incremental weights for left and right track control, used to limit the rapid changes in left and right track control inputs and ensure smooth chassis movement. and These are the left and right track control input increments in the prediction time domain, respectively; and These are the lateral terminal error weights and the heading terminal error weights, respectively, used to enhance the trajectory convergence at the end of the prediction time domain.

[0079] It should be noted that this embodiment introduces a disturbance compensation term, which refers to the linear velocity disturbance estimate and angular velocity disturbance estimate introduced into the prediction model for external disturbances (such as changes in soil resistance and ground subsidence) and unmodeled errors (such as track slippage and uneven wheel speed) experienced by the tracked chassis in the yellow sand substrate environment of the solar greenhouse. By setting corresponding weighted penalty terms in the objective function and combining them with a recursive update mechanism, real-time observation and compensation of disturbances can be achieved, thereby improving the robustness of trajectory tracking.

[0080] S424. Constraint Setting: Considering the hardware performance limitations of the tracked electric chassis and the working space constraints of the greenhouse, set the constraints for predictive control, mainly including control input constraints and state constraints. Specifically, these include: To ensure safe operation between greenhouse rows and the safety of the chassis structure, the following constraints are applied: , , , , In the formula, and These are the lower and upper limits for the left and right track control inputs, respectively. and These are the lower and upper limits for controlling the increment, respectively.

[0081] At the same time, the lateral deviation must meet the safety constraints of the inter-row boundary: , In the formula, The effective passage width between ridges (refers to the effective space width between adjacent planting ridges, crop row boundaries, or ridge boundaries that allows the chassis to pass safely). The outer width of the tracked electric chassis (including the left and right tracks and the maximum width of the protruding structures on both sides of the chassis in the lateral direction). The lateral safety margin is used to compensate for safety risks arising from factors such as sensor measurement errors, pose estimation errors, crop leaf spread, local ridge surface undulations, and control execution lag. To ensure the safe operation of the chassis within the inter-ridge passage, it should meet the following requirements. > +2 If this condition is not met, it indicates that the current inter-row passage width is insufficient, and the chassis should reduce speed, stop operation, or replan the work path.

[0082] To suppress sideslip caused by excessive speed during the U-turn phase, lateral acceleration constraints must also be met: , In the formula, The maximum permissible lateral acceleration.

[0083] In non-U-turn scenarios, to avoid unnecessary reverse driving, the following settings can be configured: , In U-turn situations, to allow for turning on the spot or turning with a small radius, the following restrictions can be relaxed: , The two types of constraint boundaries mentioned above are not fixed, but switch online according to the working condition identification results. This process makes the chassis more stable during straight driving and more flexible during U-turns, which is one of the important features that distinguishes this embodiment from fixed parameter path tracking control.

[0084] S5: Based on the current operating conditions of the chassis, adjust the relevant control weights of the trajectory tracking predictive control model online and update the constraints of the trajectory tracking predictive control model.

[0085] In this embodiment, the working conditions include going straight, turning, and making a U-turn. The identification of the working conditions can be based on the curvature of the reference trajectory. The distance from the chassis to the U-turn area at the ridge end is used for determination. Let... This represents the distance from the current position of the chassis to the entrance of the U-turn area. and If the curvature threshold is used, then the operating condition variable... Defined as: , In the formula, This represents the working condition identification result at the k-th sampling time. =1 indicates straight-line driving mode. =2 indicates a normal turning condition. =3 indicates a turning point at the ridge end or a high-curvature turn; Identify threshold distances for U-turn areas.

[0086] Furthermore, to achieve adaptive control based on operating conditions, this embodiment can also adjust the weights of lateral deviation, heading deviation, slip penalty, disturbance compensation, and left and right track control increments in the objective function online according to the current operating conditions of the chassis. This allows the predictive control model to adapt to different operating states such as straight-line travel, general turns, and U-turns at the end of a ridge. The weight adjustment relationship can be expressed as: , , , , , , in, , , , , and These are the baseline weights for lateral deviation, heading deviation, track slippage, disturbance compensation, left track control increment, and right track control increment under the corresponding working conditions. , , , , and It is a non-negative adjustment coefficient; and These are the linear velocity perturbation estimators and the angular velocity perturbation estimators, respectively. This is a function indicating the turning condition. It takes a value of 1 when the chassis is turning at the end of a ridge or making a large curvature turn, and a value of 0 otherwise.

[0087] When the chassis is in a straight-line driving condition, the system maintains a high weight for lateral deviation and control increment, allowing the chassis to travel smoothly along the centerline between the rows. When the chassis enters a normal turning condition, the system appropriately increases the weight for heading deviation, enabling the chassis to follow the curvature changes of the reference trajectory in a timely manner. When the chassis enters a U-turn at the end of the row or a high-curvature turn, the system increases the weight for heading deviation, slip penalty, and disturbance compensation, while reducing the weight for control increment of the left and right tracks. This allows the controller to adjust the speed difference between the left and right tracks more quickly while meeting the constraints, suppressing yaw accumulation and slip amplification during the U-turn phase.

[0088] Furthermore, this embodiment combines the operating condition identification results to update the speed constraints, speed increment constraints, and steering safety constraints set in S4 online, ensuring that the constraints match the current operating conditions. The specific implementation is as follows: (1) Speed ​​constraint update: Adjust the upper and lower limits of the left and right track control inputs according to the working conditions. and These are the lower and upper limits for the left and right track control inputs, respectively. and and When traveling straight, the upper speed limit is relaxed and the lower speed limit is tightened to ensure smooth operation; when turning around, the upper speed limit is tightened and the lower speed limit is relaxed to avoid excessive speed causing skidding.

[0089] (2) Speed ​​increment constraint update: Synchronously adjust the upper and lower limits of the control increment. and It adapts to speed constraints and avoids sudden changes in control quantities.

[0090] (3) Steering safety constraint update: Focus on adjusting lateral acceleration constraints upper limit When turning around, the upper limit should be lowered to suppress sideslip; when going straight, the upper limit should be appropriately increased to ensure operational efficiency.

[0091] Meanwhile, based on the adaptation logic of the inter-row boundary safety constraint for lateral deviation during working condition switching, the allowable range of lateral deviation is appropriately reduced during the turning condition to ensure the safety of inter-row operations.

[0092] S6: Based on the rolling optimization solution, the target speed correction of the left and right tracks is obtained, and after amplitude limiting, the final control command is generated and output.

[0093] This step is based on the aforementioned trajectory tracking predictive control model. The objective function defined by the model is minimized as the solution objective. Combined with the predictive capability of the model, the future control sequence is optimized and solved. At the same time, the solution process must meet the constraints obtained from the model and operating condition updates to ensure that the control commands are within the range of the chassis's physical performance and operational safety.

[0094] In this embodiment, during the rolling optimization solution stage, the objective function and constraints are organized into a standard quadratic optimization problem: ,

[0095] In the formula, The sequence of future control increments is the set of left and right track speed control increments to be solved, containing the left track control increments at several future times (the control time domain of the predictive control model in S4.3). And right track control increment ; It is a Hessian matrix, a symmetric matrix used to describe the characteristics of the quadratic term of the objective function. It is derived from the coefficients of the quadratic term of the objective function. Its core function is to reflect the mutual influence between different control increments, which affects the convergence speed of the optimization solution. This is a vector of linear terms, used to describe the characteristics of the linear terms in the objective function and to help determine the optimization direction; the vector dimension is... Consistent, its elements are derived from the coefficients of the linear terms of the objective function in S4.3. Its core function is to help determine the optimization direction of the control increment and ensure that the objective function is minimized.

[0096] and These are the constraint matrix and constraint boundary vector, which together constitute the constraint conditions. The constraints updated in S5 are transformed into a standardized form for limitation. Within a feasible range, ensure that the control increment obtained from the solution meets the actual engineering requirements.

[0097] in, (Constraint Matrix): Used to convert various constraints (speed constraints, speed increment constraints, steering safety constraints, etc.) after S5 update into matrix form, realizing the standardized expression of constraints; (Constraint Boundary Vector): The boundary values ​​corresponding to the constraint conditions, which are the upper and lower limits of the constraints after online adjustment by S5 (e.g., , A set of values ​​is used to implement the limiting processing of the target speed correction amount, that is, if the speed correction amount is greater than the upper limit ( If the upper limit is less than the lower limit, then the upper limit is taken; if the lower limit is less than the lower limit, then the upper limit is taken. If the value is less than or equal to the minimum value, then the lower limit value is taken; otherwise, the original calculated value is retained.

[0098] To balance online solution speed and feasibility, this embodiment employs an iterative optimization method with constrained projection, the iterative form of which is: , In the formula, For the first The control increment sequence for each iteration; This is the iteration step size; Represents the feasible region of constraints The projection operator.

[0099] Once the optimization iteration converges, the first set of control increments in the optimal control sequence is taken as the execution quantity at the current moment: , The above technology ensures that the optimized output meets the maximum speed of the left and right tracks, the maximum speed change rate, the safety distance between the ridges, and the anti-skid constraint.

[0100] To further improve the response quality during U-turns and curves, a feedforward term based on reference speed and reference curvature is introduced. The left and right track feedforward speeds are as follows: , , In the formula, and These are the left and right track feed-forward speeds, respectively. Used as reference linear velocity; The curvature is used as a reference trajectory. In the formula, B is the track width between the left and right tracks (an inherent hardware parameter of the chassis), and the curvature is... The speed difference between the left and right tracks is determined by the curvature: the greater the curvature, the greater the speed difference between the left and right tracks, which is suitable for steering requirements.

[0101] Therefore, the final control commands for the left and right tracks are as follows: , , This composite control structure of "feedforward baseline + rolling correction" enables the chassis to first give a basic steering trend according to the geometric characteristics of the reference trajectory, and then use the optimization correction term to compensate for slip, disturbance and U-turn deviation. Therefore, it has better dynamic response capability than simple feedback control (which only relies on error correction), especially improving the control accuracy of U-turns and curve segments.

[0102] S7. Receive the final control command and drive the left and right tracks to perform trajectory correction actions. At the same time, update the disturbance estimate, slip state parameters and predicted state according to the control command execution results. Repeat the data acquisition to trajectory correction process from S2 to S6 until the inter-row operation trajectory tracking is completed.

[0103] In this embodiment, the final control command is output to the left and right motor drivers to execute trajectory correction actions. After the control command is executed, the disturbance estimate, track slip state parameters, and predicted state are updated online to match the changes in the ground state of the yellow sand matrix in real time, providing more accurate prior state information for the next control cycle.

[0104] The disturbance estimate is updated online using a recursive method, with the following recursive form: In this embodiment, after control execution, the disturbance term needs to be updated online to reflect changes in the state of the yellow sand matrix. Its recursive form can be written as: , , In the formula, and Forgetting coefficient, , These are the linear velocity disturbance estimator and the angular velocity disturbance estimator, respectively. and For disturbance correction gain; and These are the one-step predicted velocity and the one-step predicted angular velocity, respectively, output by the S4.3 prediction model.

[0105] Figure 4 This is a flowchart illustrating the closed-loop control process for dynamic correction of trajectory tracking in an embodiment of the present invention. It shows the complete control signal flow from reference trajectory input, state estimation, operating condition identification, dynamic control correction to chassis execution and state feedback, demonstrating the closed-loop control logic of the method. Figure 4 As shown, through the above recursive updates, the controller can continuously correct its understanding of the disturbance of the yellow sand matrix and the track slip characteristics in each control cycle, thereby improving the trajectory prediction and tracking control accuracy for the next cycle. After the update is completed, S2 to S6 are repeated to form a closed-loop control until the chassis completes the tracking of all inter-row operation trajectories.

[0106] In a specific application example where the chassis performs spraying operations along the centerline between tomato planting rows, when the chassis is in a straight section, the curvature of the reference trajectory is small, and the system identifies it as a straight-line condition. At this time, the weight of lateral deviation is large, and the weight of control increment smoothing is high to ensure that the spraying process is smooth and the trajectory runs closely to the centerline between rows. When the chassis approaches the end of the row, the curvature of the reference trajectory increases and the distance to the end area decreases. The system switches to a turning or U-turn condition. At this time, the weight of heading deviation and slip compensation is increased, and the control smoothing constraint is appropriately relaxed, so that the chassis can complete the attitude adjustment more quickly. If the left track slips significantly due to local softness at a certain moment, the left track slip rate, angular velocity deviation, and speed mismatch jointly drive the controller to increase the speed difference between the left and right tracks and suppress the forward speed, thereby reducing yaw accumulation and completing trajectory regression.

[0107] Corresponding to the above method, the present invention also provides a tracked electric chassis trajectory tracking dynamic correction system 100 for cultivation environments in solar greenhouses with yellow sand substrate, such as... Figure 5 As shown, the system 100 includes a reference trajectory construction module 101, a data acquisition module 102, a state fusion estimation module 103, a predictive control modeling module 104, a working condition identification and parameter update module 105, a dynamic correction control module 106, and an execution drive and closed-loop update module 107. Among them, The reference trajectory construction module 101 is used to construct the reference trajectory during the chassis operation process based on the boundary information of the working environment, the direction of travel information, and the start and end information of the operation, and to mark the U-turn area at the end of the ridge. The data acquisition and preprocessing module 102 is used to acquire observation data of the current operating status of the chassis in real time during chassis operation. The observation data includes encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data. The state fusion estimation module 103 is used to extract the center line of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted center line of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; and to perform multi-source pose fusion estimation in combination with the observation data to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis. The predictive control modeling module 104 is used to construct a time-series embedded extended-dimensional state vector based on the error state, velocity state, control state and disturbance estimate at the current and historical times, and to establish a trajectory tracking predictive control model containing objective function and constraint conditions based on the time-series embedded extended-dimensional state vector. The working condition identification and parameter update module 105 is used to adjust the relevant control weights of the trajectory tracking predictive control model online according to the current working condition of the chassis, and update the constraints of the trajectory tracking predictive control model. The dynamic correction control module 106 is used to solve the future control increment sequence of the left and right tracks based on rolling optimization, obtain the target speed correction amount, and generate and output the final control command after amplitude limiting processing. The execution drive and closed-loop update module 107 is used to receive the final control command and drive the left and right tracks to perform trajectory correction actions. At the same time, it updates the disturbance estimate, slip state parameters and predicted state according to the control command execution results, and repeats the process from data acquisition to trajectory correction until the inter-row operation trajectory tracking is completed.

[0108] Furthermore, the system also includes: Storage module 108 is used to store reference trajectory, chassis operating status, control parameters and intermediate calculation results; The human-machine interaction module 109 is used to receive user-set operating parameters, display chassis operating status and trajectory tracking results.

[0109] The tracked electric chassis trajectory tracking dynamic correction system provided in this embodiment updates the disturbance estimate, slip state parameters and predicted state based on the control results after control execution by the state fusion estimation module and the dynamic correction control module. It then repeatedly executes data acquisition, state estimation, dynamic correction and drive execution to form closed-loop control until the inter-row operation trajectory tracking is completed.

[0110] In summary, this invention addresses the problems of significant slippage, pose inaccuracy, accumulated turning deviation, and insufficient trajectory tracking stability in tracked chassis operating in yellow sand substrate environments within solar greenhouses. It proposes a dynamic trajectory tracking correction method that unifies and couples multi-source pose fusion, slip parameter estimation, temporal embedding extended-dimensional state modeling, adaptive weight adjustment based on operating conditions, and constrained projection rolling optimization. This method ensures both the tracking accuracy between rows during straight-line movement and enhances the dynamic correction capability during row-end turning, demonstrating good engineering applicability and scenario specificity.

[0111] The parts not described in detail in this application are all existing conventional technologies and will not be elaborated here.

[0112] It is understood that the above specific description of the present invention is only for illustrating the present invention and is not limited to the technical solutions described in the embodiments of the present invention. Those skilled in the art should understand that modifications or equivalent substitutions can still be made to the present invention to achieve the same technical effect; as long as the use needs are met, they are all within the protection scope of the present invention.

Claims

1. A dynamic correction method for trajectory tracking of a tracked electric chassis, characterized in that, The tracked electric chassis adopts a dual-track independent drive structure and is equipped with an encoder, inertial measurement unit, lidar, and vision sensor. The method includes: Based on the boundary information of the working environment, the direction of travel, and the start and end points of the operation, a reference trajectory is constructed during the chassis operation, and the turning-off area at the ridge end is marked. During chassis operation, real-time observation data of the chassis's current operating status is collected, including encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data; Extract the centerline of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted centerline of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; combine the observation data to perform multi-source pose fusion estimation to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis. Based on the error state, velocity state, control state, and disturbance estimate at the current and historical moments, a time-series embedded extended-dimensional state vector is constructed, and a trajectory tracking predictive control model containing objective function and constraints is established based on the time-series embedded extended-dimensional state vector. Based on the current working condition of the chassis, the relevant control weights of the trajectory tracking predictive control model are adjusted online, and the constraints of the trajectory tracking predictive control model are updated. Based on the rolling optimization solution of the future control increment sequence of the left and right tracks, the target speed correction is obtained, and after amplitude limiting, the final control command is generated and output. The system receives the final control command and drives the left and right tracks to perform trajectory correction actions. At the same time, it updates the disturbance estimate, slip state parameters and predicted state according to the control command execution results, and repeats the data acquisition to trajectory correction process until the inter-row operation trajectory tracking is completed.

2. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The acquisition of the lateral and heading deviations of the chassis relative to the reference trajectory includes: Select the reference trajectory point that is closest to the current position of the chassis on the reference trajectory, and determine the coordinates and tangential direction angle of the reference trajectory point; Based on the center features of the work channel, and combined with the coordinates and tangential direction angle of the reference trajectory point, the lateral deviation and heading deviation of the chassis relative to the reference trajectory are calculated. The acquisition of the chassis's current pose, lateral deviation, heading deviation, and track slippage parameters includes: Construct a multi-source fusion state vector that includes chassis pose, motion state, slip parameters and disturbance correction terms, and establish the corresponding system state equation and measurement equation to predict the chassis pose, motion parameters and slip disturbance state at the next moment. An extended Kalman filter algorithm is used to perform multi-source pose fusion estimation based on observation data, and the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis are output simultaneously.

3. The trajectory tracking dynamic correction method according to claim 2, characterized in that, The track slip state parameters include at least one or more of the following: left track slip ratio, right track slip ratio, instantaneous angular velocity deviation, attitude drift during the turn-around phase, and speed mismatch caused by local sinking; wherein, the speed mismatch caused by local sinking is the difference between the linear velocity calculated by the encoder and the actual velocity estimated by fusion, and the attitude drift during the turn-around phase is the cumulative or average amount of the heading deviation within a preset turn-around window.

4. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The construction of the temporal embedded extended-dimensional state vector includes: Determine the basic state vector, control input vector, and timing embedding parameters. The basic state vector corresponds to the core state of the chassis, the control input vector is the left and right track control input, and the timing embedding parameters include the state embedding order and the control input backtracking order. Based on the basic state vector, control input vector, and time-series embedding parameters, a time-series embedded extended-dimensional state vector is constructed. The extended-dimensional state vector includes the current error, historical error, historical control input, reference trajectory curvature, and reference curvature change rate.

5. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The trajectory tracking predictive control model, based on temporal embedded extended-dimensional state vectors and combined with the chassis kinematics model, includes the construction of an objective function and constraints, comprising: Prediction time domain setting: Set the prediction time domain length to N. p The control time domain length is N c Used to predict the future N p The chassis condition and trajectory deviation at each moment are used to optimize the future N. c Control quantity at any given moment; Predictive Model Construction: Based on the constructed temporal embedded extended-dimensional state vector and combined with the chassis discrete kinematics update equation, a discrete predictive model related to operating conditions is established to predict the future N. p Chassis status and trajectory deviation at any given moment; Objective function construction: The objective is to achieve optimal trajectory tracking accuracy and optimal control smoothness in the prediction time domain N. p Internally, establish a rolling optimization objective function; Constraint setting: Taking into account the hardware performance limitations of the tracked electric chassis and the working space constraints of the greenhouse, the constraints of predictive control are set, including control input constraints and state constraints.

6. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The online adjustment of the relevant control weights of the trajectory tracking predictive control model includes: adjusting the control weights online based on lateral deviation, heading deviation, reference trajectory curvature change rate, and working condition identification results; when the chassis is in a turning condition or a large curvature turn condition, increasing the heading deviation weight, slip compensation weight, and disturbance compensation weight, and reducing the constraint threshold corresponding to the track speed change smoothing term to suppress overshoot and yaw accumulation during the turning phase.

7. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The updating of the constraints on the trajectory tracking predictive control model includes: Speed ​​constraint update: Adjust the upper and lower limits u of the left and right track control inputs according to the working conditions. max and u min When traveling straight, the upper speed limit is relaxed and the lower speed limit is tightened to ensure smooth operation; when turning around, the upper speed limit is tightened and the lower speed limit is relaxed to avoid excessive speed causing skidding. Speed ​​Increment Constraint Update: Synchronously Adjust the Upper and Lower Limits Δu of the Control Increment max and Δu min This adapts to speed constraints and avoids sudden changes in control quantities. Steering safety constraints updated: The upper limit of lateral acceleration constraints is adjusted, with the upper limit lowered for U-turns to suppress sideslip, and the upper limit appropriately increased for straight-line operations to ensure operational efficiency.

8. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The acquisition of the final control commands for the left and right tracks includes: The future control increment sequence is obtained by using an iterative optimization method with constrained projection. Once the optimization iteration converges, the first set of control increments in the optimal control sequence is taken as the execution quantity at the current moment. The left and right track feedforward velocities are obtained based on reference velocity and reference curvature; The current execution quantity is superimposed with the feedforward speed to obtain the final control commands for the left and right tracks.

9. The trajectory tracking dynamic correction method according to claim 1, characterized in that, The trajectory tracking prediction control model also introduces an online disturbance compensation term to characterize unmodeled disturbances caused by changes in the ground state of the yellow sand matrix, local subsidence, and crop interference.

10. A dynamic correction system for track tracking of a tracked electric chassis, characterized in that, include: The system comprises a reference trajectory construction module, a data acquisition module, a state fusion estimation module, a predictive control modeling module, a working condition identification and parameter update module, a dynamic correction control module, and an execution-driven and closed-loop update module; among which, The reference trajectory construction module is used to construct the reference trajectory of the chassis during the operation process based on the boundary information of the working environment, the direction of travel information, and the start and end information of the operation, and to mark the U-turn area at the end of the ridge. The data acquisition and preprocessing module is used to collect observation data of the current operating status of the chassis in real time during chassis operation. The observation data includes encoder speed data, inertial measurement unit attitude data, lidar environmental contour data, and visual image data. The state fusion estimation module is used to extract the center line of the furrow or at least one boundary feature from the visual image data, and calculate the center feature of the working channel based on the extracted center line of the furrow or boundary feature to obtain the lateral deviation and heading deviation of the chassis relative to the reference trajectory; and to perform multi-source pose fusion estimation in combination with the observation data to obtain the current pose, lateral deviation, heading deviation and track slip state parameters of the chassis. The predictive control modeling module is used to construct a time-series embedded extended-dimensional state vector based on the error state, velocity state, control state and disturbance estimate at the current and historical times, and to establish a trajectory tracking predictive control model containing objective function and constraints based on the time-series embedded extended-dimensional state vector. The working condition identification and parameter update module is used to adjust the relevant control weights of the trajectory tracking predictive control model online according to the current working condition of the chassis, and update the constraints of the trajectory tracking predictive control model. The dynamic correction control module is used to solve the future control increment sequence of the left and right tracks based on rolling optimization, obtain the target speed correction amount, and generate and output the final control command after amplitude limiting processing. The execution drive and closed-loop update module is used to receive the final control command and drive the left and right tracks to perform trajectory correction actions. At the same time, it updates the disturbance estimate, slip state parameters and predicted state according to the control command execution results, and repeats the process from data acquisition to trajectory correction until the inter-row operation trajectory tracking is completed.

Images