Track agricultural machinery transfer platform system for realizing dynamic pose adjustment based on optimized AKF algorithm and closed-loop control method thereof

Abstract

The invention provides a track agricultural machine transfer platform system for achieving dynamic pose adjustment based on an optimized AKF algorithm and a closed-loop control method thereof.The system comprises a fixed base, a dynamic regulation and control platform and a control system, the fixed base is used for providing an installation foundation, and the dynamic regulation and control platform is installed on the fixed base; the dynamic regulation and control platform is used for bearing the agricultural machinery to be transferred and achieving three-dimensional space pose adjustment, and the control system is electrically connected with the dynamic regulation and control platform and used for processing the collected data, executing the optimized AKF algorithm and outputting a control instruction. According to the method, a three-degree-of-freedom dynamic regulation and control platform and an optimized AKF algorithm are fused, a complete sensing-decision-execution closed-loop control system is constructed, position tracking adjustment and tipping early warning of the agricultural machinery are achieved, pneumatic active damping and a linear motor module are adopted for cooperative work, sensor data are more reliable, and the accuracy of the system is improved. Safety guarantee is provided for agricultural machinery transfer in hills and mountains, and high-reliability technical guarantee is provided for intelligent agricultural machinery equipment for agricultural mechanical transportation.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of intelligent agricultural equipment design, and particularly relates to a track agricultural machine transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment and a closed-loop control method thereof. BACKGROUND

[0002] In China, the cultivated land in hilly and mountainous areas accounts for more than 30%. Due to large terrain undulations and broken land blocks, the slope is generally 10°~25°, and it is relatively difficult to transfer agricultural machines there. In complex mountainous environments, there are many challenges. Long-term bearing can cause local bending of the double-track track, causing high-frequency vibration of the transfer platform. When the slope suddenly changes or the agricultural machine is shifted, the instantaneous inclination can reach more than 10°, and there is a risk of sudden tilting. At the same time, dust obscures the sensor field of view, and changes in light affect the accuracy of visual detection, further exacerbating the instability of the system. The existing track transportation system has obvious defects in attitude perception and attitude control. The existing technology (Wang J, Wang H, Fu J, Liu L. Design and test of two-section track walking mechanism in hilly and mountainous areas [J]. Agricultural mechanization research, 2023, (No. 9)) developed a track-type mountainous transfer system based on a single-axis inclination sensor and a PID algorithm. Simulation and test show that the single-axis sensor cannot capture the multi-dimensional attitude change of the complex mountainous area, and no anti-interference scheme is designed for dust and light fluctuations, resulting in a significant decrease in inclination detection accuracy in harsh environments. At the same time, the PID algorithm has a lag response to dynamic vibration errors caused by track bending, and it is difficult to meet the real-time attitude control requirements under high-frequency vibration.

[0003] To solve the above technical problems, the present application designs a track agricultural machine transfer platform based on an optimized AKF algorithm for dynamic pose adjustment and a closed-loop control system and method. SUMMARY

[0004] In view of the deficiencies in the prior art, the present application provides a track agricultural machine transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment and a closed-loop control method thereof, which solves the problems of perception distortion, insufficient reliability of sensor data, and insufficient response of control lag in sudden situations in the prior art.

[0005] The present application achieves the above technical objectives through the following technical means.

[0006] A track agricultural machine transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, comprising a fixed base, a dynamic control platform, and a control system. The fixed base is used to provide a mounting base. The dynamic control platform is installed on the fixed base and is used to carry the agricultural machine to be transferred and to realize three-dimensional space pose adjustment. The control system is electrically connected to the dynamic control platform and is used to process collected data, execute the optimized AKF algorithm, and output control instructions to the dynamic control platform. The dynamic regulation platform comprises an X-axis linear motor module fixed on a fixed base, a sliding table of the X-axis linear motor module is connected with a base of a Y-axis linear motor module, and a platform mounting plate is fixed on a sliding table of the Y-axis linear motor module; cylinder bodies of two Z-axis pneumatic cylinders are respectively positioned and installed in non-stroke areas at the front end and the tail end of the Y-axis linear motor module, and top ends of piston rods of the Z-axis pneumatic cylinders are rigidly connected with the platform mounting plate; six-axis IMUs are arranged at four corners of the bottom of the platform mounting plate, and a binocular camera and a laser radar are installed on the top of the platform side bin; the dynamic regulation platform further comprises a platform side bin fixed at the end of the fixed base; The control system comprises an X-axis servo motor driver, an X-axis servo motor, a Y-axis servo motor driver, a Y-axis servo motor, a proportional valve controller, an upper computer and a lower computer fixed in the platform side bin; the upper computer is connected with the binocular camera through a PCIe x4 interface and connected with the laser radar through a USB interface, and the lower computer is a real-time PLC connected with the X-axis servo motor driver, the X-axis servo motor, the Y-axis servo motor driver, the Y-axis servo motor and the proportional valve controller through an EtherCAT bus.

[0007] Further, the fixed base comprises an aluminum alloy shockproof base, a shock-absorbing rubber pad and a platform support frame; the aluminum alloy shockproof base is fixed on the platform support frame through bolts, and the platform support frame is located on the track and can slide on the track; a dovetail-shaped positioning groove is processed at the bottom of the aluminum alloy shockproof base, and cooperates with a convex part of the shock-absorbing rubber pad to ensure that the shock-absorbing rubber pad is tightly installed at the bottom of the aluminum alloy shockproof base.

[0008] A closed-loop control method of the above-mentioned track agricultural machinery transfer platform system based on the optimized AKF algorithm for dynamic pose adjustment comprises the following processes: Step 1: the PLC first controls the servo motor driver and the servo motor to perform self-checking, controls the linear motor module to perform zero-point calibration, and adjusts the air intake amount of the Z-axis pneumatic cylinder through the proportional valve controller to make the piston rod rise to the position of the horizontal zero point of the platform mounting plate; Step 2: the transfer platform carries the agricultural machinery to walk on the track, in this process, the upper computer starts the binocular camera to complete binocular image calibration, drives the laser radar to scan the track plane to construct an initial point cloud map, and collects three-axis acceleration / angle speed data of the six-axis IMU in real time; all sensor data are transmitted back to the upper computer through a shielded cable, and an initial pose matrix is generated by the upper computer through fusion; Step 3: The host computer continuously receives multi-source sensor data streams, establishes a coupled dynamic state model using the optimized AKF algorithm, integrates the physical correlation of vision, three-dimensional point cloud, and inertial motion to describe the transfer platform motion; at the same time, dynamically adjusts the process noise covariance and measurement noise covariance, and fuses multi-source data through weighted centroid fusion, combines the risk assessment results to feedback adjust the noise parameters and fusion weight, forms a closed-loop optimization control of the transfer platform pose adjustment; Step 4: In the face of sudden conditions, start the hierarchical response mechanism; Step 5: After the transfer platform adjustment is completed, the adhesion of the agricultural machine and the transfer platform is detected through the binocular camera, and the horizontal degree graph is generated by the laser radar plane scanning; the host computer continuously optimizes the AKF parameters; finally, the adjustment data containing the actuator displacement curve, sensor confidence distribution and energy consumption statistics are output, realizing dynamic pose adjustment in the cycle and completing the on-orbit state perception.

[0009] Further, the specific process of step 3 is as follows: Step 3.1: First, two coupled state models are established to describe the transfer platform dynamics, including the platform level state vector and the agricultural machine level state vector ; the coupled dynamics equation is introduced, which takes into account the platform acceleration, slope gravity component and friction coefficient, providing a more realistic mathematical model for the transfer platform: wherein, represents the transfer platform acceleration, represents the equivalent damping coefficient between the transfer platform and the agricultural machine, represents the acceleration of gravity, represents the longitudinal relative displacement of the agricultural machine centroid, represents the longitudinal relative acceleration of the agricultural machine centroid; Step 3.2: Design a real-time terrain data-driven noise adjuster, so that the process noise covariance and the measurement noise covariance of the optimized AKF algorithm can be dynamically adjusted in real time, and the specific adjustment formula is as follows:

[0010] wherein, represents the initial process noise covariance, represents the adjustment coefficient, represents the real-time terrain gradient, represents the initial measurement noise covariance, represents the adjustment coefficient; Step 3.3: The center of mass position of the agricultural machine is the key to the rollover risk judgment. After obtaining the original point cloud from the binocular camera, the ground point cloud is segmented, and the point cloud cluster belonging to the agricultural machine is separated. The weighted centroid is calculated by adopting the inverse proportion to the Z coordinate weighting method, which provides an advanced judgment for risk warning. Step 3.4: The upper computer generates control instructions according to the detected position of the agricultural machine and the weighted centroid calculated in step 3.3 inputs the decision judgment and evaluates the risk according to the two core criteria, which are as follows: wherein, represents the track gauge, represents the length of the platform mounting plate, represents the safety factor, represents the pitch angle of the platform mounting plate; represents the square of the longitudinal speed of the transfer platform; If any criterion is triggered, the upper computer immediately generates control instructions; Step 3.5: The upper computer sends the generated control instructions to the PLC, and the PLC sends motion instructions to the X-axis servo motor driver and Y-axis servo motor driver. The motion controller integrated in the driver will adopt an S-shaped acceleration and deceleration curve planning to make the speed of the sliding table change continuously. For Z-axis deviation, the PLC controls the proportional valve controller to adjust the vertical acceleration collected by the six-axis IMU. When a sudden impact is detected, the pneumatic active damper instantly opens the pressure relief channel to reduce the cylinder pressure to absorb the shock. The six-axis IMU returns the adjustment data in real time, which is verified by the upper computer for pose error; Step 3.6: After the execution of the control instructions, the upper computer collects the state data after the adjustment and execution through various sensors, and synchronizes through a unified time stamp; Step 3.7: Based on the feedback data in step 3.6, the platform level state vector and the agricultural machine level state vector of the optimized AKF are updated, and the dynamic correction of state estimation is realized. After the state vector is updated, it replaces the original initial state vector as the "current state reference" for the next round of data fusion of the optimized AKF algorithm, avoiding state estimation drift caused by execution deviation.

[0011] Further, in step 3.4, the specific instruction values of the control instructions generated by the upper computer are as follows:

[0012]

[0013] wherein, is the proportional gain coefficient of the leveling controller, is the gain coefficient of the rotation speed controller; represents the centroid height correction coefficient; This indicates the current yaw rate of the transfer platform; This indicates the increment of the pitch angle that the transfer platform needs to adjust; Indicates the target's yaw rate; This represents the lateral displacement of the agricultural machinery's center of mass in the platform coordinate system.

[0014] Furthermore, the weighting method in step 3.3 is as follows:

[0015]

[0016] in, As weight, For the first point cloud The height coordinates of each point For the first point cloud The coordinate vector of a point, It is a very small positive number. This represents the total number of points in the point cloud cluster belonging to agricultural machinery. Indicating the first point cloud The horizontal coordinates of the points Indicating the first point cloud The vertical coordinates of each point.

[0017] Furthermore, in step 4, the hierarchical response mechanism specifically includes: When the missing point cloud rate of the lidar reaches the set threshold, it switches to the tight coupling mode of the six-axis IMU and the binocular camera and increases the AKF prediction step size; if the Z-axis response time exceeds the time limit, the proportional valve controller switches to the fully open mode to realize the full-speed output of the Z-axis pneumatic cylinder; when the binocular camera detects an obstacle within 1 meter of the track, the PLC immediately cuts off the power to the X-axis servo motor and the Y-axis servo motor and causes the Z-axis pneumatic cylinder to depressurize to a safe height.

[0018] The present invention has the following beneficial effects: This invention constructs a complete "perception-decision-execution" closed-loop control system by integrating a three-degree-of-freedom dynamic control platform with an optimized AKF algorithm. This solves the safety hazards existing in traditional hilly and mountainous rail agricultural machinery transportation. It adopts pneumatic active damping and linear motor modules working together to improve the reliability of sensor data. By optimizing the AKF algorithm, it realizes the position tracking and adjustment of agricultural machinery and the overturning warning, providing a safety guarantee technical solution for the transportation of agricultural machinery in hilly and mountainous areas. The dual-layer control architecture (GPU decision + PLC execution) reduces the response time to sudden overturning, and, together with the hydraulic leveling system, realizes real-time tilt angle compensation, providing a highly reliable technical guarantee for intelligent agricultural machinery equipment for mechanized agricultural transportation. Attached Figure Description

[0019] Figure 1 Figure 3 is a schematic diagram of the three-dimensional structure of the track agricultural machinery transfer platform system; Figure 2 Figure 4 is a schematic diagram of the three-dimensional structure of the fixed base; Figure 3 Figure 5 is a schematic diagram of the three-dimensional structure of the dynamic control platform; Figure 4 Figure 6 is a schematic diagram of the sensor installation; Figure 5 Figure 7 is a schematic diagram of the installation of each component inside the platform side box; Figure 6 Figure 8 is a schematic diagram of the optimized AKF algorithm process; Figure 7 Figure 9 is a logic diagram of the hierarchical response mechanism.

[0020] In the figure: 101-aluminum alloy shockproof base; 102-shock-absorbing rubber pad; 103-bolt hole; 104-platform support frame; 201-platform mounting plate; 202-six-axis IMU; 203-X-axis linear motor module; 204-Y-axis linear motor module; 205-Z-axis pneumatic cylinder A; 206-Z-axis pneumatic cylinder B; 301-track: 401-platform side box; 501-upper computer; 601-binocular camera; 701-laser radar; 801-X-axis servo motor driver; 802-X-axis servo motor; 803-Y-axis servo motor driver; 804-Y-axis servo motor; 805-proportional valve controller DETAILED DESCRIPTION

[0021] The application will be further described below in conjunction with the accompanying drawings and specific embodiments, but the scope of protection of the application is not limited thereto.

[0022] The track agricultural machinery transfer platform system based on the optimized AKF algorithm for dynamic pose adjustment provided by the application comprises a fixed base, a dynamic control platform, and a control system. The fixed base is used to provide a stable installation foundation. The dynamic control platform is installed on the fixed base and is used to carry the agricultural machinery to be transferred and realize three-dimensional space pose adjustment. The control system is electrically connected with the dynamic control platform and is used to process collected data, execute the optimized adaptive Kalman filter (AKF) algorithm, and output control instructions.

[0023] As Figure 1 , 2As shown, the fixed base includes an aluminum alloy shockproof base 101, a shock-absorbing rubber pad 102, and a platform support frame 104. The fixed base adopts a layered assembly structure, the aluminum alloy shockproof base 101 is installed on the platform support frame 104, the platform support frame 104 is located on the track 301 and can slide on the track 301 to realize the track transportation operation of the agricultural equipment, the specific design and movement control of the platform support frame 104 are not within the research scope of the present application, and a traditional transportation structure can be used, so it is not repeated here; the aluminum alloy shockproof base 101 is provided with bolt holes 103 at four corners, the bolt holes 103 are distributed in cross symmetry, and the aluminum alloy shockproof base 101 is fixed on the platform support frame 104 by bolts; the aluminum alloy shockproof base 101 is processed with a dovetail-shaped positioning groove at the bottom, which cooperates with the protruding part of the shock-absorbing rubber pad 102 to ensure that the shock-absorbing rubber pad 102 is tightly installed at the bottom of the aluminum alloy shockproof base 101.

[0024] As shown in Figure 1 , 3 , 4, 5, the dynamic control platform includes a platform mounting plate 201, an X-axis linear motor module 203, a Y-axis linear motor module 204, a Z-axis pneumatic cylinder A 205, a Z-axis pneumatic cylinder B 206, a six-axis IMU 202, a binocular camera 601, a laser radar 701, and a platform side box 401. The platform side box 401 is fixed on the aluminum alloy shockproof base 101 and located at the front end of the platform mounting plate 201 and the aluminum alloy shockproof base 101, serving as a bearing platform for various data acquisition devices and power output devices. The six-axis IMU 202 is symmetrically arranged at the four corners of the bottom of the platform mounting plate 201, which can realize the purpose of functional separation and structural balance, and is used for measuring the angular velocity and linear acceleration of the platform mounting plate 201. The laser radar 701 is installed at the top of the platform side box 401 and is used for scanning and calculating the real-time terrain gradient; the binocular camera 601 is installed at the top of the platform side box 401 and is used for acquiring three-dimensional point cloud data of the agricultural machine; all sensor data are synchronized through a unified timestamp to ensure the time consistency when data fusion. The fusion perception system composed of the binocular camera 601 and the laser radar 701 is integrated above the platform side box 401 to maximize the front view perception ability.

[0025] As shown in Figure 1 , 3As shown in FIGS. 4, 5, the X-axis servo motor driver 801, the X-axis servo motor 802, the Y-axis servo motor driver 803, the Y-axis servo motor 804 and the proportional valve controller 805 are fixed inside the platform side box 401. The movement of the X-axis linear motor module 203 is controlled by the X-axis servo motor 802, and the base of the X-axis linear motor module 203 is fixed on the aluminum alloy shockproof base 101. The movement of the Y-axis linear motor module 204 is controlled by the Y-axis servo motor 804, and the base of the Y-axis linear motor module 204 is connected to the slide table of the X-axis linear motor module 203 through a flange. The connecting surface is coated with conductive paste and locked with screws, and the platform mounting plate 201 is fixed on the slide table of the Y-axis linear motor module 204.

[0026] As shown in FIGS. 4, 5, the X-axis servo motor driver 801, the X-axis servo motor 802, the Y-axis servo motor driver 803, the Y-axis servo motor 804 and the proportional valve controller 805 are fixed inside the platform side box 401. The movement of the X-axis linear motor module 203 is controlled by the X-axis servo motor 802, and the base of the X-axis linear motor module 203 is fixed on the aluminum alloy shockproof base 101. The movement of the Y-axis linear motor module 204 is controlled by the Y-axis servo motor 804, and the base of the Y-axis linear motor module 204 is connected to the slide table of the X-axis linear motor module 203 through a flange. The connecting surface is coated with conductive paste and locked with screws, and the platform mounting plate 201 is fixed on the slide table of the Y-axis linear motor module 204. Figure 3 4 As shown in FIGS. 4, 5, the X-axis servo motor driver 801, the X-axis servo motor 802, the Y-axis servo motor driver 803, the Y-axis servo motor 804 and the proportional valve controller 805 are fixed inside the platform side box 401. The movement of the X-axis linear motor module 203 is controlled by the X-axis servo motor 802, and the base of the X-axis linear motor module 203 is fixed on the aluminum alloy shockproof base 101. The movement of the Y-axis linear motor module 204 is controlled by the Y-axis servo motor 804, and the base of the Y-axis linear motor module 204 is connected to the slide table of the X-axis linear motor module 203 through a flange. The connecting surface is coated with conductive paste and locked with screws, and the platform mounting plate 201 is fixed on the slide table of the Y-axis linear motor module 204.

[0027] The dynamic regulation platform realizes global collaborative perception with dynamic error compensation by integrating the six-axis IMU 202. The six-axis IMU 202 captures the micro-vibration and attitude change of the dynamic regulation platform in real time, provides compensation reference for the data of the four-corner perception array, eliminates the measurement error caused by mechanical vibration, and ensures the positioning accuracy in high-speed motion state.

[0028] ​The controller hardware in the control system responds to the in-orbit position information received based on the optimized AKF algorithm to complete dynamic pose adjustment; the control system comprises an upper computer 501, a lower computer, an X-axis servo motor driver 801, an X-axis servo motor 802, a Y-axis servo motor driver 803, a Y-axis servo motor 804; the upper computer 501 is an industrial-grade embedded GPU, and a power module is additionally installed, and the upper computer 501 is installed on the side box 401 of the platform, the output end of the power module is connected in series with a self-recovery fuse, the upper computer 501 is connected with a binocular camera 601 through a PCIe x4 interface and connected with a laser radar 701 through a USB interface; the lower computer is a real-time PLC, which is connected with the X-axis servo motor driver 801, the X-axis servo motor 802, the Y-axis servo motor driver 803, the Y-axis servo motor 804 and the proportional valve controller 805 through an EtherCAT bus, and the PLC realizes the driving of the X-axis servo motor 802 and the Y-axis servo motor through a transmission line, and realizes the accurate positioning and control adjustment of in-orbit sensing of each module.

[0029] The closed-loop control method of the track agricultural machine transfer platform system based on the optimized AKF algorithm for realizing dynamic pose adjustment, comprises the following processes: Step 1: the PLC first sends instructions to the X-axis servo motor driver 801, the X-axis servo motor 802, the Y-axis servo motor driver 803 and the Y-axis servo motor 804 through the EtherCAT bus, and first performs self-checking, the X-axis linear motor module 203 and the Y-axis linear motor module 204 execute zero-point calibration, and the sliding table retreats to the mechanical limit trigger switch; the Z-axis pneumatic cylinder A 205 and the Z-axis pneumatic cylinder B 206 adjust the air inlet amount through the proportional valve controller 805, so that the piston rod rises to the position of the horizontal zero point of the platform mounting plate 201.

[0030] Step 2: the transfer platform track 301 walks on the agricultural machine equipment, in this process, the embedded GPU of the upper computer 501 activates the sensor array, starts the binocular camera 601 through the PCIe x4 interface to complete binocular image calibration, drives the laser radar 701 through the USB3.0 interface to scan the track 301 plane to construct an initial point cloud map, and real-time collects three-axis acceleration / angle speed data of the six-axis IMU 202; all sensor data are transmitted back to the upper computer 501 through the shielding cable, and an initial pose matrix is generated by the upper computer 501, then an adaptive Kalman filtering algorithm is used as a core fusion framework to cope with sensor noise time-varying and motion uncertainty, so as to achieve the purpose of clearly knowing the position information of the agricultural machine on the platform, and to give a reference for subsequent real-time data change adjustment.

[0031] Step 3: The host computer 501 continuously receives multi-source sensor data streams, obtains the relative pose of the agricultural machine through the binocular camera 601, registers and outputs the pose through the laser radar 701, and updates the attitude angle through the six-axis IMU 202, forms two state vectors at the platform level and the agricultural machine level, and optimizes the AKF algorithm in the process, and finally outputs the fused pose estimation value and the covariance matrix.

[0032] The optimized AKF algorithm in the embodiment utilizes multi-source data of the binocular camera 601, the laser radar 701 and the six-axis IMU 202 to establish a coupled dynamic state model, integrates the physical correlation of vision, three-dimensional point cloud and inertial motion to accurately depict the system motion; at the same time, dynamically adjusts the process noise covariance and the measurement noise covariance, and fuses multi-source data through weighted centroid, adjusts the noise parameters and fusion weights according to the risk assessment results, and forms a closed-loop optimization; the three-layer innovative design of the optimized AKF algorithm is carried out from different levels: the first layer is the state model layer, which breaks through the limitation of traditional single model, integrates the visual constraint of the binocular camera 601, the three-dimensional constraint of the laser radar 701 and the inertial constraint of the six-axis IMU 202, and constructs a coupled dynamic equation containing position, velocity, attitude and other multi-dimensional; the second layer is the noise processing layer, which dynamically adjusts the process noise covariance based on the motion mode, and maps the measurement noise into an adjustment coefficient according to the sensor performance index, realizes the adaptive matching of noise characteristics; the third layer is the decision and feedback layer, which adjusts the noise covariance and the sensor fusion weight through the trace of the state estimation covariance matrix and the statistical risk level of the innovation sequence, combines the control instruction and the early warning signal output, and achieves the collaborative optimization of algorithm accuracy, robustness and safety; Through the three-layer innovative design, the position tracking and rollover warning of the agricultural machine are realized, and the specific process of data processing by the optimized AKF algorithm is as follows: Figure 6 Step 3.1: First, two coupled state models are established to accurately describe the dynamic of the transfer platform: The platform level state vector is:

[0033] The related parameters of the platform level state vector can be directly obtained through the six-axis IMU 202; wherein, is the roll angle of the transfer platform, is the pitch angle of the transfer platform, is the longitudinal velocity of the transfer platform, is the lateral velocity of the transfer platform, is the rotational angular velocity of the transfer platform.

[0034] The agricultural machine level state vector is: ​

[0035] The agricultural machine hierarchical state vector is used to represent the displacement of the agricultural machine relative to the center of the transfer platform and its rate of change; wherein, 、 、 are the longitudinal, lateral and vertical displacements of the agricultural machine mass center in the platform coordinate system respectively, 、 、 are the corresponding rates of change, i.e. the longitudinal, lateral and vertical displacement rates of the agricultural machine mass center in the platform coordinate system.

[0036] The coupling dynamics equation is introduced, taking into account the platform acceleration, slope gravity component and friction coefficient, to provide a mathematical model closer to the real scene for the transfer platform: ; wherein, represents the acceleration of the transfer platform, represents the equivalent damping coefficient between the transfer platform and the agricultural machine, represents the gravitational acceleration, represents the longitudinal relative displacement of the agricultural machine mass center, represents the longitudinal relative acceleration of the agricultural machine mass center.

[0037] Step 3.2: The fixed noise parameters of the traditional AKF algorithm cannot cope with complex terrain, and the present invention designs a real-time terrain data-driven noise regulator, so that the process noise covariance and the measurement noise covariance can be dynamically adjusted in real time; when the terrain slope becomes larger, the process noise is automatically increased, indicating that the uncertainty of the model prediction is increased, and the filter will trust the sensor observation value (i.e. the measurement noise covariance formula) more, when the instantaneous angular acceleration of the transfer platform increases sharply, the measurement noise of the six-axis IMU 202 increases, and the filter turns to trust the prediction of the dynamics model (i.e. the process noise covariance formula) more; wherein, and are adjusted according to the following formula:

[0038] wherein, represents the initial process noise covariance, represents the adjustment coefficient, represents the real-time terrain gradient, represents the initial measurement noise covariance, represents the adjustment coefficient, the coefficient and need to be calibrated through preliminary field tests.

[0039] Step 3.3: The center of mass of the agricultural machine is the key to the risk judgment of tipping over; after obtaining the original point cloud from the binocular camera 601, the ground point cloud is segmented and the point cloud cluster belonging to the agricultural machine is separated; a weighting method inversely proportional to the Z coordinate is adopted, which makes the points closer to the bottom of the transfer platform contribute more to the calculation of the center of mass, thereby sensitively capturing the change in the height of the center of mass and the lifting trend of the agricultural machine due to tilting, providing an early warning for risk prediction; the weighting method is as follows:

[0040]

[0041] wherein, is the weight, is the height coordinate of the i-th point in the point cloud, is the coordinate vector of the i-th point in the point cloud, is a very small positive number, and is the weighted center of mass, i.e., the center of mass height of the agricultural machine; represents the total number of points in the point cloud cluster belonging to the agricultural machine, represents the lateral coordinate of the i-th point in the point cloud, represents the longitudinal coordinate of the i-th point in the point cloud. Step 3.4: input the detected agricultural machine position and the real-time center of mass height of the agricultural machine into the decision-making judgment, which will evaluate the risk according to two core criteria, as follows:

[0042] wherein, represents the track gauge, represents the length of the platform mounting plate 201, represents the safety factor,

[0043] represents the pitch angle of the platform mounting plate 201; represents the square of the longitudinal speed of the transfer platform; If any criterion is triggered, the upper computer 501 immediately generates a control instruction; for lateral imbalance, a leveling instruction is generated and sent to the dynamic control platform; for longitudinal slip or overall risk, a speed reduction or rotation instruction is generated and sent to the dynamic control platform, thereby forming a closed loop from perception to decision-making to execution; the specific instruction values are as follows:

[0044]

[0045] wherein, ​​​​To adjust the proportional gain coefficient of the controller, Gain coefficient of the rotation speed controller; This represents the centroid height correction factor; This indicates the current yaw rate of the transfer platform; This indicates the increment of the pitch angle that the transfer platform needs to adjust; Indicates the target's yaw rate; Step 3.5: Execution of control instructions; The host computer 501 sends the control instructions generated in step 3.4 to the PLC, and the PLC calculates the positional deviation of the agricultural machinery. The target displacement is converted into X and Y axis linear motor modules, and its control law follows the negative feedback principle: the movement direction of the slide is always opposite to the direction of the agricultural machinery's center of gravity offset, so as to make it return to the stable center of the platform; to ensure smooth movement, the PLC sends motion commands to the X-axis servo motor driver 801 and the Y-axis servo motor driver 803 via the EtherCAT bus. The motion controller integrated inside the driver will use S-shaped acceleration and deceleration curve planning to make the speed of the slide change continuously, effectively avoiding rigid impact and vibration during the start and stop phases; for the Z-axis deviation, the proportional valve controller 805 uses the vertical acceleration collected by the six-axis IMU202. The air path is adjusted. When a sudden impact is detected, the pneumatic active damper instantly opens the pressure relief channel to reduce the pressure inside the cylinder to absorb the vibration. The six-axis IMU202 transmits the adjustment data back in real time, and the host computer 501 verifies the position and posture error.

[0046] Step 3.6: After the control command is executed, the host computer 501 collects the status data after the adjustment is executed through the following sensors and synchronizes it through a unified timestamp: The six-axis IMU202 data acquisition and transfer platform can collect real-time roll angle, pitch angle, longitudinal velocity, lateral velocity, and rotational angular velocity. The binocular camera 601 collects real-time relative displacement data of agricultural machinery. The LiDAR 701 collects track terrain gradient and agricultural machinery point cloud cluster update data (used to correct the weighted centroid). Step 3.7: Based on the feedback data from Step 3.6, update and optimize the AKF platform-level state vector. Agricultural machinery hierarchical state vector This enables dynamic correction of the state estimate; after the state vector is updated, it replaces the original initial state vector. , This serves as the "current state baseline" for the next round of AKF data fusion, avoiding state estimation drift caused by execution bias.

[0047] Step 4: Refer to Figure 7, In the face of unexpected situations, the system starts a hierarchical response mechanism. When the point cloud missing rate of the laser radar 701 reaches a certain value, it switches to a tightly coupled mode of the six-axis IMU 202 and the binocular camera 601 and expands the AKF prediction step. If the Z-axis response time is overdue, the proportional valve controller 805 switches to the full open mode to realize the full speed output of the Z-axis pneumatic cylinder. When the binocular camera 601 detects an obstacle within 1 meter of the track 301, the PLC immediately cuts off the power supply of the X-axis servo motor 802 and the Y-axis servo motor 804 and allows the Z-axis pneumatic cylinder to be depressurized to a safe height. The self-restoring fuse of the power module provides hardware-level protection when the current is overloaded.

[0048] Step 5: After the adjustment is completed, the adhesion of the agricultural machine and the transfer platform is detected by the binocular camera 601, and the horizontal degree graph is generated by the planar scanning of the laser radar 701. The upper computer 501 continuously optimizes the AKF parameters, and if the pose prediction residual increases for 10 consecutive times, the noise covariance initial value is reset. The adjustment data including the actuator displacement curve, sensor confidence distribution and energy consumption statistics are finally output, realizing dynamic pose adjustment within the period, and completing the on-orbit state perception.

[0049] The embodiments are preferred embodiments of the present application, but the present application is not limited to the above embodiments. Any obvious improvements, replacements or modifications made by those skilled in the art without departing from the essential content of the present application shall fall within the protection scope of the present application.

Claims

1. A track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, characterized in that, It includes a fixed base, a dynamic control platform, and a control system. The fixed base provides the installation foundation. The dynamic control platform is installed on the fixed base to support the agricultural machinery to be transported and to realize the three-dimensional posture adjustment. The control system is electrically connected to the dynamic control platform and is used to process the collected data, execute the optimized AKF algorithm, and output control commands to the dynamic control platform. The dynamic control platform includes an X-axis linear motor module (203) with its base fixed on a fixed base. The slide of the X-axis linear motor module (203) is connected to the base of the Y-axis linear motor module (204). A platform mounting plate (201) is fixed on the slide of the Y-axis linear motor module (204). The bottom of the cylinder bodies of two Z-axis pneumatic cylinders are respectively positioned and installed in the non-stroke areas at the front and rear ends of the Y-axis linear motor module (204). The top of the piston rods of the Z-axis pneumatic cylinders are rigidly connected to the platform mounting plate (201). A six-axis IMU (202) is arranged at the four corners of the bottom of the platform mounting plate (201). A binocular camera (601) and a lidar (701) are installed on the top of the platform side box (401). The dynamic control platform also includes a platform side box (401) fixed at the end of the fixed base. The control system includes an X-axis servo motor driver (801), an X-axis servo motor (802), a Y-axis servo motor driver (803), a Y-axis servo motor (804), a proportional valve controller (805), a host computer (501), and a slave computer, all fixed inside the side box (401) of the platform. The host computer (501) is connected to a binocular camera (601) via a PCIe x4 interface and to a lidar (701) via a USB interface. The slave computer is a real-time PLC, which is connected to the X-axis servo motor driver (801), the X-axis servo motor (802), the Y-axis servo motor driver (803), the Y-axis servo motor (804), and the proportional valve controller (805) via an EtherCAT bus.

2. The track-based agricultural machinery transfer platform system based on the optimized AKF algorithm for dynamic pose adjustment as described in claim 1, characterized in that, The fixed base includes an aluminum alloy anti-vibration base (101), a shock-absorbing rubber pad (102), and a platform support frame (104). The aluminum alloy anti-vibration base (101) is fixed to the platform support frame (104) by bolts. The platform support frame (104) is located on the track (301) and can slide on the track (301). The bottom of the aluminum alloy anti-vibration base (101) is machined with a dovetail-shaped positioning groove, which cooperates with the protruding part of the shock-absorbing rubber pad (102) to ensure that the shock-absorbing rubber pad (102) is tightly fitted and installed at the bottom of the aluminum alloy anti-vibration base (101).

3. A closed-loop control method for a track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, as described in claim 1, characterized in that... The process includes the following: Step 1: The PLC first controls each servo motor driver and servo motor to perform self-test, and controls each linear motor module to perform zero-point calibration; the proportional valve controller (805) adjusts the air intake of the Z-axis pneumatic cylinder so that the piston rod rises to the horizontal zero point position of the platform mounting plate (201); Step 2: The transfer platform carries the agricultural machinery and moves on the track (301). During this process, the host computer (501) starts the binocular camera (601) to complete the binocular image calibration, drives the lidar (701) to scan the plane of the track (301) to build the initial point cloud map, and collects the three-axis acceleration / angular velocity data of the six-axis IMU (202) in real time. All sensor data are transmitted back to the host computer (501) through shielded cables, and the host computer (501) fuses them to generate the initial pose matrix. Step 3: The host computer (501) continuously receives data streams from multiple sensors, uses the optimized AKF algorithm to establish a coupled dynamic state model, integrates the physical correlation of vision, 3D point cloud, and inertial motion to describe the motion of the transport platform; at the same time, it dynamically adjusts the process noise covariance and measurement noise covariance, and fuses the multi-source data through weighted centroid, and adjusts the noise parameters and fusion weights in combination with the risk assessment results to form a closed-loop optimization control for the pose adjustment of the transport platform; Step 4: In the event of an emergency, activate the tiered response mechanism; Step 5: After the transfer platform is adjusted, the fit between the agricultural machinery and the transfer platform is detected by the binocular camera (601), and the horizontality map is generated by the plane scanning of the lidar (701); the host computer (501) continuously optimizes the AKF parameters; finally, the adjustment data including the actuator displacement curve, sensor confidence distribution and energy consumption statistics are output to realize dynamic pose adjustment within the cycle and complete the on-orbit status perception.

4. The closed-loop control method for a track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, as described in claim 3, is characterized in that... The specific process of step 3 is as follows: Step 3.1: First, establish two coupled state models to describe the dynamics of the transfer platform, including platform-level state vectors. and agricultural machinery hierarchical state vector By introducing coupled dynamic equations, taking into account platform acceleration, slope gravity components, and friction coefficient, a mathematical model closer to real-world scenarios is provided for the transfer platform. ,in, Indicates the acceleration of the transshipment platform. This represents the equivalent damping coefficient between the transfer platform and the agricultural machinery. Represents gravitational acceleration. This represents the longitudinal relative displacement of the center of gravity of the agricultural machinery. This represents the longitudinal relative acceleration of the center of mass of the agricultural machinery; Step 3.2: Design a real-time terrain data-driven noise conditioner to optimize the noise covariance of the AKF algorithm. and measurement noise covariance All can be dynamically adjusted in real time, and the specific adjustment formula is as follows: ； in, This represents the initial process noise covariance. This represents the adjustment coefficient. Represents real-time terrain gradient. This represents the initial measurement noise covariance. Indicates the adjustment coefficient; Step 3.3: The position of the centroid of the agricultural machinery is the key to the risk assessment of overturning. After obtaining the original point cloud from the binocular camera (601), the ground point cloud is segmented, the point cloud cluster belonging to the agricultural machinery is separated, and the weighted centroid is calculated by using a weighting method inversely proportional to the Z coordinate, so as to provide an advanced judgment for risk warning. Step 3.4: The host computer will detect the location of the agricultural machinery. and the weighted centroid calculated in step 3.3 Input the decision judgment and conduct a risk assessment based on two core criteria, as follows: ； in, Indicates track gauge. This indicates the length of the platform mounting plate (201). Indicates the safety factor. Indicates the pitch angle of the platform mounting plate (201); This represents the square of the longitudinal velocity of the transfer platform; When any criterion is triggered, the host computer (501) immediately generates a control command; Step 3.5: The host computer (501) sends the generated control instructions to the PLC. The PLC sends motion instructions to the X-axis servo motor driver (801) and the Y-axis servo motor driver (803). The motion controller integrated inside the driver will use an S-shaped acceleration and deceleration curve to make the speed of the slide table change continuously. For the Z-axis deviation, the PLC controls the proportional valve controller (805) to adjust the air path according to the vertical acceleration collected by the six-axis IMU (202). When a sudden impact is detected, the pneumatic active damper instantly opens the pressure relief channel to reduce the pressure inside the cylinder to absorb the vibration. The six-axis IMU (202) transmits the adjustment data back in real time, and the host computer (501) verifies the position and posture error. Step 3.6: After the control command is executed, the host computer (501) collects the status data after the adjustment is executed through each sensor and synchronizes it through a unified timestamp; Step 3.7: Based on the feedback data from Step 3.6, update and optimize the platform-level state vector and the agricultural machinery-level state vector of AKF to achieve dynamic correction of state estimation. After the state vector is updated, it replaces the original initial state vector and serves as the "current state reference" for the next round of AKF algorithm data fusion optimization, avoiding state estimation drift caused by execution deviation.

5. The closed-loop control method for a track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, as described in claim 4, is characterized in that... In step 3.4, the specific instruction values ​​of the control instructions generated by the host computer (501) are as follows: ； in, To adjust the proportional gain coefficient of the controller, Gain coefficient of the rotation speed controller; This represents the centroid height correction factor; This indicates the current yaw rate of the transfer platform; This indicates the increment of the pitch angle that the transfer platform needs to adjust; Indicates the target's yaw rate; This represents the lateral displacement of the agricultural machinery's center of mass in the platform coordinate system.

6. The closed-loop control method for a track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment, as described in claim 4, is characterized in that... The weighting method in step 3.3 is as follows: ； in, As weight, For the first point cloud The height coordinates of each point For the first point cloud The coordinate vector of a point, It is a very small positive number. This represents the total number of points in the point cloud cluster belonging to agricultural machinery. Indicating the first point cloud The horizontal coordinates of the points Indicating the first point cloud The vertical coordinates of each point.

7. The closed-loop control method for a track-based agricultural machinery transfer platform system based on an optimized AKF algorithm for dynamic pose adjustment as described in claim 3, characterized in that, In step 4, the hierarchical response mechanism specifically includes: When the point cloud missing rate of the lidar (701) reaches the set threshold, it switches to the tight coupling mode of the six-axis IMU (202) and the binocular camera (601) and expands the AKF prediction step size; if the Z-axis response time exceeds the time limit, the proportional valve controller (805) switches to the fully open mode to realize the full-speed output of the Z-axis pneumatic cylinder; when the binocular camera (601) detects an obstacle within 1 meter of the track (301), the PLC immediately cuts off the power supply of the X-axis servo motor (802) and the Y-axis servo motor (804) and causes the Z-axis pneumatic cylinder to depressurize to a safe height.

Images