Tower crane slewing tower boom speed estimation method based on kalman filtering

Abstract

This invention relates to the field of automated control technology for construction machinery, and discloses a method for estimating the speed of the slewing boom of a tower crane based on Kalman filtering. The method includes constructing a nonlinear state-space model of the system, including determining the state vector, nonlinear state equation, and observation equation. By establishing a refined nonlinear model with seven degrees of freedom, especially explicitly modeling wind load and random torque disturbances, this invention more closely approximates the real system in terms of mechanism. The EKF (Extended Kalman Filter) can effectively handle this nonlinear model. Simultaneously, the high-precision speed and sway state estimation provides reliable feedback for subsequent active anti-sway control. Closed-loop control based on this estimation can achieve a load sway suppression rate of up to 95%, significantly improving operational safety and efficiency. Furthermore, an adaptive mechanism can be designed to fine-tune the noise covariance matrix Q and R of the EKF online, enabling it to adapt to sensor performance degradation and slow time-varying model parameters, maintaining long-term estimation performance.

Description

Technical Field

[0001] This invention relates to the field of automated control technology for construction machinery, and in particular to a method for estimating the speed of the slewing boom of a tower crane based on Kalman filtering. Background Technology

[0002] Tower cranes, as core vertical and horizontal transportation equipment in modern construction, directly affect the progress and cost of the entire construction project through their operational efficiency, positioning accuracy, and operational safety. Slewing motion is one of the key actions for tower cranes to transfer materials in space, and the speed control accuracy of the slewing boom directly affects the positioning accuracy and placement efficiency of the load. Under ideal conditions, good performance can be achieved through closed-loop speed control via motor encoder feedback. However, in the complex actual construction environment, tower cranes often face many challenges: 1) Strong nonlinear coupling: There is a high degree of nonlinear coupling between the load swing dynamics and the slewing, luffing, and hoisting motions, resulting in significant Coriolis force and centripetal force effects; 2) Multi-source disturbances: Wind load disturbances during outdoor operations are random and time-varying, and friction and gear backlash in the transmission system cause torque fluctuations; 3) Measurement noise and uncertainty: Sensors themselves have noise, and installation deviations and calibration errors introduce measurement uncertainty; 4) Time-varying parameters: The length of the hoisting rope and the load mass change frequently during operation, causing time-varying system dynamic parameters. In existing technologies, the estimation and control of tower crane slewing speed often employs the following methods: First, the speed is obtained directly using the differential encoder of the motor, but this method has high noise, significant delay, and cannot reflect the actual swaying state of the load; second, state estimation is performed using an observer based on a linear model or a linear Kalman filter, but linear models are difficult to accurately describe the nonlinear dynamics of the system, and the estimation deviation is large under strongly nonlinear conditions, which may even lead to filter divergence; third, simple PID control combined with open-loop anti-sway strategies such as input shaping is used, which has low dependence on model accuracy but weak adaptability and anti-interference ability, and cannot effectively suppress swaying caused by random wind loads, etc. For example, some solutions use a linear quadratic regulator combined with a state observer for control, which is based on the premise that the system can be linearized near the operating point. When the load swings significantly or rotates rapidly, the error of the linear model increases dramatically, and the control performance deteriorates. Other solutions attempt to use fuzzy logic or neural networks to compensate for nonlinearity, but these methods rely on a large amount of data for training, the generalization ability needs to be verified, and the real-time performance is difficult to guarantee.

[0003] To address the aforementioned technical deficiencies, a solution is proposed. Summary of the Invention

[0004] The purpose of this invention is to provide a high-precision, robust, and real-time method for estimating the slewing jib speed of a tower crane, in order to solve the problems of decreased estimation accuracy and load sway caused by nonlinear coupling and multi-source disturbances under complex working conditions. Another purpose of this invention is to provide a control system that implements this method. This system can integrate information from multiple sensors, estimate the system state online and compensate for disturbances, and ultimately achieve precise and stable slewing motion control and active anti-sway.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a method for estimating the speed of the slewing jib of a tower crane based on Kalman filtering, comprising the following steps: S1. Establish a nonlinear coupled dynamic model of the tower crane's slewing system. This model is a continuous system model containing seven degrees of freedom, including: slewing angle. 1. Trolley position r, 2. Hoisting rope length l, 3. Load radial swing angle , load tangential swing angle Wind load deflection angle and external torque disturbance ; S2. Based on the nonlinear dynamic model established in step S1, construct the nonlinear state-space model of the system, including determining the state vector, nonlinear state equation and observation equation. S3. Based on the nonlinear state-space model obtained in step S2, design an extended Kalman filter. The design of the extended Kalman filter includes: defining the initial state and covariance matrix of the filter; performing a prediction step in each sampling period to calculate the predicted state value and the prediction error covariance matrix; and performing an update step using the Joseph stable form based on sensor observation data to calculate the updated values ​​of the Kalman gain, the state estimate, and the estimation error covariance matrix. S4. During the operation of the tower crane, multi-source sensor data is collected in real time and input into the extended Kalman filter designed in step S3. The prediction and update steps are executed online in real time to output an accurate estimate of the angular velocity of the slewing tower arm.

[0006] Furthermore, the nonlinear coupled dynamics model is established using the nonlinear Euler-Lagrange equations, specifically including: Dynamic equation of radial oscillation under load:

[0007] in, The length of the suspension rope is in meters (m). The value is the rate of change of the suspension rope length, in m / s. The acceleration is the length of the suspension rope, expressed in m / s². Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. Radial angular acceleration, in rad / s². The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tower arm rotation angle is expressed in rad. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². The position of the trolley is in meters. The speed of the trolley is expressed in m / s. The acceleration of the car is expressed in m / s². This is the acceleration due to gravity, expressed in m / s². Load mass, in kg. The radial damping coefficient is expressed in kg / s. The radial equivalent elastic stiffness is expressed in N / m. The radial nonlinear damping coefficient is... This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. Wind deflection angle, in rad. The torque radial oscillation coupling coefficient is... External disturbance torque, in N·m; Dynamic equation of tangential oscillation under load:

[0008] in, The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tangential angular acceleration is expressed in rad / s². The damping coefficient is in the tangential direction, expressed in kg / s. The equivalent elastic stiffness in the tangential direction is expressed in N / m. The tangential nonlinear damping coefficient is... is the torque tangential oscillation coupling coefficient.

[0009] Furthermore, the nonlinear coupled dynamic model also includes: wind load disturbance dynamic equations: ,in, Wind deflection angle, in rad. The wind deflection angular velocity, in rad / s. The acceleration due to wind deflection is expressed in rad / s². This is the wind direction damping coefficient, in N·m·s / rad. The equivalent rotational inertia of wind disturbance is expressed in kg·m². This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. This is the coupling coefficient between rotational speed and wind direction disturbance; External torque disturbance model: described by an Ornstein-Uhlenbeck stochastic process, with the differential form as follows: ,in, External disturbance torque, in N·m. The mean recovery rate is expressed in units of 1 / s. Noise intensity, in N·m / √s. The increment is for the standard Wiener process, in √s.

[0010] Furthermore, the state vector is defined as: ,in, Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². Wind deflection angle, in rad. External disturbance torque, in N·m; The nonlinear state equation is in the form of: ,in, The derivative of the state vector. Let be the system state vector. It is a nonlinear state transition function. To control the input vector, This is the process noise vector; The observation equation is: ,in, For observation vector / measurement vector, The value of the nonlinear observation function. For nonlinear observation functions, Let be the system state vector. To measure the noise vector.

[0011] Furthermore, the design of the extended Kalman filter specifically includes: S31. Linearization is achieved by calculating the Jacobian matrix online: ,in, Let Jacobian be the state transition matrix. To observe the Jacobian matrix, It is a nonlinear state transition function. For nonlinear observation functions, Let be the system state vector. Let be the partial derivative of the state transition function with respect to the state vector. Let be the partial derivative of the observation function with respect to the state vector. This is a one-step predicted state estimate at time k; S32. Prediction Steps: At time k, based on the optimal estimate at time k-1, calculate the one-step predicted value of the state: ,in, This is a one-step predicted state estimate at time k. For the state estimation at time k-1, It is a nonlinear state transition function. Let k be the system control input vector at time k; With the prediction error covariance matrix ,in, Let be the prediction error covariance matrix at time k. Let Jacobian be the state transition matrix. Here is the updated covariance matrix at time k-1. The process noise covariance matrix; S33. Update Steps: Calculate Kalman Gain ,in, Here is the Kalman gain matrix. Let be the prediction error covariance matrix at time k. To observe the Jacobian matrix, Observation noise covariance matrix; Then, the Joseph stable form is used to update the state estimate and the covariance matrix of the estimation error: ,in, The updated state estimate at time k. This is a one-step predicted state estimate at time k. Here is the Kalman gain matrix. Let k be the actual measurement vector at time k. It is a nonlinear observation function; ,in, Let be the updated covariance matrix at time k. It is the identity matrix. Here is the Kalman gain matrix. To observe the Jacobian matrix, Let be the prediction error covariance matrix at time k. To observe the noise covariance matrix.

[0012] Furthermore, the observation noise covariance matrix mentioned in step S33 process noise covariance matrix It can make online adaptive adjustments based on the system's operating status to adapt to changes in sensor characteristics and model uncertainties.

[0013] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are: The Kalman filter-based method for estimating the slewing jib speed of a tower crane has the following advantages: 1. High estimation accuracy: By establishing a refined nonlinear model with seven degrees of freedom, especially explicitly modeling wind load and random torque disturbances, the mechanism is closer to the real system. EKF can effectively handle this nonlinear model. Compared with methods based on linear models or simplified nonlinear models, the slewing speed estimation error can be reduced by more than 50%. 2. Strong robustness: The model contains the main disturbance sources, which enables EKF to naturally "identify" and "separate" these disturbances during the estimation process, thereby enhancing the system's anti-interference ability under complex working conditions such as wind load, variable rope length, and variable load. 3. Good real-time performance: Although the EKF algorithm involves Jacobian matrix calculation, its computational load is acceptable for modern embedded processors. By optimizing the code and utilizing the processor's parallel computing capabilities, it can fully meet the real-time requirements of the tower crane control system (usually the control cycle is within 10ms). 4. Excellent anti-sway effect: High-precision speed and sway state estimation provides reliable feedback for subsequent active anti-sway control. Closed-loop control based on this estimation can achieve a load sway suppression rate of up to 95%, significantly improving work safety and efficiency. 5. Adaptive capability: An adaptive mechanism can be designed to fine-tune the noise covariance matrix Q and R of the EKF online, enabling it to adapt to sensor performance degradation, slow time-varying model parameters, and other conditions, and maintain long-term estimation performance; 6. High engineering practicality: The sensors (encoders, IMUs, anemometers) used are all commonly used equipment in engineering. The system architecture is clear and easy to upgrade and embed into existing tower crane control systems. Attached Figure Description

[0014] Figure 1 A schematic diagram of the method flow of the present invention is shown; Figure 2 A front view schematic diagram of the tower crane slewing system of the present invention is shown; Figure 3 A top view schematic diagram of the tower crane slewing system of the present invention is shown; Figure 4 This diagram illustrates the overall architecture of the speed estimation and control system of the present invention. Figure 5 The flowchart of the extended Kalman filter algorithm of the present invention is shown; Figure 6 A comparison curve of the rotational speed estimation of the present invention is shown; Figure 7 A comparison curve of the load swing angle suppression effect of the present invention is shown. Detailed Implementation

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

[0016] like Figures 1-7 As shown, the core idea of ​​the tower crane slewing boom speed estimation method based on Kalman filtering is as follows: First, a seven-degree-of-freedom nonlinear system model is constructed that can accurately characterize the slewing-swing coupled dynamics of the tower crane and explicitly include wind load and random torque disturbance models; then, an extended Kalman filter is designed for this nonlinear model, and through local linearization and optimal estimation algorithms, real-time high-precision estimation of key states (especially slewing angular velocity) that cannot be directly measured or have high noise levels is achieved.

[0017] 1. Nonlinear dynamics modeling This invention abstracts the tower crane slewing system into a dynamic system with seven degrees of freedom: Controlling the relevant degrees of freedom (3): rotation angle (around the vertical axis), trolley position r (radial along the boom), and rope length l.

[0018] Load swing degrees of freedom (2): Radial swing angle of the load relative to the vertical plane (In the plane of rotation) Tangential swing angle (Perpendicular to the plane of rotation).

[0019] Disturbance-related degrees of freedom (2): Overall deflection angle of tower arm and load caused by wind load. (Considered as an equivalent degree of freedom), external random torque disturbance acting on the rotary mechanism .

[0020] Based on the Euler-Lagrange equations, the complete nonlinear dynamic equations of the system are derived. The dynamic equations for the load swing fully consider the effects of nonlinear geometric relationships, Coriolis acceleration, centripetal acceleration, and rope length variation. (1) Equation of radial oscillation:

[0021] in, The length of the suspension rope is in meters (m). The value is the rate of change of the suspension rope length, in m / s. The acceleration is the length of the suspension rope, expressed in m / s². Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. Radial angular acceleration, in rad / s². The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tower arm rotation angle is expressed in rad. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². The position of the trolley is in meters. The speed of the trolley is expressed in m / s. The acceleration of the car is expressed in m / s². This is the acceleration due to gravity, expressed in m / s². Load mass, in kg. The radial damping coefficient is expressed in kg / s. The radial equivalent elastic stiffness is expressed in N / m. The radial nonlinear damping coefficient is... This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. Wind deflection angle, in rad. The torque radial oscillation coupling coefficient is... External disturbance torque, in N·m; (2) Tangential oscillation equation:

[0022] in, The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tangential angular acceleration is expressed in rad / s². The damping coefficient is in the tangential direction, expressed in kg / s. The equivalent elastic stiffness in the tangential direction is expressed in N / m. The tangential nonlinear damping coefficient is... is the torque tangential oscillation coupling coefficient.

[0023] To describe the main external disturbances, the present invention specifically introduces: Wind load disturbance model: It is modeled as an additional torque on the rotating system, and its inertial damping characteristics are considered: ,in, Wind deflection angle, in rad. The wind deflection angular velocity, in rad / s. The acceleration due to wind deflection is expressed in rad / s². This is the wind direction damping coefficient, in N·m·s / rad. The equivalent rotational inertia of wind disturbance is expressed in kg·m². This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. This is the coupling coefficient between rotational speed and wind direction disturbance; Stochastic torque disturbance model: To characterize the stochastic disturbances caused by friction fluctuations and unmodeled dynamics in the transmission system, an Ornstein-Uhlenbeck stochastic process is used for description. ,in, External disturbance torque, in N·m. The mean recovery rate (attenuation rate) is expressed in units of 1 / s. Noise intensity (diffusion coefficient), unit: N·m / √s. The increment is for the standard Wiener process, in √s.

[0024] This model can better characterize colored noise with "mean regression" characteristics of disturbances, and is more in line with physical reality than the simple white noise model.

[0025] 2. Construction of Nonlinear State-Space Model The above system of second-order differential equations is transformed into a first-order state-space form. The state vector x is defined as follows: ,in, Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². Wind deflection angle, in rad. External disturbance torque, in N·m; This is a 13-dimensional state vector that comprehensively covers the system's motion, oscillation, and disturbance states.

[0026] The nonlinear state equation of the system can be written as: ,in, The derivative of the state vector. Let be the system state vector. It is a nonlinear state transition function. To control the input vector, This is the process noise vector; The system's observation equations are: ,in, For observation vector / measurement vector, The value of the nonlinear observation function (the result of mapping the state to the observation space). It is a nonlinear observation function (a function that maps state x to measurement y). This is the system state vector (the input to the observation function, i.e., the state to be estimated). This is the measurement noise vector (a random term of sensor noise and measurement error).

[0027] Rotation angle measured by a rotary encoder .

[0028] The position r of the vehicle is measured by a linear encoder or a laser rangefinder.

[0029] The length l of the hoisting rope is measured by the encoder of the hoisting mechanism.

[0030] The radial sway angle of the load, measured by an inertial measurement unit mounted on the hook or load. Tangential swing angle And its corresponding angular velocity (or can be obtained through difference).

[0031] Wind speed measured by an anemometer installed at the top of the tower.

[0032] 3. Extended Kalman Filter Design Since both the system models f(x, u) and h(x) are nonlinear, this invention employs an extended Kalman filter (EKF) for state estimation. The EKF achieves local linearization by performing a first-order Taylor expansion of the nonlinear function at the current state estimation point (i.e., calculating the Jacobian matrix).

[0033] Linearization is achieved by calculating the Jacobian matrix online: ,in, This is the state transition Jacobian matrix (at time k, used for linearization of the prediction step). To observe the Jacobian matrix (at time k, used for linearization of the update step), It is a nonlinear state transition function (describing how the state evolves over time). It is a nonlinear observation function (describing how the state maps to the measured value). This is the system state vector (the object to which partial derivatives are calculated). It is the partial derivative of the state transition function with respect to the state vector (Jacobi matrix). It is the partial derivative of the observation function with respect to the state vector (Jacobi matrix). For the one-step predicted state estimate at time k ( and (linearization point); (2) Prediction steps (time update): At time k, make a prediction using the optimal estimate at time k-1.

[0034] State prediction: (Numerical integration, such as the Runge-Kutta method, is typically used), where, This is a one-step predicted state estimate at time k. For the state estimation at time k-1, It is a nonlinear state transition function. Let k be the system control input vector at time k; Error covariance prediction: ,in, Let be the prediction error covariance matrix at time k. Let Jacobian be the state transition matrix. Here is the updated covariance matrix at time k-1. The process noise covariance matrix; (3) Update step (measurement update): After obtaining the actual observation data at time k, update is performed.

[0035] Calculate the Kalman gain: ,in, Here is the Kalman gain matrix. Let be the prediction error covariance matrix at time k. To observe the Jacobian matrix, Observation noise covariance matrix; Updated state estimate: ,in, The updated state estimate at time k. This is a one-step predicted state estimate at time k. Here is the Kalman gain matrix. Let k be the actual measurement vector at time k. It is a nonlinear observation function; Update error covariance (Joseph form): ,in, Let be the updated covariance matrix at time k. It is the identity matrix. Here is the Kalman gain matrix. To observe the Jacobian matrix, Let be the prediction error covariance matrix at time k. To observe the noise covariance matrix.

[0036] Updating covariance using the Joseph form can guarantee It is always a positive definite symmetric matrix, which improves numerical stability, especially when the nonlinearity is strong or the linearization error is large.

[0037] The output state estimation vector of the filter Covariance Matrix This includes the required high-precision angular velocity estimate, as well as the optimal estimate of all other state variables.

[0038] 4. System Implementation Based on the above method, the present invention also provides a tower crane slewing control system, the hardware architecture of which mainly includes: Multi-sensor data acquisition unit: Integrating high-precision encoders, IMUs, anemometers, etc., it is responsible for raw signal acquisition and preprocessing, and is used to obtain the operating status information of the tower crane in real time. It includes at least: encoders for measuring slewing angle, trolley position and hoisting rope length, inertial measurement unit for measuring load swing angle and angular velocity, and anemometer for measuring ambient wind speed.

[0039] Embedded signal processing and estimation algorithm unit: High-performance computing chips such as TI C6000 series DSP or NXP i.MX series ARM processor are used as the carrier for algorithm execution, and the EKF estimation program is executed in real time.

[0040] The anti-sway and motion control unit receives state estimates (especially velocity and sway angle estimates) from the EKF output. Combined with operator commands or upper-level path planning, it employs advanced control laws such as sliding mode control and feedback linearization to generate the final control signal for the drive motor, achieving precise positioning and active sway suppression. It should be noted that the embedded signal processing and estimation algorithm unit uses a digital signal processor or a high-performance embedded processor based on the ARM architecture. The multi-sensor data acquisition unit communicates with the embedded signal processing and estimation algorithm unit via a CAN bus or Ethernet.

[0041] Compared with the prior art, the present invention has the following advantages: 1. High estimation accuracy: By establishing a refined nonlinear model with seven degrees of freedom, especially explicitly modeling wind load and random torque disturbances, the mechanism is closer to the real system. EKF can effectively handle this nonlinear model. Compared with methods based on linear models or simplified nonlinear models, the slewing speed estimation error can be reduced by more than 50%. 2. Strong robustness: The model contains the main disturbance sources, which enables EKF to naturally "identify" and "separate" these disturbances during the estimation process, thereby enhancing the system's anti-interference ability under complex working conditions such as wind load, variable rope length, and variable load. 3. Good real-time performance: Although the EKF algorithm involves Jacobian matrix calculation, its computational load is acceptable for modern embedded processors. By optimizing the code and utilizing the processor's parallel computing capabilities, it can fully meet the real-time requirements of the tower crane control system (usually the control cycle is within 10ms). 4. Excellent anti-sway effect: High-precision speed and sway state estimation provides reliable feedback for subsequent active anti-sway control. Closed-loop control based on this estimation can achieve a load sway suppression rate of up to 95%, significantly improving work safety and efficiency. 5. Adaptive capability: An adaptive mechanism can be designed to fine-tune the noise covariance matrix Q and R of the EKF online, enabling it to adapt to sensor performance degradation, slow time-varying model parameters, and other conditions, and maintain long-term estimation performance; 6. High engineering practicality: The sensors (encoders, IMUs, anemometers) used are all commonly used equipment in engineering. The system architecture is clear and easy to upgrade and embed into existing tower crane control systems.

[0042] Furthermore, this embodiment details the software implementation steps of the speed estimation method, assuming the control period is T=10ms.

[0043] Step 1: System Modeling and Initialization 1.1 Based on the tower crane model (e.g., QTZ80), determine the model parameters: load mass m (variable, initial value is rated load), rope linear density, structural dimensions, etc. Set the swing damping coefficients c_α, c_β, wind load model parameters J_γ, c_γ, and OU process parameters τ, σ.

[0044] 1.2 Define a 13-dimensional state vector x, and construct the mathematical expressions and code implementations of nonlinear functions f(x, u) and h(x).

[0045] 1.3 Initialize EKF: Set the initial state estimate value \(\hat{x}_{0|0}\) (e.g., all positions and velocities are zero, and the oscillation angle is zero). Set the initial covariance matrix \(P_{0|0}\) as a diagonal matrix, where the diagonal elements represent the confidence level of the initial uncertainty of each state (for angles, this can be (0.01 rad)^2; for velocities, it can be (0.1 rad / s)^2, etc.). Set the initial values ​​of the process noise covariance matrix Q and the observation noise covariance matrix R, which can be based on the sensor manual and engineering experience.

[0046] Step 2: Online Recursive Estimation Within each control cycle k (k=1,2,3,...), the following loop is executed: 2.1 Data Acquisition: Sensor data at time k is read from the bus and used to construct the observation vector z_k. For example: \(z_k = [\theta_{enc}, r_{enc}, l_{enc}, \alpha_{IMU}, \beta_{IMU}, \dot{\alpha}_{IMU}, \dot{\beta}_{IMU}, V_{wind}]^T\).

[0047] At the same time, obtain the control quantity u_{k-1} calculated in the previous cycle.

[0048] 2.2 Prediction Steps: a) Calculate \(\hat{x}_{k|k-1}\ from \(\hat{x}_{k-1|k-1}\) and u_{k-1} using numerical integration (such as the fourth-order Runge-Kutta method).

[0049] b) Calculate the Jacobian matrix \(F_{k-1} = \frac{\partial f}{\partial x} \bigg|_{\hat{x}_{k-1|k-1}, u_{k-1}}\). Due to the complexity of the model, its expression can be derived offline using symbolic computation tools, and then the values ​​can be assigned and calculated online; or automatic differentiation techniques can be used for online calculation.

[0050] c) Calculate the prediction error covariance: \(P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T +Q_{k-1}\).

[0051] 2.3 Update Steps: a) Calculate the observed prediction: \(\hat{y}_{k} = h(\hat{x}_{k|k-1})\).

[0052] b) Calculate the observation Jacobian matrix \(H_k = \frac{\partial h}{\partial x} \bigg|_{\hat{x}_{k|k-1}}\).

[0053] c) Calculate the new information covariance: \(S_k = H_k P_{k|k-1} H_k^T + R_k\).

[0054] d) Calculate the Kalman gain: \(K_k = P_{k|k-1} H_k^T S_k^{-1}\).

[0055] e) Update state estimate: \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - \hat{y}_{k})\).

[0056] f) Update the covariance using the Joseph form: \(P_{k|k} = (I - K_k H_k) P_{k|k-1} (I -K_k H_k)^T + K_k R_k K_k^T\).

[0057] 2.4 Output and Preparation: Extract the required rotational angular velocity estimate \(\hat{\dot{\theta}}_k\) and the yaw angle estimates \(\hat{\alpha}_k, \hat{\beta}_k\) from \(\hat{x}_{k|k}\) and send them to the control law module. Store \(\hat{x}_{k|k}\) and \(P_{k|k}\) for prediction in the next cycle.

[0058] This embodiment describes the hardware implementation scheme of the control system, including (such as...) Figure 4 (as shown) 1. Sensor layer: Rotary encoder: A multi-turn absolute encoder is used, which is installed on the rotary motor or rotary support. The resolution is not less than 17 bits and it is output through SSI or BiSS-C interface.

[0059] Carriage / Lifting Encoder: A linear encoder or draw-wire encoder is used to measure the position, while an incremental encoder is used to measure the motor speed for assistance.

[0060] IMU Unit: An industrial-grade MEMS IMU module is selected, containing a three-axis accelerometer and a gyroscope. It is mounted on the hook assembly and transmits data to the processor via wireless transmission (such as WIA-FA industrial wireless) or a wired slip ring. Attitude calculation can be completed within the IMU module, directly outputting Euler angles (α, β) and angular velocity.

[0061] Anemometer: An ultrasonic anemometer, installed on the top of the tower cap, outputs instantaneous wind speed and direction via RS485 or Ethernet.

[0062] 2. Processing and Control Layer: Main controller: Employs a high-performance ARM Cortex-A series processor (such as TI AM335x) paired with a real-time coprocessor (such as PRU) or directly uses a high-end DSP (such as TI TMS320C6748). It is responsible for running the embedded Linux or real-time operating system and executing the EKF algorithm and core control laws.

[0063] Data Interface: Equipped with a rich set of communication interfaces, including CAN bus (connecting to drivers), Ethernet (connecting to HMI and remote monitoring), RS485 / 232 (connecting to sensors), and wireless receiver module.

[0064] 3. Execution layer: Servo drive: Controls three AC servo motors or frequency converter motors for rotation, trolley, and lifting. Receives torque or speed commands from the main controller.

[0065] HMI (Hardware Management Unit): Provides an interface for operators to display estimated speed, oscillation status, alarm information, etc., and to receive operating instructions.

[0066] Effect verification Joint simulation and testing were conducted using MATLAB / Simulink and a laboratory tower crane experimental platform.

[0067] Test conditions: Model parameters were set based on the QTZ63 tower crane. Load m=2t, rope length l varied from 20m to 30m, and the slewing mechanism was given an S-curve speed command including acceleration and deceleration. A step wind load disturbance of 10m / s was applied at t=15s.

[0068] Comparison: The method of this invention (complete nonlinear model + EKF) vs. the Kalman filter (LKF) method based on a linearized model.

[0069] Results analysis: Accuracy of velocity estimation: such as Figure 6 As shown, during the acceleration and deceleration phases, due to strong nonlinear coupling, the velocity estimated by LKF (dashed line) lags significantly and deviates from the actual velocity (dotted line), with a maximum error exceeding 1.5° / s. In contrast, the velocity curve estimated by the method of this invention (solid line) almost coincides with the actual velocity, with a root mean square error (RMSE) of less than 0.3° / s.

[0070] Oscillation suppression effect: such as Figure 7 As shown, after wind load is applied, the system using conventional PID control (dashed line) exhibits continuous oscillation of the load radial swing angle α with slow decay, and the maximum amplitude exceeds 8°. In contrast, the system using the sliding mode anti-sway controller (solid line) based on the estimation results of this invention can quickly suppress the sway, reducing the sway amplitude to within 0.5° within 5 seconds, demonstrating extremely strong anti-interference and anti-sway capabilities.

[0071] Real-time performance: On the ARM Cortex-A8 @ 1GHz processor, a single EKF loop (containing all matrix operations) takes about 2.5ms, which is much less than the 10ms control cycle and meets the real-time requirements.

[0072] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for estimating the speed of the slewing jib of a tower crane based on Kalman filtering, characterized in that, Includes the following steps: S1. Establish a nonlinear coupled dynamic model of the tower crane's slewing system. This model is a continuous system model containing seven degrees of freedom, including: slewing angle.

1. Trolley position r, 2. Hoisting rope length l, 3. Load radial swing angle , load tangential swing angle Wind load deflection angle and external torque disturbance ; S2. Based on the nonlinear dynamic model established in step S1, construct the nonlinear state-space model of the system, including determining the state vector, nonlinear state equation and observation equation. S3. Based on the nonlinear state-space model obtained in step S2, design an extended Kalman filter. The design of the extended Kalman filter includes: defining the initial state and covariance matrix of the filter; performing a prediction step in each sampling period to calculate the predicted state value and the prediction error covariance matrix; and performing an update step using the Joseph stable form based on sensor observation data to calculate the updated values ​​of the Kalman gain, the state estimate, and the estimation error covariance matrix. S4. During the operation of the tower crane, multi-source sensor data is collected in real time and input into the extended Kalman filter designed in step S3. The prediction and update steps are executed online in real time to output an accurate estimate of the angular velocity of the slewing tower arm.

2. The method for estimating the slewing jib speed of a tower crane based on Kalman filtering according to claim 1, characterized in that, The nonlinear coupled dynamic model is established through the nonlinear Euler-Lagrange equations, specifically including: Dynamic equation of radial oscillation under load: ； in, The length of the suspension rope is in meters (m). The rate of change of the suspension rope length is expressed in m / s. The acceleration is the length of the suspension rope, expressed in m / s². Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. Radial angular acceleration, in rad / s². The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tower arm rotation angle is expressed in rad. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². The position of the trolley is in meters. The speed of the trolley is expressed in m / s. The acceleration of the car is expressed in m / s². This is the acceleration due to gravity, expressed in m / s². Load mass, in kg. The radial damping coefficient is expressed in kg / s. The radial equivalent elastic stiffness is expressed in N / m. The radial nonlinear damping coefficient is... This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. Wind deflection angle, in rad. The torque radial oscillation coupling coefficient is... External disturbance torque, in N·m; Dynamic equation of tangential oscillation under load: ； in, The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The tangential angular acceleration is expressed in rad / s². The damping coefficient is in the tangential direction, expressed in kg / s. The equivalent elastic stiffness in the tangential direction is expressed in N / m. The tangential nonlinear damping coefficient is... is the torque tangential oscillation coupling coefficient.

3. The method for estimating the slewing jib speed of a tower crane based on Kalman filtering according to claim 1, characterized in that, The nonlinear coupled dynamic model also includes: wind load disturbance dynamic equations: ,in, Wind deflection angle, in rad. The wind deflection angular velocity, in rad / s. The acceleration due to wind deflection is expressed in rad / s². This is the wind direction damping coefficient, in N·m·s / rad. The equivalent rotational inertia of wind disturbance is expressed in kg·m². This is the wind force coefficient, in N / (m / s)². Wind speed, in m / s. This is the coupling coefficient between rotational speed and wind direction disturbance; External torque disturbance model: described by an Ornstein-Uhlenbeck stochastic process, with the differential form as follows: ,in, External disturbance torque, in N·m. The mean recovery rate is expressed in units of 1 / s. Noise intensity, in N·m / √s. The increment for the standard Wiener process is expressed in √s.

4. The method for estimating the slewing jib speed of a tower crane based on Kalman filtering according to claim 1, characterized in that, The state vector is defined as follows: ,in, Radial swing angle of the load, in rad. Radial angular velocity, in rad / s. The tangential swing angle of the load, in rad. The tangential angular velocity is expressed in rad / s. The rotational angular velocity is expressed in rad / s. Angular acceleration, in rad / s². Wind deflection angle, in rad. External disturbance torque, in N·m; The nonlinear state equation is in the form of: ,in, The derivative of the state vector. Let be the system state vector. It is a nonlinear state transition function. To control the input vector, This is the process noise vector; The observation equation is: ,in, For observation vector / measurement vector, The value of the nonlinear observation function. For nonlinear observation functions, Let be the system state vector. To measure the noise vector.

5. The method for estimating the slewing jib speed of a tower crane based on Kalman filtering according to claim 1, characterized in that, The design of the extended Kalman filter specifically includes: S31. Linearization is achieved by calculating the Jacobian matrix online: ,in, Let Jacobian be the state transition matrix. To observe the Jacobian matrix, It is a nonlinear state transition function. For nonlinear observation functions, Let be the system state vector. Let be the partial derivative of the state transition function with respect to the state vector. Let be the partial derivative of the observation function with respect to the state vector. This is a one-step predicted state estimate at time k; S32. Prediction Steps: At time k, based on the optimal estimate at time k-1, calculate the one-step predicted value of the state: ,in, This is a one-step predicted state estimate at time k. For the state estimation at time k-1, It is a nonlinear state transition function. Let k be the system control input vector at time k; With the prediction error covariance matrix ,in, Let be the prediction error covariance matrix at time k. Let Jacobian be the state transition matrix. Here is the updated covariance matrix at time k-1. The process noise covariance matrix; S33. Update Steps: Calculate Kalman Gain ,in, Here is the Kalman gain matrix. Let be the prediction error covariance matrix at time k. To observe the Jacobian matrix, Observation noise covariance matrix; Then, the Joseph stable form is used to update the state estimate and the covariance matrix of the estimation error: ,in, The updated state estimate at time k. This is a one-step predicted state estimate at time k. Here is the Kalman gain matrix. Let k be the actual measurement vector at time k. It is a nonlinear observation function; ,in, Let be the updated covariance matrix at time k. It is the identity matrix. Here is the Kalman gain matrix. To observe the Jacobian matrix, Let be the prediction error covariance matrix at time k. To observe the noise covariance matrix.

6. The method for estimating the slewing jib speed of a tower crane based on Kalman filtering according to claim 5, characterized in that, The observation noise covariance matrix mentioned in step S33 process noise covariance matrix It can make online adaptive adjustments based on the system's operating status to adapt to changes in sensor characteristics and model uncertainties.

Images