A portal lifting stability control feedback system and method applied to a forklift AGV

Abstract

The application discloses a portal lifting stable control feedback system and method applied to a forklift AGV, and the system comprises an IMU, an abnormality judgment module, a speed planning module, a feedforward item determination module, a PID control module and a valve control module. The IMU outputs acceleration data; the abnormality judgment module determines a portal lifting abnormality judgment result according to the acceleration data; the speed planning module obtains a target speed and determines a target acceleration when the portal lifting is not abnormal; the feedforward item determination module determines vertical motion state corresponding data of the portal at the current moment according to the acceleration data, and determines a feedforward item according to the target acceleration and the vertical motion state corresponding data; the PID control module determines a compensated speed tracking control instruction according to the target speed and the feedforward item; and the valve control module determines a proportional valve opening degree according to the control instruction, and executes deceleration stop control when the portal lifting is abnormal. The application can improve the consistency between the portal lifting control and the actual motion state and the operation stability.

Description

Technical Field

[0001] This invention relates to the field of forklift AGV control technology, and in particular to a mast lifting stability control feedback system and method for forklift AGVs. Background Technology

[0002] Forklift AGVs (Automated Guided Vehicles) are widely used in material handling operations in warehousing, logistics, and manufacturing. During high-level racking, aisle positioning, and continuous operation, mast lifting is a critical factor affecting the overall operational status and continuity of the machine. In engineering applications, forklift AGVs operating at high levels typically need to continuously perform mast lifting actions under varying load conditions, work cycles, and site disturbances. The actual motion state of the mast directly impacts the correspondence between mast movement and control execution. When the actual motion state of the mast during lifting fluctuates, and this state is difficult to reflect in the control processing in a timely and accurate manner, the engineering site often exhibits decreased coordination during mast lifting, deviations between control output and actual movement, difficulty in maintaining stable operation, and disruption of operational continuity. Such situations are more likely to limit the consistency of mast lifting operations under high-level, continuous, or complex conditions, further affecting the stable operation and reliability of the forklift AGV in relevant scenarios. Therefore, how to reflect the actual motion state of the mast during the lifting and lowering of the forklift AGV in a timely and accurate manner, and how to maintain a good correspondence between control execution and actual motion, has become a technical issue that needs attention in engineering applications. Summary of the Invention

[0003] This invention proposes a mast lifting stability control feedback system and method for forklift AGVs to improve the problems that the actual motion state of the mast is not easily reflected in a timely and accurate manner during high-level operations of forklift AGVs, and that the consistency between control execution and actual motion is insufficient. It enables a better correspondence between the operating status feedback and control processing during the mast lifting process, thereby supporting the stable operation of forklift AGVs in relevant operating scenarios.

[0004] To achieve the above objectives, a first aspect of the present invention provides a mast lifting stability control feedback system for forklift AGVs, comprising: an IMU, an anomaly detection module, a speed planning module, a feedforward term determination module, a PID control module, and a valve control module; wherein, The IMU is used to output acceleration data; The anomaly detection module is used to determine the anomaly detection result of the gantry lifting based on the acceleration data; The speed planning module is used to obtain a target speed and determine a target acceleration based on the target speed when the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal. The feedforward term determination module is used to determine the vertical motion state corresponding data of the gantry at the current moment based on the acceleration data when the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, and to determine the feedforward term based on the target acceleration and the vertical motion state corresponding data of the gantry at the current moment. The PID control module is used to perform PID speed tracking based on the target speed, obtain the speed tracking result, and determine the compensated speed tracking control command based on the speed tracking result and the feedforward term. The valve control module is used to determine the proportional valve opening based on the compensated speed tracking control command, and to execute deceleration and stop control when the gantry lifting abnormality judgment result indicates that the gantry lifting is abnormal.

[0005] A second aspect of the present invention provides a mast lifting stability control feedback method applied to forklift AGVs, comprising the following steps: Acquire acceleration data from the IMU installed on top of the highest-level gantry; The gantry lifting anomaly is determined based on the acceleration data. If the gantry lifting abnormality judgment result indicates that the gantry lifting is abnormal, deceleration and stop control shall be performed on the gantry lifting; If the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, the target velocity is obtained, and the target acceleration is determined based on the target velocity; The vertical motion state of the gantry at the current moment is determined based on the acceleration data; The feedforward term is determined based on the target acceleration and the corresponding data of the vertical motion state of the gantry at the current moment; PID speed tracking is performed based on the target speed to obtain the speed tracking result, and a compensated speed tracking control command is determined based on the speed tracking result and the feedforward term. The proportional valve opening is determined based on the compensated speed tracking control command.

[0006] Compared with the prior art, the mast lifting stability control feedback system and method provided by the present invention for forklift AGVs have the following technical effects: This gantry lifting stability control feedback system can use acceleration data as the input basis for judging and controlling the gantry's operating status, and can execute deceleration and stop control when abnormal gantry lifting is detected. When the gantry lifting is normal, the system can generate target-side control input from target speed to target acceleration, and can introduce the current vertical motion state data of the gantry and the target acceleration into the feedforward term determination process. At the same time, the system can adopt different control paths under abnormal and normal operating conditions, and can combine the PID speed tracking result with the feedforward term to apply to the compensated speed tracking control command, further implementing it in the proportional valve opening determination. Based on the above overall configuration, the system can improve the coordination between control execution and actual motion state, and can make the state feedback and control processing related to gantry lifting during high-level operations of the forklift AGV form a more coherent execution relationship. Attached Figure Description

[0007] Figure 1 This is a schematic diagram of the architecture of a gantry lifting stability control feedback system applied to a forklift AGV, provided by an embodiment of the present invention; Figure 2 This is a schematic diagram of a mast lifting stability control feedback system for forklift AGVs provided in an embodiment of the present invention; Figure 3 A functional unit block diagram of a gantry lifting stability control feedback system for forklift AGVs provided in an embodiment of the present invention; Figure 4 This is a flowchart illustrating a mast lifting stability control feedback method for forklift AGVs provided in an embodiment of the present invention. Figure 5 A flowchart illustrating another mast lifting stability control feedback method for forklift AGVs provided in this embodiment of the invention; Figure label: 100-Mass lifting stability control feedback system, 10-Forklift AGV, 11-Mass, 12-Highest level mast, 13-IMU, 14-Fork arm, 110-Function module group, 120-Abnormal judgment module, 130-Speed ​​planning module, 140-Feedforward term determination module, 150-PID control module, 160-Valve control module. Detailed Implementation

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

[0009] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0010] Example 1 This embodiment provides a mast lifting stability control feedback system and method for forklift AGVs, which improves the problem that the actual motion state of the mast during mast lifting is not easily reflected in a timely and accurate manner during high-level operations, and that the consistency between control execution and actual motion is insufficient. It enables a better correspondence between the operational status feedback and control processing during mast lifting, thereby supporting the stable operation of forklift AGVs in relevant operational scenarios. This embodiment is applicable to high-level rack picking and placing operations in warehousing, logistics, and manufacturing scenarios, and is particularly suitable for operating environments where forklift AGVs experience load changes, mechanism response fluctuations, and operational disturbances during mast lifting.

[0011] In this embodiment, the gantry lifting stability control feedback system is configured around the state acquisition, anomaly judgment, control processing and execution output during the gantry lifting process, and enters different control paths in abnormal and non-abnormal states based on the state data.

[0012] Combination Figure 1 , Figure 2 As shown, Figure 1 This is a schematic diagram of the architecture of a mast lifting stability control feedback system applied to forklift AGVs. Figure 2 This is a schematic diagram of a mast lifting stability control feedback system applied to a forklift AGV. The forklift AGV 10 includes a mast 11, a top-level mast 12, and forks 14. The mast lifting stability control feedback system 100 is mounted on the forklift AGV 10, and the functional module group 110 is used to support the various functional modules in the system. An IMU 13, as a status acquisition component, is mounted on top of the top-level mast 12 of the forklift AGV 10 and is used to output acceleration data. The acceleration data serves as the input basis for mast operation status judgment and control processing, and is read and processed by relevant modules in the functional module group 110. The IMU is an Inertial Measurement Unit, used to output acceleration data reflecting the mast's motion state.

[0013] Combination Figure 3 As shown, Figure 3This is a block diagram of the functional units of a mast lifting stability control feedback system applied to a forklift AGV. The mast lifting stability control feedback system 100 includes: an IMU 13, an anomaly judgment module 120, a speed planning module 130, a feedforward term determination module 140, a PID control module 150, and a valve control module 160. Specifically, the IMU 13 outputs acceleration data to the anomaly judgment module 120 and the feedforward term determination module 140; the anomaly judgment module 120 determines the mast lifting anomaly judgment result based on the acceleration data; when the mast lifting anomaly judgment result indicates that the mast lifting is not abnormal, the speed planning module 130 and the feedforward term determination module 140 enter the subsequent control processing flow, the PID control module 150 performs PID speed tracking based on the target speed to obtain the speed tracking result, and forms a compensated speed tracking control command based on the speed tracking result and the feedforward term; the valve control module 160 determines the proportional valve opening based on the compensated speed tracking control command; when the mast lifting anomaly judgment result indicates that the mast lifting is abnormal, the valve control module 160 executes deceleration and stop control. Thus, Figure 3 The system shown forms a complete chain in terms of function: "status input - anomaly judgment - control processing - execution output". The PID controller is a proportional-integral-derivative (PID) control method used for speed tracking control based on the target speed.

[0014] In this embodiment, IMU 13 is used to output acceleration data. Preferably, IMU 13 is mounted on top of the highest-level gantry 12, with its mounting direction horizontal to the ground. Its positive Z-axis is set vertically upward, its positive X-axis is set longitudinally forward of the vehicle, and its positive Y-axis is set laterally to the left relative to the positive X-axis. The acceleration data is preferably triaxial acceleration data. To achieve continuous feedback, IMU 13 outputs acceleration data according to a preset control cycle, which can be, for example, 10ms to 100ms.

[0015] Unless otherwise stated, in this embodiment, the state acquisition, anomaly judgment, feedforward term determination, PID speed tracking, and valve control output are all updated according to the preset control cycle. The acceleration data output by IMU 13 is read by the anomaly judgment module 120 and the feedforward term determination module 140 to determine the anomaly judgment result of the gantry lifting and the corresponding data of the vertical motion state of the gantry at the current moment, respectively.

[0016] The anomaly detection module 120 is used to determine the anomaly detection result of the gantry lifting process based on the acceleration data. In this embodiment, the anomaly detection module 120 receives acceleration data from the IMU 13 for the current control cycle and performs judgment processing on the operating status of the gantry lifting process based on the acceleration data. As one possible implementation, the anomaly detection module 120 can classify the gantry operating status at the current moment according to a preset judgment rule and output the anomaly detection result of the gantry lifting process. The anomaly detection result of the gantry lifting process includes at least two states, namely, an abnormal gantry lifting state and a normal gantry lifting state. In this embodiment, the anomaly detection module 120 preferably makes a judgment based on whether the acceleration data in the current control cycle falls within a preset normal state range. When the acceleration data falls within the preset normal state range, it is determined that the gantry lifting process is normal; when the acceleration data does not fall within the preset normal state range, it is determined that the gantry lifting process is abnormal. In other embodiments, the change in acceleration data between adjacent control cycles can also be used for auxiliary judgment. In a preferred implementation, the anomaly determination can also be based on the magnitude of the triaxial acceleration data, the horizontal acceleration component, and the acceleration change between adjacent control cycles. For a more detailed implementation, please refer to Embodiment 2.

[0017] The anomaly detection module 120 can perform status judgment on the acceleration data output by IMU 13 in each control cycle, and output the gantry lifting anomaly judgment result corresponding to the current moment for use in subsequent control path branches.

[0018] In this embodiment, the gantry lifting anomaly judgment result serves as a path branch. When the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, the speed planning module 130 enters the working state and continues to generate target-side control input, while the feedforward term determination module 140 enters the working state and generates actual state-side input and feedforward terms. When the gantry lifting anomaly judgment result indicates that the gantry lifting is abnormal, the valve control module 160 enters the abnormal control path and executes deceleration and stop control. Thus, the anomaly judgment module 120 not only undertakes the function of state judgment but also divides the system control flow into normal control paths and abnormal control paths.

[0019] The speed planning module 130 is used to obtain the target speed and determine the target acceleration based on the target speed when the gantry lifting abnormality judgment result indicates that the gantry lifting is not abnormal, so as to form the target side input under the normal control path.

[0020] As a direct implementation method, the speed planning module 130 can determine the target speed at the current moment based on the current gantry lifting control task. The target speed can be a target speed value in the gantry lifting direction, and its magnitude can be determined within the range of 0 m / s to the rated lifting speed according to the current control requirements.

[0021] After acquiring the target speed, the speed planning module 130 further determines the target acceleration based on the target speed. As one possible implementation, the speed planning module 130 can form the target acceleration at the current moment based on the target speed change relationship between adjacent control cycles. That is, the target acceleration can be used as the result of further processing of the target speed to represent the degree of dynamic change of the gantry lifting control task at the current moment. After the target speed and target acceleration are formed, they are output by the speed planning module 130 to the subsequent control unit, where the target speed is used by the PID control module 150 to perform PID speed tracking, and the target acceleration is used by the feedforward term determination module 140 to form a feedforward term.

[0022] In this embodiment, the output of the velocity planning module 130 can also be continuously updated according to the control cycle. For example, with a preset control cycle of 10ms to 100ms, the velocity planning module 130 can update the target velocity and target acceleration once in each control cycle, thereby forming a corresponding timing relationship with the state data output by the IMU 13. This setting helps to maintain good synchronization between the target-side input and the actual state-side input in time, establishing a consistent input basis for subsequent control processing.

[0023] Therefore, this embodiment establishes a basic link formed by state input, anomaly judgment and target side input, providing a foundation for subsequent feedforward term determination, PID speed tracking and valve control execution.

[0024] The feedforward term determination module 140 is used to determine the vertical motion state corresponding data of the gantry at the current moment based on the acceleration data when the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, and to determine the feedforward term based on the target acceleration and the vertical motion state corresponding data of the gantry at the current moment. In this embodiment, the vertical motion state corresponding data is formed by acceleration data and is used to characterize the gantry lifting direction running state at the current moment. The feedforward term determination module 140 receives acceleration data from IMU 13 on the one hand and target acceleration from velocity planning module 130 on the other hand, thus forming a dual-input structure of "actual state side input + target side input". Compared with the method of control processing based only on target side input, the feedforward term determination module 140 in this embodiment incorporates the actual running state information of the current gantry into the control processing chain, so that the subsequent control processing not only reflects the control target of the gantry, but also reflects the actual motion state of the gantry at the current moment.

[0025] First, the feedforward term determination module 140 determines the vertical motion state corresponding data of the gantry at the current moment based on the acceleration data. As a directly implementable method, the feedforward term determination module 140 can extract data related to the gantry's lifting direction from the acceleration data output by IMU 13 and use this data as the vertical motion state corresponding data of the gantry at the current moment. In a preferred implementation, the feedforward term determination module 140 can extract the Z-axis acceleration az from the acceleration data, perform high-pass filtering on the Z-axis acceleration az to obtain the filtered real-time Z-axis acceleration, and use the filtered real-time Z-axis acceleration as the vertical motion state corresponding data of the gantry at the current moment. Since the gantry lifting process in this embodiment mainly unfolds along the vertical direction of the gantry, the vertical motion state corresponding data can be used to characterize the motion state of the gantry in the lifting direction at the current moment. The formation of this data can be consistent with the output period of the acceleration data. For example, within each control cycle, the feedforward term determination module 140 reads and processes the acceleration data corresponding to the current control cycle once to obtain the vertical motion state corresponding data at the current moment.

[0026] The vertical motion state data generated by the feedforward determination module 140 can be directly entered into the subsequent feedforward determination process within the current control cycle to establish a correspondence with the target acceleration in the same cycle.

[0027] After obtaining the vertical motion state corresponding data, the feedforward term determination module 140 determines the feedforward term based on the target acceleration and the vertical motion state corresponding data of the gantry at the current moment. As one possible implementation, the feedforward term determination module 140 preferably forms the feedforward term based on the difference between the target acceleration and the vertical motion state corresponding data; in a preferred implementation, the feedforward term can be determined according to K × (target acceleration – vertical motion state corresponding data), where K is the feedforward gain weight. In other implementations, the feedforward term can also be formed based on a preset calculation relationship between the two. This difference relationship can be understood as follows: the target acceleration represents the target dynamic change information corresponding to the gantry lifting control task within the current control cycle, and the vertical motion state corresponding data represents the actual operating state information of the gantry in the lifting direction within the same control cycle; the difference between the two is then used to form the feedforward term. The feedforward term thus formed is sent as a compensation input to the PID control module 150, used to form a compensated speed tracking control command together with the PID speed tracking result. In some implementations, the feedforward term determination module 140 may also employ different feedforward gain weights depending on whether the gantry is currently in a loaded or unloaded state. The loaded or unloaded state may be determined based on a loading status signal, which may include at least one of, fork pressure signal, weighing signal, and task loading information.

[0028] In engineering implementation, the feedforward term determination module 140 can also operate according to a preset control cycle. For example, when the control cycle is 10ms to 100ms, the feedforward term determination module 140 can receive the target acceleration and vertical motion state corresponding data in each control cycle and form the feedforward term for the current control cycle. This allows the feedforward term to maintain a time correspondence with the current gantry operating state, enabling the compensation input to participate in the control processing of the current cycle. It should be understood that this embodiment only provides one implementation method for continuously updating the feedforward term according to the control cycle. Those skilled in the art can also achieve continuous generation of the feedforward term by using an equal-period or near-real-time refresh method without departing from the technical concept of this embodiment.

[0029] The PID control module 150 is used to perform PID speed tracking based on the target speed, obtain the PID speed tracking result, and determine the compensated speed tracking control command based on the PID speed tracking result and the feedforward term. In this embodiment, the PID control module 150 receives the target speed from the speed planning module 130 and performs PID speed tracking based on the target speed. As a directly implementable method, the PID control module 150 can pre-store PID control parameters for speed tracking, and use the target speed in the current control cycle as the input target quantity for PID control, combined with the control state in the current gantry lifting control process, to obtain the PID speed tracking result corresponding to the current control cycle. The PID speed tracking result can be understood as the basic control result formed only based on the target speed tracking process, or it can be called the uncompensated speed tracking result.

[0030] After generating the PID speed tracking result, the PID control module 150 further receives the feedforward term output from the feedforward term determination module 140 and incorporates the feedforward term into the control command formation process to determine the compensated speed tracking control command. As one possible implementation, the PID control module 150 can add the feedforward term to the PID speed tracking result to generate the compensated speed tracking control command. Thus, the compensated speed tracking control command is no longer solely determined by the target speed tracking result, but simultaneously reflects the processing result after the participation of both the target-side input and the actual state-side input. To ensure that the PID control module 150 maintains timing coordination with the upstream module, the PID control module 150 can complete one control process per control cycle, i.e., receiving the target speed and feedforward term in each control cycle, first obtaining the PID speed tracking result, and then generating the compensated speed tracking control command. The advantage of this setup is that both the PID speed tracking result and the feedforward term correspond to the input state in the current control cycle, thereby helping to ensure that the compensated speed tracking control command maintains a consistent time correlation with the current gantry operating state.

[0031] The valve control module 160 is used to determine the proportional valve opening based on the compensated speed tracking control command, and to execute deceleration and stop control when the gantry lifting anomaly judgment result indicates an abnormal gantry lifting. In this embodiment, the valve control module 160 is located at the final execution output end of the entire system. It receives the compensated speed tracking control command under the normal control path on the one hand, and the deceleration and stop control requirement under the abnormal path on the other hand, thus forming an execution end structure shared by normal control and abnormal control.

[0032] If the gantry lifting anomaly judgment result indicates no anomaly, the valve control module 160 receives the compensated speed tracking control command output by the PID control module 150 and determines the proportional valve opening accordingly. As a direct implementation method, the valve control module 160 can convert the compensated speed tracking control command into a corresponding proportional valve opening control quantity based on a preset control quantity-valve opening correspondence or a preset conversion relationship, to control the gantry lifting actuator. Here, the proportional valve opening can be understood as the valve control output quantity corresponding to the gantry lifting execution process, and its range can vary, for example, between 0% and 100%. The proportional valve opening is used to adjust the valve control output corresponding to the gantry lifting actuator to execute the gantry lifting action. The valve control module 160 continuously updates the proportional valve opening based on the compensated speed tracking control command generated under different control cycles, thereby enabling the gantry lifting process to continuously operate according to the normal control path.

[0033] If the gantry lifting anomaly detection result indicates an anomaly, the valve control module 160 executes deceleration and stop control. As one possible implementation, after receiving the anomaly result from the anomaly detection module 120, the valve control module 160 suspends the continuous update process of the proportional valve opening corresponding to the current normal control path and enters the deceleration and stop control state. In this state, the valve control module 160 performs deceleration and stop control processing on the gantry lifting, causing the gantry lifting process to exit the current normal operating path. In this way, the abnormal path and the normal path are uniformly converged at the execution end: when in a normal state, the valve control module 160 is responsible for executing normal lifting control; when in an abnormal state, the valve control module 160 is responsible for executing deceleration and stop control.

[0034] Based on the above system structure, this embodiment also provides a feedback method for mast lifting stability control applied to forklift AGVs. This method can be derived from... Figure 3 The functional modules shown work together, and the process is as follows: Figure 4 As shown. Figure 4This is a flowchart illustrating a feedback method for mast lifting stability control applied to forklift AGVs. The method includes steps S100 to S800, where steps S100 to S800 have a clear functional mapping relationship with the aforementioned functional modules: IMU 13 corresponds to step S100, anomaly detection module 120 corresponds to step S200, valve control module 160 corresponds to steps S300 and S800, speed planning module 130 corresponds to step S400, feedforward term determination module 140 corresponds to steps S500 and S600, and PID control module 150 corresponds to step S700.

[0035] S100: Acquire acceleration data output from the IMU installed on top of the highest-level gantry. In one possible implementation, the IMU 13 continuously outputs acceleration data according to a preset control cycle, and the acceleration data can be triaxial acceleration data. The preset control cycle can be, for example, 10ms to 100ms. The acceleration data output within the current control cycle is read by the anomaly detection module 120 and the feedforward term determination module 140 for state judgment and feedforward term determination processing corresponding to the current cycle.

[0036] S200. Determine whether the gantry lifting is abnormal based on the acceleration data. Optionally, the anomaly judgment module 120 receives the acceleration data of the current control cycle obtained in step S100 and outputs the gantry lifting anomaly judgment result according to a preset judgment rule. The gantry lifting anomaly judgment result includes at least two states: gantry lifting abnormal and gantry lifting not abnormal. In this embodiment, it is preferable to judge based on whether the acceleration data in the current control cycle falls within a preset normal state range. When the acceleration data falls within the preset normal state range, it is determined that the gantry lifting is not abnormal; when the acceleration data does not fall within the preset normal state range, it is determined that the gantry lifting is abnormal. If the judgment result indicates an anomaly, the process proceeds to step S300; if the judgment result indicates no anomaly, the process proceeds to step S400. In a preferred implementation, the anomaly judgment in step S200 can also be further refined by combining the magnitude of the three-axis acceleration data, the horizontal acceleration component, and the acceleration change between adjacent control cycles, as detailed in Embodiment 2.

[0037] S300. If an abnormality is detected in the gantry lifting mechanism, deceleration and stopping are performed on the gantry lifting. As a specific implementation, after receiving the abnormal result output by the abnormality judgment module 120, the valve control module 160 executes deceleration and stopping control. At this time, the current normal control path stops executing, and the compensated speed tracking control command is no longer used for normal valve control output, thus switching the process flow to the abnormal stop handling path.

[0038] S400: If it is determined that the gantry lifting is not abnormal, acquire the target speed and determine the target acceleration based on the target speed. In one possible implementation, the speed planning module 130 determines the target speed at the current moment based on the current gantry lifting control task, and forms the target acceleration based on the target speed. The target speed can be determined, for example, within the range of 0 m / s to the rated lifting speed. The output of step S400 includes the target speed and the target acceleration, where the target speed is used for subsequent PID speed tracking, and the target acceleration is used for subsequent feedforward term determination.

[0039] S500: Determine the vertical motion state corresponding data of the gantry at the current moment based on the acceleration data. Optionally, the feedforward term determination module 140 reads the acceleration data in step S100 within the current control cycle and forms current moment data related to the lifting direction of the gantry, using it as the vertical motion state corresponding data of the gantry at the current moment. In a preferred implementation, the feedforward term determination module 140 extracts the Z-axis acceleration az from the acceleration data and performs high-pass filtering on it to obtain the filtered real-time Z-axis acceleration, and uses the filtered real-time Z-axis acceleration as the vertical motion state corresponding data of the gantry at the current moment. This data corresponds to the current control cycle and serves as one of the inputs for the subsequent step S600.

[0040] S600: Determine the feedforward term based on the target acceleration and the corresponding data of the vertical motion state of the gantry at the current moment. In one specific implementation, the feedforward term determination module 140 simultaneously receives the target acceleration output in step S400 and the corresponding data of the vertical motion state formed in step S500 within the current control cycle, and performs correlation processing on the two, forming the feedforward term corresponding to the current control cycle based on the difference relationship or a preset calculation relationship. In a preferred implementation, the feedforward term can be determined according to K × (target acceleration – corresponding data of vertical motion state), where K is the feedforward gain weight. The feedforward term formed in step S600 is output to the PID control module 150 as one of the inputs to step S700.

[0041] S700: Perform PID speed tracking based on the target speed to obtain a PID speed tracking result, and determine a compensated speed tracking control command based on the PID speed tracking result and the feedforward term. In one possible implementation, the PID control module 150 receives the target speed output in step S400 and performs speed tracking processing on the target speed according to preset PID control parameters to form a PID speed tracking result for the current control cycle. Subsequently, the PID control module 150 further receives the feedforward term output in step S600 and introduces the feedforward term based on the PID speed tracking result to form a compensated speed tracking control command corresponding to the current control cycle. In this way, the compensated speed tracking control command simultaneously reflects the control processing result after the target-side input and the actual state-side input participate together.

[0042] S800: Determine the proportional valve opening according to the compensated speed tracking control command. Optionally, the valve control module 160 receives the compensated speed tracking control command output in step S700 and determines the proportional valve opening according to the preset control quantity-valve opening correspondence to control the gantry lifting actuator. The proportional valve opening can vary, for example, within the range of 0% to 100%. The valve control module 160 synchronously updates the proportional valve opening according to the compensated speed tracking control command updated in different control cycles, thereby ensuring that the gantry lifting process runs along the normal control path. It should be noted that step S800 is performed only if step S200 determines that the gantry lifting is not abnormal and continues to execute the normal control path; if the judgment result in step S200 indicates an abnormality, the process directly enters the abnormal control processing corresponding to step S300, and does not continue to execute steps S400 to S800.

[0043] Therefore, Figure 4 The method flow shown is the same as Figure 3 The system modules shown form a clear one-to-one correspondence: step S100 is handled by IMU 13, step S200 by the anomaly detection module 120, steps S300 and S800 by the valve control module 160, step S400 by the speed planning module 130, steps S500 and S600 by the feedforward term determination module 140, and step S700 by the PID control module 150. In other words, Figure 4 The method shown is not independent of Figure 3 Instead of the system shown, it is an alternative solution. Figure 3 The system shown is executed according to the specific process during actual operation.

[0044] It should be noted that, in this embodiment, the target speed is preferably determined by the speed planning module 130 based on the current gantry lifting control task; however, it can also be determined by the current operation stage or the current control cycle without changing the overall control chain. In this embodiment, the vertical motion state corresponding data is preferably generated by the feedforward term determination module 140 from the acceleration data to form current moment data related to the gantry lifting direction; however, it can also be generated by processing the acceleration data in the corresponding direction without changing the overall control logic. In this embodiment, the feedforward term is preferably formed based on the difference between the target acceleration and the vertical motion state corresponding data; however, it can also be formed based on a preset calculation relationship without changing the control chain. All of the above different implementation methods serve the implementation of independent technical solutions and do not change the basic coordination relationship between the various functional modules.

[0045] In engineering applications, the functional modules of the system can be implemented as independent software functional units or integrated within the same controller and implemented in a functionally differentiated manner. For example, the anomaly judgment module 120, speed planning module 130, feedforward term determination module 140, PID control module 150, and valve control module 160 can be deployed together in the same controller. The controller calls the program segments corresponding to different functional modules according to a preset control cycle to sequentially complete state judgment, target-side input formation, actual state-side input formation, feedforward term determination, PID speed tracking, and valve control output processing. Alternatively, some functions can be deployed within the same board or processing unit, as long as the data transmission relationship and control sequence requirements between the modules can be met, the technical solution described in this embodiment can be achieved.

[0046] From the perspective of technical problem-solving path, this embodiment does not isolate the state acquisition, state judgment, and control execution during the gantry lifting process, but rather establishes a continuous feedback control chain around the same object of gantry lifting. Specifically, IMU 13 first outputs the acceleration data corresponding to the current control cycle, providing the system with the input basis for the gantry operating state; the anomaly judgment module 120 provides the gantry lifting anomaly judgment result based on this input basis, enabling the system to divide the path between abnormal and non-abnormal states; when the judgment result indicates no anomaly, the speed planning module 130 provides the target speed and target acceleration as the target-side control input, while the feedforward term determination module 140 provides the vertical motion state corresponding data as the actual state-side input, and forms a feedforward term based on the target acceleration and the actual state-side input; the PID control module 150 forms a PID speed tracking result based on the target speed, and introduces the feedforward term to form a compensated speed tracking control command; the valve control module 160 finally implements the compensated speed tracking control command into the proportional valve opening, thereby completing the gantry lifting control. Thus, the system forms a complete closed loop from status feedback to control processing and execution output when the gantry lifting is normal.

[0047] When the gantry lifting anomaly detection result indicates an anomaly, the system will no longer continue along the normal control path. Instead, the valve control module 160 will directly execute deceleration and stop control. The significance of this setup is that the system's control logic does not always operate along a single path; rather, it first determines the path type for subsequent control processing based on the gantry's operating state. Under normal operating conditions, the system continues to execute target speed planning, feedforward term determination, and PID speed tracking control to complete continuous control during the gantry lifting process. Under abnormal conditions, the system promptly exits the normal control chain and enters the deceleration and stop control path. Thus, the system forms two distinct control paths, one for abnormal conditions and one for normal conditions, but both converge at the execution end.

[0048] From an engineering implementation perspective, gantry operation status feedback, anomaly detection, and control execution are typically configured separately. However, this embodiment organizes these three into a unified control chain, incorporating the gantry's current operational status into the control processing. In other words, this embodiment does not simply push control output unidirectionally along the target input; instead, it introduces the gantry's current motion state information into the control processing, forming a feedforward term, which, together with the PID speed tracking result, generates a compensated speed tracking control command. Thus, the system's control processing no longer relies solely on the target control input but simultaneously reflects both the target input and the actual operational status input.

[0049] Furthermore, in this embodiment, the anomaly judgment result not only plays a role in state identification but also in control path switching within the overall scheme. Without this anomaly judgment result, the system cannot clearly distinguish between abnormal and normal states, making it difficult to establish the collaborative logic of the subsequent speed planning module 130, feedforward term determination module 140, PID control module 150, and valve control module 160. If the proportional valve opening continues to be output along the normal control path in an abnormal state, abnormal control and normal control will become mixed, hindering the formation of a clear control loop. Therefore, in this embodiment, the anomaly judgment result must be formed before the normal control path is executed. Correspondingly, the target speed must be determined first before the target acceleration can be further formed; the vertical motion state corresponding data must be formed first before the feedforward term can be further formed; the PID speed tracking result and feedforward term must be formed first before the compensated speed tracking control command can be further formed; and the compensated speed tracking control command must be formed first before the valve control module 160 can determine the proportional valve opening. Thus, there is a clear sequential dependency between each module and each step.

[0050] If the above order is adjusted, for example, if the proportional valve opening is output directly before the gantry lifting anomaly judgment result is formed, it will be impossible to distinguish between abnormal and non-abnormal paths; if the feedforward term is determined directly before the target acceleration is formed, the feedforward term lacks the target-side input basis; if the feedforward term is formed before the corresponding vertical motion state data is formed, the feedforward term lacks the actual state-side input basis; if the proportional valve opening is determined before the compensated speed tracking control command is formed, the valve control module 160 cannot obtain complete control input. Therefore, the cooperation order of each module in this embodiment and Figure 4 The steps shown together form a logical closed loop with input, judgment, branching, control, and execution.

[0051] In summary, the gantry lifting stability control feedback system and method in this embodiment uses the acceleration data output by the IMU as the input basis for judging and controlling the gantry's operating state, and employs different control paths under two states: abnormal gantry lifting and normal gantry lifting. Under normal conditions, the system generates a target-side control input from the target velocity to the target acceleration, and incorporates the current vertical motion state data of the gantry and the target acceleration into the feedforward term determination process. The PID control module 150 and the valve control module 160 then generate the execution output. Under abnormal conditions, the system executes deceleration and stop control through the valve control module 160. Thus, the state feedback and control processing related to gantry lifting during high-level operations of the forklift AGV can form a relatively coherent execution relationship, thereby establishing a good correspondence between control execution and actual motion state.

[0052] Example 2 This embodiment further elaborates on the gantry lifting stability control feedback system and method based on Embodiment 1. Unlike Embodiment 1, which mainly describes the overall system structure, module cooperation relationships, and basic control flow, this embodiment further introduces several preferred or optional implementation methods, and provides detailed explanations of status acquisition, anomaly detection, feedforward compensation, path switching, and robust control under long-term continuous operation. These implementation methods can be used separately in different processing stages or combined within the same engineering system, thereby enabling the gantry lifting stability control scheme to have stronger stability, adaptability, and feasibility under complex operating environments and long-term continuous operation conditions.

[0053] Combination Figure 5 , Figure 5 This is a flowchart illustrating a mast lifting and stabilization control method applied to a forklift AGV, as exemplified in this embodiment. Figure 5 The flowchart shown explains the logical relationships between target position determination, IMU feedback acceleration acquisition, filtering, anomaly detection, normal control path, and abnormal deceleration and stop path during the gantry lifting control process. Figure 5The process shown can be derived from Figure 3 The abnormal judgment module, speed planning module, feedforward term determination module, PID control module and valve control module work together. Figure 5 The "execution speed" marked in this embodiment corresponds to the actual execution speed formed by the gantry lifting actuator after the proportional valve opening is determined according to the control command. Figure 5 The process in the code corresponds to the valve-controlled execution chain and is used to characterize the actual execution state after the valve-controlled module controls the gantry lifting actuator.

[0054] In this embodiment, the IMU is mounted on top of the highest-level gantry to collect dynamic motion information from the uppermost position of the gantry. As a directly implementable method, the IMU can be rigidly mounted on top of the highest-level gantry using bolts, mounting plates, or a housing fastener, allowing the IMU to move synchronously with the gantry during lifting and lowering. To ensure stable alignment of the three axes with the gantry's spatial orientation, the IMU is mounted horizontally to the ground; the positive Z-axis is vertically upward, the positive X-axis is the longitudinal forward direction of the vehicle (preferably the direction the forks point), and the positive Y-axis is the leftward lateral direction relative to the positive X-axis. Data output from the IMU is sent to the anomaly detection module and feedforward determination module via the controller's internal bus, the acquisition interface, or the inter-module communication interface for subsequent processing.

[0055] In this embodiment, the X-axis acceleration output by the IMU is denoted as ax, the Y-axis acceleration as ay, and the Z-axis acceleration as az. Since the IMU experiences normal measurement fluctuations during actual operation, the acceleration information it reports has a normal fluctuation range of ±0.15 m / s². Simultaneously, because a relatively stable gravitational component exists in the Z-axis direction over a long period, the gravitational acceleration reference must also be considered when analyzing gantry vibration and abnormal states. As a preferred implementation, the gravitational acceleration is taken as g = 9.81 m / s². Based on the above three-axis accelerations, the magnitude of the triaxial combined acceleration mod and the horizontal acceleration magnitude mod1 are further defined, where: ; ; Here, mod represents the overall magnitude of the triaxial resultant acceleration at the current moment, and mod1 represents the magnitude of the horizontal acceleration at the current moment. Thus, ax, ay, az, mod, and mod1 constitute the basic set of quantities for anomaly detection and subsequent enhanced control.

[0056] Figure 5The "Has the gantry reached the target position?" node describes the entry and exit conditions of this control flow. As a preferred implementation, the target position can be provided by the target height information in the current task, and the current gantry position can be obtained from the gantry height feedback device, lifting displacement detection device, or the current height record of the control system. The current gantry position is compared with the target position. When the difference falls within a preset position tolerance range, it is determined that the gantry has reached the target position. When the gantry has reached the target position, the flow enters "End Control." When the gantry has not yet reached the target position, the system continues to execute "Acquire IMU feedback acceleration" and subsequent anomaly detection and control processing.

[0057] Figure 5 The "Acquire IMU Feedback Acceleration" and "Filter Acceleration" nodes in the document describe the data preparation process before anomaly detection. "Acquire IMU Feedback Acceleration" refers to reading the three-axis acceleration data output by the IMU at the current moment within each control cycle; "Filter Acceleration" refers to processing the acquired acceleration data before anomaly detection to remove the influence of normal fluctuations, steady-state errors, or gravity background. As one possible implementation, preprocessing may include removing steady-state offsets from the original acceleration sequence, high-pass processing of the mod sequence, and period difference extraction of the Z-axis acceleration. Unless otherwise stated, in this embodiment, state acquisition, anomaly detection, feedforward term determination, PID speed tracking, and valve control output are all updated according to the same preset control cycle, which may be, for example, 10ms to 100ms. In this embodiment, Figure 5 The "filtered acceleration" in this context is not a single fixed algorithm, but rather refers to a set of preprocessing actions to generate usable data for subsequent anomaly detection and normal control. Therefore, "filtered acceleration" is a general expression of various filtering or preprocessing actions; specifically, the filtering process in the anomaly detection branch is preferably used to generate the feature quantities required for anomaly detection, while the filtering process in the normal control branch is preferably used to generate the actual acceleration input of the gantry at the current moment. Furthermore, the filtering process in the normal control branch is preferably performed on the Z-axis acceleration to generate data corresponding to the vertical motion state of the gantry at the current moment.

[0058] Figure 5The "Is the current acceleration abnormal?" node corresponds to a set of multi-condition anomaly detection mechanisms. The first set of criteria is based on a joint judgment of the deviation between the triaxial combined acceleration modulus and the gravitational acceleration reference, as well as the change in Z-axis acceleration. As a preferred implementation, the difference between mod and g is taken as mod-g. In this embodiment, the difference is preferably the direct difference between mod and g, rather than the absolute value deviation. The anomaly judgment module compares this difference with a first preset threshold and compares the change in az acceleration with a second preset threshold; wherein, the change in az acceleration can be determined by the absolute value of the difference between the Z-axis accelerations of two adjacent control cycles. In this embodiment, the first preset threshold is preferably 0.7, and the second preset threshold is preferably 0.4. When mod-g is greater than 0.7, or the change in az acceleration is greater than 0.4, it is determined that there is an anomaly in the gantry lifting. This set of criteria is mainly used to identify situations such as IMU loosening, changes in installation posture, impact on the gantry, or sudden changes in vertical acceleration.

[0059] The second set of criteria is based on the horizontal acceleration modulus (mod1) to determine the horizontal vibration state. The anomaly detection module compares mod1 with a third preset threshold; when mod1 is greater than the third preset threshold, an abnormal horizontal vibration state is determined to exist. In this embodiment, the third preset threshold is preferably 0.8. Since mod1 is formed by ax and ay, it can reflect the degree of vibration of the gantry in the horizontal direction. When there is obvious lateral sway, lateral impact, or amplified horizontal vibration during the lifting and lowering of the gantry, mod1 will increase; when mod1 exceeds 0.8, the anomaly detection result can be output and the anomaly control path can be entered.

[0060] The third set of criteria is for identifying high-frequency jitter. The anomaly detection module performs high-pass filtering on the three-axis acceleration modulus (mod) to filter out steady-state error changes in the mod, and then compares the high-pass filtered mod value with a fourth preset threshold. In this embodiment, the fourth preset threshold is preferably 0.6. When the high-pass filtered mod value is greater than 0.6, a high-frequency jitter anomaly is determined to exist. As one possible implementation, the high-pass filtering of mod can be performed continuously according to the control cycle, with the filter update cycle consistent with the system control cycle, for example, 10ms to 100ms; its cutoff frequency can be set according to the gantry vibration characteristics, for example, it can be selected in the range of 0.5Hz to 5Hz. It should be noted that the purpose of performing high-pass filtering on mod here is to extract high-frequency jitter features for anomaly detection; its object and purpose are different from the high-pass filtering of Z-axis acceleration in the subsequent normal control path to form the actual acceleration input.

[0061] The fourth criterion is severe jitter identification based on a sliding window. As a preferred implementation, the sliding window performs statistics on the time series corresponding to the mod sequence: recording the maximum and minimum values ​​of the mod sequence within a predetermined time window, and using the difference between them to form a peak value, which is then compared with a fifth preset threshold. In this embodiment, the peak value is preferably formed based on the mod sequence; in other optional implementations, it can also be formed based on a single-axis acceleration sequence or a combination thereof. In this embodiment, the predetermined time window is preferably 0.2s, and the fifth preset threshold is preferably 6. When the peak value obtained within the 0.2s window is greater than 6, a severe jitter anomaly is determined. If the control cycle is 10ms to 100ms, the 0.2s window can correspond to 2 to 20 sampling points or a data set within 2 to 20 control cycles; as the control flow is executed cyclically, the window is continuously updated in a sliding manner, enabling the anomaly judgment module to continuously evaluate the short-term severe vibration state.

[0062] In this embodiment, the four sets of anomaly criteria can participate in the "whether the current acceleration is abnormal" judgment process in parallel. As a preferred implementation, the anomaly judgment module outputs the gantry lifting anomaly judgment result as long as any one of the four criteria meets the corresponding anomaly condition. As a conservative implementation, it can also be set to output the anomaly judgment result only when at least two types of criteria are met simultaneously, to reduce the probability of misjudgment caused by short-term fluctuations in a single criterion. Regardless of the method used, once the anomaly judgment result is output, Figure 5 The process involves switching from the "Is the current acceleration abnormal?" node to either the abnormal path or the normal path.

[0063] Figure 5 The anomaly handling branch on the right explains the deceleration and stopping mechanism of the gantry lifting system when an anomaly is detected. After the anomaly judgment result is output, the process first enters the judgment node of "whether the current speed is 0". As a preferred implementation, the current speed can be calculated from the change in the current position of the gantry within adjacent control cycles, or it can be directly obtained from the current speed feedback value output by the gantry lifting control system. The current speed is 0 if the absolute value of the gantry lifting speed is less than a preset zero speed threshold, for example, the zero speed threshold can be 0 to 0.05 m / s. When the current speed is already 0, the process directly enters "end control"; when the current speed is not 0, the system enters the controlled deceleration path.

[0064] When the current speed is not 0 Figure 5The subsequent nodes are, in sequence, "target speed updated to zero, acceleration replanned," "pure PID speed tracking," and "execution speed." This means that, in a preferred implementation, under abnormal conditions, the system can update the target speed to zero, replan the acceleration, and use pure PID speed tracking to gradually reduce the gantry speed to zero, thereby achieving deceleration and stopping control. As one possible implementation, after receiving an abnormal path trigger signal, the speed planning module rewrites the original lifting target speed to zero and replans the acceleration during deceleration based on the updated target speed. Subsequently, the PID control module no longer introduces feedforward compensation terms but uses pure PID speed tracking to form deceleration control commands. The valve control module determines the proportional valve opening based on the control commands and adjusts the valve control output of the gantry lifting actuator through the proportional valve opening to achieve the actual execution speed of gradual deceleration. Afterward, the process returns to the "Is the current speed 0?" judgment node and continues looping until the current speed is determined to be 0, finally ending the control.

[0065] Figure 5 The left branch corresponds to the normal control path when the gantry lifting is in a non-abnormal state. In this path, after the anomaly judgment module outputs the result indicating that the gantry lifting is not abnormal, the process enters the "planning the execution acceleration for the next moment" node. This node is used to explain the formation process of the target speed and target acceleration. As a directly implementable method, the speed planning module obtains the target speed based on the current gantry lifting control task, and further determines the target acceleration based on the target speed. In this embodiment, the target speed can correspond to the expected lifting speed under the current lifting task of the gantry, and its value can be determined within the range of 0 m / s to the rated lifting speed according to the operating stage of the gantry. That is, the speed planning module can determine the target acceleration by performing differential processing on the target speed or S-curve planning. To form the target acceleration, the speed planning module can adopt one of two preferred paths: first, perform differential processing on the target speed to obtain the target acceleration from the change in target speed between adjacent control cycles; second, perform S-curve planning on the target speed to form a smoothly changing target acceleration. The former approach is suitable for scenarios where control refresh is fast and the target speed change relationship is clear; the latter approach is suitable for scenarios where smooth start-stop and acceleration continuity are of greater concern. The update cycle of the target acceleration can be consistent with the control cycle, for example, 10ms to 100ms; if S-curve planning is used, the acceleration change time can be set according to the device's execution characteristics, for example, it can be taken in the range of 0.1s to 1s.

[0066] Figure 5The node "Calculate PID speed tracking feedforward based on IMU feedback acceleration and calculate compensated speed tracking control command" in the left branch corresponds to the formation of actual acceleration, the generation of feedforward terms, and the establishment of the compensation control chain. In this embodiment, the feedforward term determination module preferably performs high-pass filtering on the Z-axis acceleration of the IMU under normal control path to obtain the filtered real-time Z-axis acceleration. As a directly implementable method, the feedforward term determination module extracts the Z-axis acceleration az from the three-axis acceleration data in each control cycle and performs high-pass filtering on the Z-axis acceleration to remove the steady-state components and low-frequency drift, obtaining the filtered real-time Z-axis acceleration. In this embodiment, the filtered real-time Z-axis acceleration is used as the actual acceleration of the gantry at the current moment and is further determined as the data corresponding to the vertical motion state of the gantry at the current moment; in other words, the actual acceleration is the data corresponding to the vertical motion state of the gantry at the current moment. Since the gantry's lifting direction is mainly vertical, using the filtered real-time Z-axis acceleration can better reflect the actual dynamic change state of the gantry in the lifting direction within the current control cycle.

[0067] After obtaining the target acceleration and the filtered real-time Z-axis acceleration, the feedforward term determination module generates the feedforward term. As a preferred implementation, the PID velocity tracking feedforward is generated according to the following expression: K×(target acceleration - actual acceleration); The actual acceleration is preferably the filtered real-time Z-axis acceleration, and K is the feedforward gain weight. Therefore, the feedforward term is actually a compensation amount formed based on the difference between the target acceleration and the actual acceleration. When there is a difference between the target acceleration and the actual acceleration, the difference is adjusted by the feedforward gain weight K to form a corresponding feedforward term, thereby enabling subsequent control processing to reflect the deviation between the current actual motion state of the gantry and the dynamic changes of the target. In engineering implementation, K is a positive value and can be set according to the equipment calibration results, system response sensitivity, and stability requirements; for example, it can be selected within the range of 0.1 to 2.0. To adapt to different load conditions, K can also be set in a tiered manner: when the gantry is currently under load, the first feedforward gain weight is used; when the gantry is currently under no-load, the second feedforward gain weight is used. The load state and no-load state can be determined based on the forklift AGV loading status signal, including at least one of the fork pressure signal, weighing signal, and task loading information. Optionally, the loading status signal can be used to determine whether the mast is in a loaded or unloaded state. The loading status signal includes fork pressure signal, task loading information and / or weighing signal.

[0068] After the feedforward term is formed, the PID control module further executes pure PID speed tracking and superimposes the pure PID result with the feedforward term to form a compensated speed tracking control command. As a directly implementable method, the PID control module pre-stores PID control parameters for gantry lifting speed tracking and performs speed tracking calculations based on the target speed in each control cycle to obtain a pure PID speed tracking result. This pure PID speed tracking result characterizes the basic control result obtained solely by driving the target speed without considering feedforward compensation. Based on this, the PID control module superimposes the pure PID speed tracking result with the feedforward term to obtain a compensated speed tracking control command. After the compensated speed tracking control command is formed, the valve control module determines the corresponding proportional valve opening according to a preset mapping relationship. As an implementation method, the valve control module can convert the compensated speed tracking control command into a proportional valve opening based on a preset mapping or conversion relationship between the control quantity and the valve opening. The proportional valve opening can, for example, vary within the range of 0% to 100%, and the valve control output corresponding to the gantry lifting actuator is adjusted through this proportional valve opening to form the actual execution speed. After execution, the process returns to the judgment node of "whether the gantry has reached the target position", forming a closed loop under the normal control path.

[0069] To further enhance robustness under long-term continuous operation conditions beyond normal and abnormal control paths, this embodiment further includes vertical dynamic deviation and horizontal oscillation characterization. These two intermediate characterizations are used to further characterize the current operating state of the gantry, building upon existing anomaly detection and feedforward compensation paths, and are subsequently used for combined judgment and path selection. The basis for this further embodiment remains the previously disclosed target acceleration, actual acceleration, and horizontal acceleration component data, and does not alter the overall framework of the main scheme: "state acquisition—anomaly judgment—control processing—execution output."

[0070] In this embodiment, a vertical dynamic deviation is further defined, which can be determined based on the difference between the target acceleration and the corresponding data of the gantry's current vertical motion state. As a preferred implementation, the vertical dynamic deviation is formed by the difference between the target acceleration and the filtered real-time Z-axis acceleration, characterizing the degree of dynamic deviation between the target-side input and the actual-side input in the vertical direction. To improve the stability of this value under long-term continuous operation, the difference can be statistically processed within a predetermined time window. As a direct implementation, the predetermined time window can consist of 1 to 10 control cycles, within which the instantaneous value, absolute mean, moving average, or trend of the difference can be calculated to form the vertical dynamic deviation. In a preferred approach, the difference between the target acceleration and the filtered real-time Z-axis acceleration in the current control cycle can be directly used as the vertical dynamic deviation to improve response timeliness; in another alternative approach, the absolute mean or moving average of the difference over multiple consecutive control cycles can be used to reduce changes caused by single-cycle fluctuations. The corresponding preset conditions for the vertical dynamic deviation can be formed based on the stable rise and fall samples collected during the equipment commissioning phase. For example, they can be determined based on the statistical interval, mean, and allowable fluctuation range of the stable operation samples. The corresponding preset conditions formed can be used as the decision boundary for determining whether to continue maintaining normal compensation control in the subsequent path selection logic.

[0071] In this embodiment, a horizontal oscillation characterization quantity is further set, which can be determined based on the horizontal acceleration component in the acceleration data. In this embodiment, the horizontal acceleration component includes the X-axis acceleration ax and the Y-axis acceleration ay. As a preferred implementation, the horizontal oscillation characterization quantity can be directly formed based on ax and ay; for example, mod1 can be used as the horizontal oscillation characterization quantity, or the peak-to-peak value, mean, variance, or rate of change of ax and ay within a window can be used to form the horizontal oscillation characterization quantity. As a direct implementation method, the horizontal oscillation characterization quantity can be statistically updated within a window consisting of 2 to 20 sampling points or 1 to 10 control cycles to reflect the degree of horizontal oscillation of the gantry. In the preferred path, mod1 can be directly used as the horizontal oscillation characterization quantity to maintain consistency with the aforementioned anomaly detection mechanism; in the optional path, a moving average or peak-to-peak value processing can also be performed on the time series consisting of ax and ay to form a more stable characterization result of the horizontal oscillation state. The corresponding preset and abnormal conditions for the horizontal oscillation characterization quantity can also be formed based on the equipment calibration stage or historical stable operation samples, for example, based on the statistical interval and allowable fluctuation range of the stable samples, so that the characterization quantity has an implementable condition boundary in engineering. The corresponding preset and abnormal conditions formed can be used as the judgment boundary in subsequent combination judgment and deceleration stop path triggering logic, respectively.

[0072] In this embodiment, the vertical dynamic deviation and the horizontal oscillation characteristic can jointly participate in subsequent combined judgment and path selection. As an optional engineering implementation, at least three combined states can be formed based on the two characteristic quantities. The first state is that both characteristic quantities meet the corresponding preset conditions, that is, the vertical dynamic deviation is within a preset normal range, and the horizontal oscillation characteristic is also within a preset normal range; in this case, the system can trigger the PID control module to continue executing the speed tracking control determined by the feedforward term and maintain the normal compensation control path. The second state is that at least one of the two characteristic quantities meets an abnormal condition while the other is in a critical or normal state; in this case, the system can adopt a conservative processing or delayed judgment method, for example, first maintaining the current path for several control cycles, and then deciding whether to switch paths based on the continuous judgment results of subsequent cycles. The third state is that both characteristic quantities meet an abnormal condition; in this case, the system prioritizes triggering the valve control module to execute deceleration and stop control to switch the gantry operation state to a conservative path as soon as possible. Through the above combined judgment, the system does not switch paths based solely on a single characteristic quantity, but selects different processing modes such as normal control, conservative maintenance, or deceleration and stop based on the combined state of the two characteristic quantities.

[0073] In cases where the characterization quantities conflict, change frequently, or are unstable for a short period, this implementation further introduces a determination and update mechanism for multiple consecutive control cycles. As a direct implementation method, the feedforward term determination module determines and updates the vertical dynamic deviation quantity according to multiple consecutive control cycles, and the anomaly judgment module determines and updates the horizontal oscillation characterization quantity according to multiple consecutive control cycles. Here, "multiple consecutive control cycles" refers to a predetermined number of control cycles used for continuous determination and updating of the vertical dynamic deviation quantity and the horizontal oscillation characterization quantity. This predetermined number can be set according to the control cycle size and system stability requirements; for example, it can be 3 to 20 control cycles. If the control cycle is 10ms to 100ms, then multiple consecutive control cycles can correspond to a time range of 0.03s to 2s. In a preferred implementation, when the vertical dynamic deviation and the horizontal oscillation characterization continuously meet the corresponding preset conditions within a predetermined number of consecutive control cycles, the anomaly judgment module maintains the judgment result that the gantry lifting is not abnormal, and the PID control module maintains the speed tracking control determined according to the feedforward term. Conversely, when the horizontal oscillation characterization continuously meets the abnormal conditions within the predetermined number of consecutive control cycles, the anomaly judgment module determines that the gantry lifting is abnormal, and the valve control module maintains the deceleration and stop control. To suppress frequent path jumps, a minimum hold time, window accumulation count, or trend consistency criterion can be set. For example, it can be required that the abnormal state continuously meets the window accumulation trigger condition for 2 to 10 times before entering the deceleration and stop path, or that the normal state is continuously maintained for 0.05s to 1s before resuming normal compensation control. Through the above continuous cycle update and hold mechanism, the system can reduce repeated path switching caused by instantaneous fluctuations in long-term continuous operation scenarios, thereby improving control consistency.

[0074] In this embodiment, Figure 5 The software flow shown Figure 3The functional modules shown and the aforementioned further implementations have a clear data flow, calling order, and module collaboration relationship. Specifically, the IMU, as the status acquisition terminal, outputs three-axis acceleration data to the anomaly judgment module and the feedforward term determination module according to a preset control cycle; the anomaly judgment module, based on the three-axis acceleration data and its derivatives, determines the current gantry operating state as abnormal and outputs the gantry lifting anomaly judgment result; the speed planning module, when the gantry lifting is not abnormal, obtains the target speed according to the current gantry lifting control task, and further forms the target acceleration from the target speed; the feedforward term determination module, on the one hand, forms the vertical motion state corresponding data of the gantry at the current moment based on the Z-axis acceleration, and on the other hand, forms the feedforward term based on the target acceleration and the vertical motion state corresponding data, or further forms the vertical dynamic deviation; the PID control module executes pure PID speed tracking according to the target speed and introduces the feedforward term to form a compensated speed tracking control command; the valve control module determines the proportional valve opening according to the control command and controls the gantry lifting actuator through the proportional valve opening; in abnormal states, the valve control module executes deceleration and stop control according to the anomaly judgment result. Thus, the entire scheme forms a continuous control chain from state acquisition, state analysis, control planning, control compensation to execution output.

[0075] From the perspective of structural connection and module coupling, the modules in this embodiment can be implemented as independent functional units or integrated within the same controller and implemented using software logic partitioning. As a directly implementable engineering approach, the IMU connects to the controller via an internal bus, acquisition interface, or serial communication interface. The controller reads the current triaxial acceleration data in each control cycle. The anomaly detection module, velocity planning module, feedforward term determination module, PID control module, and valve control module can be deployed within the same controller. The controller executes the corresponding processing programs according to a fixed calling order, preferably: sampling, preprocessing, anomaly detection, target planning, feedforward term calculation, PID processing, and valve control output. For the two intermediate characterization quantities and their robustness mechanisms, data interaction can also be achieved through a buffer, shared data area, or message passing structure within the controller. For example, at the end of the current control cycle, the vertical dynamic deviation and horizontal oscillation characterization quantities are written to a shared buffer and read when the path selection logic is called in the next control cycle. If a distributed implementation is adopted, the status acquisition and anomaly judgment can be deployed on the near-sensor side processing unit, while the speed planning, feedforward term determination, PID control and valve control output can be deployed on the main control unit. As long as the data synchronization, calling order and path switching relationship can be met, the technical solution described in this embodiment can be implemented.

[0076] Regarding parameter settings, the key operating parameters involved in this embodiment can be set based on engineering experience, stability considerations, or equipment calibration results. For the control cycle, a setting of 10ms to 100ms is preferred; for normal IMU fluctuations, ±0.15m / s² is preferred as the reference range for normal measurement fluctuations; for the gravitational acceleration reference, g=9.81m / s² is preferred; for the first to fifth preset thresholds, 0.7, 0.4, 0.8, 0.6, and 6 are preferred, respectively; for the sliding window length, 0.2s is preferred; for the feedforward gain weight K, it can be set within the range of 0.1 to 2.0 according to the equipment response characteristics and stability requirements, and different weights can be used for load and no-load states; for the threshold for determining whether the current velocity is zero, a setting of 0 to 0.05m / s is preferred; for the robustness judgment window for multiple consecutive control cycles, 3 to 20 control cycles are preferred. The selection of the above parameters is related to the mechanical characteristics of the gantry mechanism, the response capability of the hydraulic actuator, the refresh capability of the controller, and the stability requirements during continuous operation. Those skilled in the art can select a suitable combination of parameters within the above range based on the calibration results of specific equipment.

[0077] From the perspective of the overall solution path, this implementation method refines and enhances the key processing steps based on the basic control chain described in Example 1. First, by installing the IMU on the top of the highest-level gantry and unifying the three-axis direction definition, the actual motion state of the upper position of the gantry can be stably collected in the form of three-axis acceleration data, providing a unified data foundation for subsequent anomaly detection and control compensation. Second, by defining ax, ay, az, mod, and mod1, and introducing four sets of anomaly criteria, anomaly judgment no longer relies on a single measurement quantity, but can identify gantry anomaly states from different dimensions such as overall anomaly, vertical abrupt change, horizontal vibration, high-frequency jitter, and short-term severe anomaly. Third, in the normal control path, the target velocity and target acceleration constitute the target-side input, and the filtered real-time Z-axis acceleration constitutes the actual-side input. A feedforward term is formed by K×(target acceleration - actual acceleration), which, together with the pure PID velocity tracking result, forms a compensated velocity tracking control command, so that the control chain can reflect both the control target and the current actual motion state. Furthermore, by introducing vertical dynamic deviation, horizontal oscillation characteristics, and a maintenance mechanism for multiple consecutive control cycles, the system can select different processing paths based on the combined state of two intermediate characteristics under complex operating conditions and long-term continuous operation scenarios, and suppress frequent switching caused by short-term fluctuations. Thus, this implementation integrates state acquisition, anomaly judgment, normal control, abnormal shutdown, combined judgment, and robustness maintenance within the same control framework.

[0078] From an engineering implementation perspective, this implementation method directly introduces the IMU data from the top of the highest-level gantry into the anomaly detection and control process, so that the actual motion state of the gantry at the current moment is used for both anomaly judgment and feedforward compensation in the normal control path; at the same time, the consistency of path selection is improved through intermediate representation quantities and a mechanism to maintain multiple consecutive control cycles.

[0079] It needs to be further explained that, Figure 5 The order of nodes in the process and the dependencies between processing steps in this embodiment are clear: the determination of whether the gantry has reached the target position is at the beginning of the process; the acquisition of IMU feedback acceleration precedes anomaly detection and control compensation; the anomaly judgment result precedes the selection of the normal path and the anomaly path; the target speed, target acceleration, filtered real-time Z-axis acceleration, feedforward term, compensated speed tracking control command, and proportional valve opening are formed sequentially; in the anomaly branch, updating the target speed to zero and re-planning the execution of acceleration precede the pure PID deceleration processing. Therefore, the module cooperation order in this embodiment... Figure 5 The sequence of processes and the order of parameter updates shown together form a clear, closed, and actionable causal chain.

[0080] In summary, this embodiment further introduces IMU installation attitude definition, three-axis acceleration magnitude and horizontal acceleration magnitude, four sets of anomaly detection criteria, two types of path formation from target velocity to target acceleration, Z-axis high-pass filtering to form actual acceleration, feedforward term weighting, pure PID and compensation control superposition, vertical dynamic deviation, horizontal oscillation characterization, combined judgment, and a mechanism for maintaining stability across multiple control cycles. This allows the gantry lifting stability control feedback scheme to achieve engineering refinement and enhanced robustness without altering the overall technical framework. Through the above combination, the system can more stably reflect the actual motion state of the gantry under complex operating environments and long-term continuous operation conditions, more rationally select normal control paths and abnormal deceleration and stopping paths, and maintain a more consistent correspondence between control execution and the actual motion state of the gantry. Those skilled in the art can implement the technical solution based on the structural relationships, data sources, processing flows, operating conditions, and parameter ranges disclosed in this embodiment.

[0081] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A mast lifting stability control feedback system for forklift AGVs, characterized in that, The system includes: an IMU, an anomaly detection module, a speed planning module, a feedforward term determination module, a PID control module, and a valve control module; wherein, The IMU is used to output acceleration data; The anomaly detection module is used to determine the anomaly detection result of the gantry lifting based on the acceleration data; The speed planning module is used to obtain a target speed and determine a target acceleration based on the target speed when the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal. The feedforward term determination module is used to determine the vertical motion state corresponding data of the gantry at the current moment based on the acceleration data when the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, and to determine the feedforward term based on the target acceleration and the vertical motion state corresponding data of the gantry at the current moment. The PID control module is used to perform PID speed tracking based on the target speed, obtain the speed tracking result, and determine the compensated speed tracking control command based on the speed tracking result and the feedforward term. The valve control module is used to determine the proportional valve opening based on the compensated speed tracking control command, and to execute deceleration and stop control when the gantry lifting abnormality judgment result indicates that the gantry lifting is abnormal.

2. The gantry lifting stability control feedback system according to claim 1, characterized in that, The IMU is specifically used for: Install it on top of the highest-level gantry, with the IMU's installation direction horizontal to the ground; The positive Z-axis direction of the IMU is set to the vertical upward direction, the positive X-axis direction of the IMU is set to the longitudinal forward direction of the vehicle, and the positive Y-axis direction of the IMU is set to the left lateral direction relative to the positive X-axis direction. Output triaxial acceleration data.

3. The gantry lifting stability control feedback system according to claim 2, characterized in that, The anomaly detection module is specifically used for: Determine the X-axis acceleration ax, Y-axis acceleration ay, and Z-axis acceleration az from the triaxial acceleration data; The triaxial acceleration mod is determined based on the X-axis acceleration ax, the Y-axis acceleration ay, and the Z-axis acceleration az. The horizontal acceleration mod1 is determined based on the X-axis acceleration ax and the Y-axis acceleration ay.

4. The gantry lifting stability control feedback system according to claim 3, characterized in that, The anomaly detection module is also specifically used for: Determine the difference between the triaxial acceleration mod and the gravitational acceleration g, and compare the difference with a first preset threshold. The change in Z-axis acceleration is compared with a second preset threshold, wherein the change in Z-axis acceleration is the absolute value of the difference between the Z-axis accelerations of two adjacent control cycles. If the difference between the three-axis acceleration mod and the gravitational acceleration g is greater than the first preset threshold, or if the change in Z-axis acceleration is greater than the second preset threshold, the gantry lifting abnormality judgment result is determined to be abnormal.

5. The gantry lifting stability control feedback system according to claim 4, characterized in that, The anomaly detection module is also specifically used for: The horizontal acceleration mod1 is compared with a third preset threshold to determine whether there is an abnormal horizontal vibration state. The triaxial acceleration mod is subjected to high-pass filtering, and the high-pass filtered triaxial acceleration mod is compared with a fourth preset threshold to determine whether there is high-frequency jitter abnormality. The difference between the maximum and minimum values ​​of the triaxial acceleration mod within a predetermined time window is recorded by a sliding window to form a peak value, and the peak value is compared with a fifth preset threshold to determine whether there is a short-term severe anomaly. If the horizontal acceleration mod1 is greater than the third preset threshold, or the triaxial acceleration mod after high-pass filtering is greater than the fourth preset threshold, or the peak value is greater than the fifth preset threshold, the gantry lifting abnormality judgment result is determined to be abnormal.

6. The gantry lifting stability control feedback system according to claim 2, characterized in that, The speed planning module is specifically used to: obtain the target speed, and perform differential processing or S-curve planning on the target speed to determine the target acceleration; The feedforward term determination module is specifically used to: perform high-pass filtering on the Z-axis acceleration az to obtain the filtered real-time Z-axis acceleration, and determine the filtered real-time Z-axis acceleration as the corresponding data of the vertical motion state of the gantry at the current moment.

7. The gantry lifting stability control feedback system according to claim 6, characterized in that, The feedforward term determination module is specifically used to: determine the feedforward term according to K × (the target acceleration - the filtered real-time Z-axis acceleration), where K is the feedforward gain weight; and determine whether the gantry is currently in a loaded or unloaded state according to the loading status signal, and then adopt the feedforward gain weight corresponding to the loaded or unloaded state, wherein the loading status signal includes at least one of the fork pressure signal, weighing signal and task loading information; The PID control module is specifically used to: perform PID speed tracking based on the target speed to obtain an uncompensated speed tracking result, and superimpose the uncompensated speed tracking result with the feedforward term to obtain the compensated speed tracking control command; The valve control module is specifically used to: determine the proportional valve opening corresponding to the compensated speed tracking control command according to a preset mapping relationship.

8. The gantry lifting stability control feedback system according to claim 1, characterized in that, The feedforward term determination module is also specifically used to determine the vertical dynamic deviation based on the difference between the target acceleration and the corresponding data of the vertical motion state of the gantry at the current moment; The anomaly detection module is also specifically used to determine the horizontal oscillation characterization quantity based on the horizontal acceleration component in the acceleration data, wherein the horizontal acceleration component includes the X-axis acceleration ax and the Y-axis acceleration ay; The anomaly judgment module is also specifically used to judge and update the vertical dynamic deviation and the horizontal oscillation characterization according to multiple consecutive control cycles, and to maintain the judgment result that the gantry lifting is not abnormal if the vertical dynamic deviation continuously meets the corresponding preset conditions and the horizontal oscillation characterization continuously meets the corresponding preset conditions within a predetermined number of consecutive control cycles. The anomaly judgment module is further specifically used to determine that the gantry lifting anomaly judgment result is abnormal when the horizontal oscillation characterization quantity continuously meets the abnormal conditions within the predetermined number of consecutive control cycles, so as to trigger the valve control module to maintain the deceleration stop control.

9. A mast lifting stability control feedback method applied to forklift AGVs, characterized in that, include: Acquire acceleration data from the IMU installed on top of the highest-level gantry; The gantry lifting anomaly is determined based on the acceleration data. If the gantry lifting abnormality judgment result indicates that the gantry lifting is abnormal, deceleration and stop control shall be performed on the gantry lifting; If the gantry lifting anomaly judgment result indicates that the gantry lifting is not abnormal, the target velocity is obtained, and the target acceleration is determined based on the target velocity; The vertical motion state of the gantry at the current moment is determined based on the acceleration data; The feedforward term is determined based on the target acceleration and the corresponding data of the vertical motion state of the gantry at the current moment; PID speed tracking is performed based on the target speed to obtain the speed tracking result, and a compensated speed tracking control command is determined based on the speed tracking result and the feedforward term. The proportional valve opening is determined based on the compensated speed tracking control command.

10. The gantry lifting stability control feedback method according to claim 9, characterized in that, The step of determining the gantry lifting anomaly judgment result based on the acceleration data includes: Determine the X-axis acceleration ax, Y-axis acceleration ay, and Z-axis acceleration az from the triaxial acceleration data; The three-axis acceleration mod is determined based on the X-axis acceleration ax, the Y-axis acceleration ay, and the Z-axis acceleration az, and the horizontal acceleration mod1 is determined based on the X-axis acceleration ax and the Y-axis acceleration ay. Determine the difference between the triaxial acceleration mod and the gravitational acceleration g, and compare the difference with a first preset threshold. The absolute value of the difference between the Z-axis accelerations of two adjacent control cycles is determined as the Z-axis acceleration change, and the Z-axis acceleration change is compared with a second preset threshold. If the difference between the three-axis acceleration mod and the gravitational acceleration g is greater than the first preset threshold, or if the change in Z-axis acceleration is greater than the second preset threshold, the gantry lifting abnormality judgment result is determined to be abnormal.

Images