Railway agricultural machine transfer platform system and dynamic pose adjustment closed-loop control method thereof

Abstract

The application provides a track agricultural machine transfer platform system and a dynamic pose adjustment closed-loop control method thereof, the system comprises a fixed base, a dynamic regulation platform and a control system, the fixed base is used for providing a mounting base, the dynamic regulation platform is mounted on the fixed base and is used for bearing an agricultural machine to be transferred and realizing three-dimensional space pose adjustment, the control system is electrically connected with the dynamic regulation platform and is used for processing collected data, executing an optimized AKF algorithm and outputting a control instruction. The application combines a three-degree-of-freedom dynamic regulation platform and an optimized AKF algorithm, constructs a complete "perception-decision-execution" closed-loop control system, realizes position tracking adjustment and tipping early warning of the agricultural machine, cooperatively works a pneumatic active damping and a linear motor module, makes sensor data more reliable, provides safety guarantee for hilly and mountainous agricultural machine transfer, and provides high-reliability technical guarantee for intelligent agricultural machine equipment of agricultural mechanization 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 and a dynamic pose adjustment closed-loop control method thereof. BACKGROUND

[0002] The cultivated land in China's hilly and mountainous areas accounts for more than 30%, due to large terrain undulations, broken land blocks, and a general slope of 10° to 25°. Agricultural machines are relatively difficult to transfer between them, and in complex mountainous environments, they face many challenges. Long-term bearing can cause local bending of the double-track rail, 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's 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-type walking mechanism for 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 testing show that the single-axis sensor cannot capture the multi-dimensional attitude changes in complex mountainous areas, and no anti-interference solution 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 in response to dynamic vibration errors caused by rail bending, making it difficult to meet the real-time attitude stabilization requirements under high-frequency vibration.

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

[0004] In view of the deficiencies in the prior art, the present application provides a track agricultural machine transfer platform system and a dynamic pose adjustment closed-loop control method, 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, comprising a fixed base, a dynamic control platform, and a control system. The fixed base is used to provide an installation foundation. The dynamic control platform is installed on the fixed base and is used to carry agricultural machines to be transferred and to adjust the pose in three-dimensional space. The control system is electrically connected to the dynamic control platform and is used to process collected data, execute an optimized AKF algorithm, and output control instructions to the dynamic control platform.

[0007] 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 mounted 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;

[0008] 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.

[0009] 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.

[0010] A dynamic pose adjustment closed-loop control method of the above-mentioned track agricultural machine transfer platform system comprises the following processes:

[0011] Step 1: the PLC first controls each servo motor driver and servo motor to perform self-checking, controls each 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 so that the piston rod is lifted to the position of the horizontal zero point of the platform mounting plate;

[0012] Step 2: the transfer platform carries the agricultural machine 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 is transmitted back to the upper computer through a shielded cable, and an initial pose matrix is generated by the upper computer through fusion;

[0013] 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; simultaneously, 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;

[0014] Step 4: In the face of sudden conditions, start the hierarchical response mechanism;

[0015] Step 5: After the transfer platform adjustment is completed, the binocular camera detects the adhesion of the agricultural machine and the transfer platform, and the laser radar plane scanning generates a levelness graph; 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 within the cycle and completing on-orbit state perception.

[0016] Further, the specific process of step 3 is as follows:

[0017] Step 3.1: First, establish two coupled state models to describe the transfer platform dynamics, including the platform level state vector X p and the agricultural machine level state vector X a ; introduce the coupled dynamics equation, consider the platform acceleration, slope gravity component, and equivalent damping coefficient, and provide a mathematical model closer to the real scene for the transfer platform: Where a platform represents the transfer platform acceleration, μ represents the equivalent damping coefficient between the transfer platform and the agricultural machine, g represents the gravity acceleration, d x represents the longitudinal relative displacement of the agricultural machine centroid, represents the longitudinal relative acceleration of the agricultural machine centroid;

[0018] Step 3.2: Design a real-time terrain data-driven noise adjuster, so that the process noise covariance Q k and the measurement noise covariance R IMU of the optimized AKF algorithm can be dynamically adjusted in real time, and the specific adjustment formula is as follows:

[0019]

[0020] Where Q0 represents the initial process noise covariance, k q represents the adjustment coefficient, represents the real-time terrain gradient, R0 represents the initial measurement noise covariance, k r represents the adjustment coefficient;

[0021] 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.

[0022] Step 3.4: The host computer inputs the detected agricultural machine position (d x , d y ) and the weighted centroid h centroid calculated in step 3.3 into the decision-making judgment, and conducts risk assessment according to the two core criteria, which are as follows:

[0023]

[0024] Wherein, L track represents the track gauge, L platform represents the length of the platform mounting plate, K safe represents the safety factor, and θ represents the pitch angle of the platform mounting plate; v x 2 represents the square of the longitudinal speed of the transfer platform.

[0025] If any criterion is triggered, the host computer immediately generates a control command.

[0026] Step 3.5: The host computer sends the generated control command to the PLC, and the PLC sends the motion command to the X-axis servo motor driver and the 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 air path according to 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 host computer for pose error.

[0027] Step 3.6: After the execution of the control command, the host computer collects the state data after the adjustment and execution through various sensors, and synchronizes through a unified time stamp.

[0028] Step 3.7: Based on the feedback data of 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.

[0029] Further, in step 3.4, the specific instruction values of the control command generated by the host computer are as follows:

[0030]

[0031] wherein, K p is the proportional gain coefficient of the leveling controller, K w is the gain coefficient of the rotation speed controller; K h represents the centroid height correction coefficient; ω now represents the current yaw angular velocity of the transfer platform; Δθ represents the pitch angle increment that the transfer platform needs to adjust; ω new represents the target yaw angular velocity; d y is the lateral displacement of the centroid of the agricultural machine in the platform coordinate system.

[0032] Further, the weighting manner in step 3.3 is as follows:

[0033]

[0034] wherein, ω i is the weight, Z i is the height coordinate of the i th point in the point cloud, p i is the coordinate vector of the i th point in the point cloud, ∈ is a very small positive number, m represents the total number of points in the point cloud cluster belonging to the agricultural machine, x i represents the lateral coordinate of the i th point in the point cloud, y i represents the longitudinal coordinate of the i th point in the point cloud.

[0035] Further, in step 4, the hierarchical response mechanism specifically comprises:

[0036] When the laser radar point cloud loss rate reaches the set threshold, the six-axis IMU and the binocular camera are switched to the tight coupling mode, and the AKF prediction step is expanded; if the Z-axis response time is overdue, the proportional valve controller is switched to the full 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 supply of the X-axis servo motor and the Y-axis servo motor and makes the Z-axis pneumatic cylinder depressurize to a safe height.

[0037] The present application has the following beneficial effects:

[0038] The present application fuses a three-degree-of-freedom dynamic regulation platform and an optimized AKF algorithm, constructs a complete "perception-decision-execution" closed-loop control system, solves the safety hidden trouble problem existing in the traditional hilly and mountainous track agricultural machine transportation, cooperates the pneumatic active damping and the linear motor module to make the sensor data reliability improve, realizes the position tracking adjustment and the rollover early warning of the agricultural machine through the optimized AKF algorithm, provides a safe guarantee technical scheme for the hilly and mountainous agricultural machine transfer, reduces the sudden overturning response time through the double-layer control architecture (GPU decision + PLC execution), and realizes the real-time compensation of the inclination angle in cooperation with the hydraulic leveling system, and provides a high reliability technical guarantee for the intelligent agricultural machinery equipment of agricultural mechanization transportation. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 Figure 1 is a schematic diagram of the three-dimensional structure of the track agricultural machine transfer platform system;

[0040] Figure 2 Figure 3 is a schematic diagram of the three-dimensional structure of the fixed base;

[0041] Figure 3 Figure 4 is a schematic diagram of the three-dimensional structure of the dynamic regulation platform;

[0042] Figure 4 Figure 5 is a schematic diagram of the sensor installation;

[0043] Figure 5 Figure 6 is a schematic diagram of the installation of various components inside the platform side box;

[0044] Figure 6 Figure 7 is a schematic diagram of the optimization AKF algorithm process;

[0045] Figure 7 Figure 8 is a logic diagram of the hierarchical response mechanism.

[0046] 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

[0047] 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.

[0048] The track agricultural machine transfer platform system described in the application comprises a fixed base, a dynamic regulation platform, and a control system. The fixed base is used to provide a stable installation foundation; the dynamic regulation platform is installed on the fixed base and is used to carry the agricultural machine to be transferred and realize the pose adjustment in three-dimensional space; and the control system is electrically connected with the dynamic regulation platform and is used to process the collected data, execute the optimized adaptive Kalman filter (AKF) algorithm, and output the control instruction.

[0049] 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.

[0050] 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.

[0051] 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.

[0052] 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.

[0053] 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.

[0054] ​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.

[0055] The dynamic pose adjustment closed-loop control method of the track agricultural machine transfer platform system comprises the following processes:

[0056] 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, and 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.

[0057] Step 2: The agricultural machine equipment walks on the transfer platform track 301, 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 collects three-axis acceleration / angular velocity data of the six-axis IMU 202 in real time; all sensor data is transmitted back to the upper computer 501 through the shielding cable, and the initial pose matrix is generated by the upper computer 501, and then the adaptive Kalman filtering algorithm is used as the 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 provide a reference for subsequent real-time data change adjustment.

[0058] 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 of 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.

[0059] 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;

[0060] Through the three-layer innovative design, the position tracking and the rollover early warning of the agricultural machine are realized, and the specific process of data processing by the optimized AKF algorithm is as follows: Figure 6

[0061] Step 3.1: First, two coupled state models are established to accurately describe the dynamic of the transfer platform:

[0062] Among them, the platform level state vector X p is:

[0063] X p =[θ roll ,θ pitch ,V x ,V y ,ω z ] T

[0064] The related parameters of the platform level state vector can be directly obtained through the six-axis IMU 202; wherein, θ roll is the roll angle of the transfer platform, θ pitch is the pitch angle of the transfer platform, V​x V is the longitudinal velocity of the transport platform y ω is the lateral velocity of the transport platform z is the angular velocity of the transport platform.

[0065] X is the agricultural machine level state vector a is:

[0066]

[0067] The agricultural machine level state vector is used to characterize the displacement of the agricultural machine relative to the center of the transport platform and its rate of change; wherein d x , d y , d z are the longitudinal, lateral, and vertical displacements of the center of mass of the agricultural machine in the platform coordinate system, are the corresponding rates of change, i.e., the longitudinal, lateral, and vertical displacement rates of the center of mass of the agricultural machine in the platform coordinate system.

[0068] The coupling dynamics equation is introduced, taking into account the platform acceleration, slope gravity component, and equivalent damping coefficient, to provide a mathematical model for the transport platform that is closer to the real scenario:

[0069]

[0070] wherein a platform represents the acceleration of the transport platform, μ represents the equivalent damping coefficient between the transport platform and the agricultural machine, g represents the acceleration of gravity, d x represents the longitudinal relative displacement of the center of mass of the agricultural machine, represents the longitudinal relative acceleration of the center of mass of the agricultural machine.

[0071] 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 Q k and the measurement noise covariance R IMU 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 observed values of the sensor (i.e., the measurement noise covariance formula), when the instantaneous angular acceleration α t of the transport platform increases sharply, the measurement noise of the six-axis IMU 202 increases, and the filter instead trusts the prediction of the dynamics model (i.e., the process noise covariance formula); wherein Q k and R IMU are adjusted according to the following formula:

[0072]

[0073] wherein Q0 represents the initial process noise covariance, kq denotes an adjustment coefficient, denotes a real-time terrain gradient, R0 denotes an initial measurement noise covariance, k r denotes an adjustment coefficient, the coefficient k q and k r Need to be calibrated through pre-site testing.

[0074] Step 3.3: The position of the center of mass of the agricultural machine is the key to the rollover risk judgment; 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, and providing early warning for risk warning; the weighting method is as follows:

[0075]

[0076] wherein ω i is the weight, Z i is the height coordinate of the i-th point in the point cloud, p i is the coordinate vector of the i-th point in the point cloud, ∈ is a very small positive number, h centroid is the weighted center of mass, i.e., the height of the center of mass of the agricultural machine; m represents the total number of points in the point cloud cluster belonging to the agricultural machine, x i represents the lateral coordinate of the i-th point in the point cloud, y i represents the longitudinal coordinate of the i-th point in the point cloud.

[0077] Step 3.4: input the detected agricultural machine position (d x , d y ) and the real-time center of mass height h centroid of the agricultural machine into the decision-making judgment, which will be evaluated according to two core criteria, as follows:

[0078]

[0079] wherein L track represents the track gauge, L platform represents the length of the platform mounting plate 201, K safe represents the safety factor, θ represents the pitch angle of the platform mounting plate 201; v x 2 represents the square of the longitudinal speed of the transfer platform;

[0080] When any criterion is triggered, the host computer 501 immediately generates control instructions; for lateral imbalance, generate leveling instructions and send to the dynamic control platform; for longitudinal slip or overall risk, generate speed reduction or rotation instructions and send to the dynamic control platform, thus forming a closed loop from perception to decision-making to execution; the specific instruction values are as follows:

[0081]

[0082] wherein K p is the proportional gain coefficient of the leveling controller, K w is the gain coefficient of the rotation speed controller; K h represents the centroid height correction coefficient; ω now represents the current yaw angular velocity of the transfer platform; Δθ represents the pitch angle increment that the transfer platform needs to adjust; ω new represents the target yaw angular velocity;

[0083] Step 3.5: Control instruction execution;

[0084] The host computer 501 sends the control instructions generated in step 3.4 to the PLC, which converts the pose deviation (d x , d y ) of the agricultural machine into target displacement of the X and Y axis linear motor modules, and the control law follows the negative feedback principle: the moving direction of the sliding table is always opposite to the centroid deviation direction of the agricultural machine, so as to make it return to the stable center of the platform; in order to ensure smooth movement, the PLC sends movement instructions to the X-axis servo motor driver 801 and the Y-axis servo motor driver 803 through the EtherCAT bus, and the motion controller integrated in the driver will use an S-shaped acceleration and deceleration curve to plan, so that the speed of the sliding table changes continuously, effectively avoiding the rigid impact and vibration in the start and stop stages; for Z-axis deviation, the proportional valve controller 805 adjusts the air circuit according to the vertical acceleration a z collected by the six-axis IMU 202, and when a sudden impact is detected, the pneumatic active damper instantaneously opens the pressure relief channel to reduce the cylinder pressure to absorb the shock, and the six-axis IMU 202 returns the adjustment data in real time, which is verified by the host computer 501 for pose error.

[0085] Step 3.6: After the execution of the control instructions, the host computer 501 collects the state data after the adjustment execution through the following sensors, and synchronizes through a unified time stamp:

[0086] The six-axis IMU 202 collects the real-time roll angle, pitch angle, longitudinal speed, lateral speed, and rotation angular velocity of the transfer platform;

[0087] The binocular camera 601 collects the real-time relative displacement of the agricultural machine;

[0088] The laser radar 701 collects track terrain gradient and farm machine point cloud cluster update data (for correcting weighted centroid);

[0089] Step 3.7: Based on the feedback data of step 3.6, update the platform level state vector X of the optimized AKF p and the farm machine level state vector X a , realize dynamic correction of state estimation; after updating the state vector, replace the original initial state vector X p , X a as the "current state reference" for the next round of AKF data fusion, to avoid state estimation drift caused by execution deviation.

[0090] Step 4: Refer to Figure 7 In the face of sudden conditions, 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 the 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 makes the Z-axis pneumatic cylinder depressurize to a safe height. The self-restoring fuse of the power module provides hardware-level protection when overcurrent occurs.

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

[0092] The embodiments described above are preferred embodiments of the present application, but the present application is not limited to the above embodiments, and 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 rail-mounted agricultural machinery transfer platform system, 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) whose base is 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 rod of the Z-axis pneumatic cylinder is 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. The fixed base includes an aluminum alloy shock-absorbing base (101), a shock-absorbing rubber pad (102), and a platform support frame (104). The aluminum alloy shock-absorbing 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 shock-absorbing 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 shock-absorbing base (101).

2. A closed-loop control method for dynamic posture adjustment of the track-based agricultural machinery transfer platform system 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 an 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 multiple 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.

3. The dynamic pose adjustment closed-loop control method for the rail-mounted agricultural machinery transfer platform system according to claim 2, 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 the platform-level state vector X. p and the agricultural machinery hierarchical state vector X a By introducing coupled dynamic equations, the platform's acceleration, gradient gravity components, and equivalent damping coefficient are taken into account, providing a mathematical model for the transfer platform that more closely resembles the real-world scenario. Among them, a platform Let μ represent the acceleration of the transfer platform, g represent the equivalent damping coefficient between the transfer platform and the agricultural machinery, and d represent the acceleration due to gravity. x 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 Q during the AKF algorithm optimization process. k and measurement noise covariance R IMU All can be dynamically adjusted in real time, and the specific adjustment formula is as follows: Where Q0 represents the initial process noise covariance, k q This represents the adjustment coefficient. R0 represents the real-time terrain gradient, and R0 represents the initial measurement noise covariance. r 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 transmit the detected location of the agricultural machinery (d) x d y The weighted centroid h calculated in step 3.3 centroid Input the decision judgment and conduct a risk assessment based on two core criteria, as follows: Among them, L track L represents the track gauge. platform K represents the length of the platform mounting plate (201). safe The safety factor is represented by θ, and the pitch angle of the platform mounting plate (201) is represented by v. x 2 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 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.

4. The dynamic pose adjustment closed-loop control method for the track-based agricultural machinery transfer platform system according to claim 3, characterized in that, In step 3.4, the specific instruction values ​​of the control instructions generated by the host computer (501) are as follows: Among them, K p To adjust the proportional gain coefficient of the controller, K w Gain coefficient of the rotation speed controller; K h ω represents the centroid height correction factor; now ω represents the current yaw rate of the transfer platform; Δθ represents the increment of the pitch angle that the transfer platform needs to adjust; new d represents the target yaw rate; y This represents the lateral displacement of the agricultural machinery's center of mass in the platform coordinate system.

5. The dynamic pose adjustment closed-loop control method for the rail-mounted agricultural machinery transfer platform system according to claim 3, characterized in that, The weighting method in step 3.3 is as follows: Where, ω i As the weight, Z i Let p be the height coordinate of the i-th point in the point cloud. i Let x be the coordinate vector of the i-th point in the point cloud, ∈ is a very small positive number, m represents the total number of points in the point cloud cluster belonging to the agricultural machinery, and x i Let y represent the horizontal coordinate of the i-th point in the point cloud. i This represents the vertical coordinate of the i-th point in the point cloud.

6. The dynamic pose adjustment closed-loop control method for the rail-mounted agricultural machinery transfer platform system according to claim 2, 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