A method for estimating the speed of a free end of a rope based on multi-sensor fusion

Abstract

A kind of rope free end take-up speed estimation method based on multi-sensor fusion belongs to the technical field of information physical system.To solve the way of using accelerometer and other sensors installed on the winch end fusion for real-time, accurate speed and position estimation, the present application includes constructing kinematic model based on wire rope winch end or fixed end using angle sensor, constructing kinematic model based on wire rope winch end or fixed end using speed sensor, integrating to obtain the state equation model of the system, then discretizing the state equation model of the system and introducing process noise, obtaining the state equation model of the system with process noise for process noise and observation noise decorrelation processing, obtaining the decorrelated state equation model, designing steady-state Kalman filter algorithm for the obtained decorrelated state equation model, for rope free end take-up speed estimation based on multi-sensor fusion.The present application estimates accurately.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of information physical systems, and particularly relates to a rope free end winding and unwinding speed estimation method based on multi-sensor fusion. BACKGROUND

[0002] The rope free end speed estimation method based on multi-sensor fusion is used in occasions where the motion speed and position of the rope free end in the hoist system cannot be directly and accurately measured in real time. The lower end of the rope system often hoists other workpieces and devices which are replaced at any time, and there is a lack of reference coordinates around, so the relative speed and position of the hook cannot be directly measured in real time. The hoist end can be measured by an angle measuring element such as a code disc, but the transmission flexibility in the middle causes a too large measurement error of high frequency components when the hoist end position and speed are used to approximate the free end position and speed. Especially in dynamic assembly systems and force control systems, the speed and position measurement accuracy and real-time performance of the suspension point end are required to be high, and the too large measurement error has a great influence on the system. SUMMARY

[0003] The problem to be solved by the application is to use an accelerometer and other sensors installed at the hoist end to fuse and perform real-time and accurate speed and position estimation, and a rope free end winding and unwinding speed estimation method based on multi-sensor fusion is provided.

[0004] To achieve the above object, the application realizes the following technical scheme:

[0005] A rope free end winding and unwinding speed estimation method based on multi-sensor fusion comprises the following steps:

[0006] S1. Based on different motion states, angle measuring sensors or speed measuring sensors are installed at the steel wire rope hoist end or the fixed end, and speed measuring sensors are installed at the steel wire rope free end to collect data under different motion states;

[0007] S2. A kinematic model is constructed based on the angle measuring sensors at the steel wire rope hoist end or the fixed end, a kinematic model is constructed based on the speed measuring sensors at the steel wire rope hoist end or the fixed end, the state equation model of the system is obtained by integration, then the state equation model of the system is discretized and process noise is introduced to obtain the state equation model of the system with process noise introduced;

[0008] S3. The state equation model of the system with process noise introduced obtained in step S2 is subjected to process noise and observation noise decorrelation processing to obtain a decorrelated state equation model;

[0009] S4. A steady Kalman filter algorithm is designed based on the decorrelated state equation model obtained in step S3 and is used for rope free end winding and unwinding speed estimation based on multi-sensor fusion.

[0010] Furthermore, in step S1, the winch end or the fixed end is in rotational motion, and one of the following is installed: an encoder, a circular grating, a magnetic grating, an inductive synchro, a rotary transformer, or a tachometer motor, for measuring the relative rotation angle and angular velocity of the stator and rotor.

[0011] The fixed end is linearly movable and is equipped with either a linear grating or a magnetic grating to measure the relative linear displacement of the stator and mover.

[0012] An accelerometer is installed at the free end.

[0013] Furthermore, the specific implementation method for constructing the kinematic model based on the winch end or fixed end using an angle measuring sensor in step S2 is as follows:

[0014] The kinematic model of the free end is established as follows:

[0015]

[0016] Where the free end position is x, the free end velocity is v, the accelerometer reading is a, and the accelerometer zero bias is θ. The derivative is the value at the free end position;

[0017] The state-motion model of the free-end kinematic model is denoted as:

[0018]

[0019] in, The derivative of the state variable. For the system matrix, Let z1 be the input matrix and z1 be the state variable.

[0020] The observation equation is established based on the use of an angle measuring sensor at the winch end or the fixed end:

[0021]

[0022] Where y1 is the measurement of the rotation angle at the installation position using an angle measuring sensor at the winch end or fixed end, and c r The transmission ratio is [ratio]. For high-frequency measurement error, ζ is the zero-mean white noise of the observation equation;

[0023] set up It is generated by driving the following high-pass circuit, namely a high-pass filter that suppresses low-frequency signals while allowing high-frequency signals to pass normally, as expressed in the following expression:

[0024]

[0025] Where, ω afor driving the loop angle frequency, s is the integral symbol, w1 is the zero mean white noise of the driving link;

[0026] The state space minimum realization is obtained by driving the following high-pass link:

[0027]

[0028] Further, the specific implementation method of constructing the kinematic model based on the hoisting end or the fixed end using the speed sensor in step S2 is:

[0029] The kinematic model of the free end is established as follows:

[0030]

[0031] Where, x is the position of the free end, v is the speed of the free end, a is the accelerometer reading, and θ is the zero offset of the accelerometer.

[0032] The observation equation is established based on the hoisting end or the fixed end using the speed sensor:

[0033]

[0034] Where, y2 is the measurement of the rotational angular velocity at the installation position of the speed sensor at the hoisting end or the fixed end.

[0035] Further, the state equation model of the system obtained after step S2 is:

[0036]

[0037] y = Cz + D2w1 + ζ

[0038] Where, z is the extended state vector, is the extended system matrix, is the extended input matrix, is the input matrix of white noise, C is the extended observation matrix, and D2 is the input coefficient corresponding to w1;

[0039]

[0040] C = [C1-ω a ]

[0041] D = [0D2]

[0042] D2 = ω a

[0043] Where, C1 is the original system observation matrix, and D is the feedforward matrix of the extended system.

[0044] Then the state equation model of the system obtained after the integration is discretized to obtain the discretized state equation model of the system:

[0045] z(k+1) = Az(k) + B1a(k) + B2w1(k)

[0046] wherein k is a discrete sampling time index, A is a discrete system matrix, B1 is a discrete input matrix, B2 is a discrete input matrix of white noise, w1(k) is a zero mean white noise of the driving link at the discrete sampling time;

[0047] Then the process noise is introduced into the discretized state equation model of the system to obtain:

[0048] z(k+1) = Az(k) + B1a(k) + B2w1(k) + B3η(k)

[0049] wherein B3 is an input matrix of process noise, and η(k) is a process noise matrix at the discrete sampling time.

[0050] Further, the specific implementation method of step S3 includes the following steps:

[0051] S3.1. Define the covariance matrix of noise as follows:

[0052]

[0053] wherein Q w , Q2, R ζ are the covariance matrices of w1, η, ζ, respectively, E is a mathematical expectation symbol; E{w1} = 0, E{η} = 0, E{ζ} = 0 are set.

[0054] S3.2. The state equation of the discretized system with process noise introduced in step S3 is introduced into the supplementary coefficient K1 to obtain the compensated state equation of the discretized system:

[0055]

[0056] Then the state term and the noise term are combined based on the above equation to obtain the state equation expression of the system with the state term and the noise term combined as follows:

[0057]

[0058] S3.3. Based on the process noise and observation noise decorrelation processing, the following target is set:

[0059]

[0060] After expansion, the expansion formula is:

[0061]

[0062] Then the definition noise covariance matrix defined in step S3.1 is brought into the expansion, and the following is obtained

[0063]

[0064] Based on R ζ is a positive definite matrix, D2Q w D2 T +R ζ is reversible, and the calculation result of K1 is obtained:

[0065]

[0066] Where, K 11 is the redefinition of the internal parameters of K1;

[0067] S3.4. The calculation result of K1 obtained in step S3.3 is substituted into the state equation of the system obtained by combining the state term and the noise term obtained in step S3.2, respectively, to obtain the state equation of the de-correlated system, and the expression is:

[0068]

[0069] y′=Cz+ξ

[0070] Where, F is the state transition matrix after de-correlation, G is the noise input matrix after de-correlation, w4 is the process noise after de-correlation, ξ is the observation noise after de-correlation, and y′ is the measurement output vector after de-correlation;

[0071]

[0072] B=[B1 B2K 11 ]

[0073] G=[B2 B3]

[0074]

[0075] ξ=D2w1+ζ.

[0076] Further, the specific implementation method of step S4 includes the following steps:

[0077] S4.1. Set E{w4}=0, E{ξ}=0, and construct the expression of the covariance matrix of the process noise after de-correlation as:

[0078]

[0079] Then, substituting the calculated result of K1 into the expression for the covariance matrix of the decorrelated process noise, we obtain:

[0080]

[0081] Based on R ζ Positive definiteness, therefore Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w Positive definite, let the expression for the covariance matrix R of the decorrelated observation noise be:

[0082] R=E{ξξ T}=D2Q w D2 T +R ζ ;

[0083] E{w4ξ T} = 0;

[0084] S4.2. Based on the covariance matrix of the decorrelated process noise and the covariance matrix R of the decorrelated observation noise constructed in step S4.1, design a Kalman filtering algorithm for the state equation of the decorrelated system:

[0085] calculate The corresponding mean square error:

[0086] P(k|k-1)=FP(k-1)F T +GQG T

[0087] in, P(k|k-1) refers to the Kalman filter prediction value. The corresponding mean square error, P(k-1) refers to... The corresponding mean square error, F T G represents the transpose of the state transition matrix. T Let F be the transpose of the system noise driving matrix, F be the state transition matrix, and G be the system noise driving matrix.

[0088] The filter gain is calculated using the following expression:

[0089] K(k)=P(k|k-1)C T (CP(k|k-1)C T +R) -1

[0090] Wherein, K(k) is the Kalman filter gain matrix of discrete time index k;

[0091] Recursive calculation is obtained:

[0092] P(k) = (I-K(k)C)P(k|k-1)

[0093] Wherein, I is the unit matrix;

[0094] Then the state equation of the system after designing Kalman filter algorithm for decorrelation is obtained, and the update is:

[0095]

[0096] Then the state equation of the system after designing Kalman filter algorithm for decorrelation is obtained, and the update is:

[0097]

[0098] The steady-state solution of the state equation of the system after designing Kalman filter algorithm for decorrelation is obtained by Riccati equation:

[0099] P = FPF T -FPC T (CPC T +R) -1 CPF T +GQG T

[0100] The steady-state gain is as follows:

[0101] K = FPC T (CPC T +R) -1

[0102] The predicted state of the state equation of the system after designing Kalman filter algorithm for decorrelation is obtained, and the expression is:

[0103]

[0104] The expression of B is brought into, and the steady-state Kalman filter of one-step prediction is obtained:

[0105]

[0106] The beneficial effects of the present application are:

[0107] The rope free end release and take-up speed estimation method based on multi-sensor fusion provided by the application establishes a state equation and a measurement equation of signal motion, wherein the measurement noise is composed of a high-frequency error term and zero-mean white noise; the process noise variance of each state is a positive definite adjustable term, which is used to attenuate the influence of unknown initial state; output feedback is used to complete the decoupling of measurement noise and motion noise; the zero offset and noise of the accelerometer are considered and compensated; a steady-state Kalman filtering algorithm is designed to estimate the speed, position and accelerometer zero offset at the installation position of the accelerometer. The application does not need to establish a complex dynamic model, and high precision can be obtained only by using a low-order kinematic model, has the characteristics that the low-frequency tends to be the differential of the code disc signal, and the medium-high frequency motion details tend to be the integral of the accelerometer, and has low dependence on the high-frequency uncertainty of transmission dynamics and model accuracy. BRIEF DESCRIPTION OF DRAWINGS

[0108] Figure 1 It is a schematic diagram of a simplified system model;

[0109] Figure 2 It is a simulation position signal diagram;

[0110] Figure 3 It is a simulation speed signal diagram;

[0111] Figure 4 It is a simulation acceleration signal diagram;

[0112] Figure 5 It is a simulation speed estimation diagram of the multi-source information fusion speed estimation method embodiment 1 of the application;

[0113] Figure 6 It is a simulation state estimation diagram of the multi-source information fusion speed estimation method embodiment 1 of the application;

[0114] Figure 7 It is a simulation position estimation diagram of the multi-source information fusion speed estimation method embodiment 1 of the application;

[0115] Figure 8 It is a simulation acceleration zero offset estimation diagram of the multi-source information fusion speed estimation method embodiment 1 of the application;

[0116] Figure 9 It is a simulation diagram of speed estimation by low-pass and high-pass filtering and weighted averaging after processing the position signal and the acceleration signal;

[0117] Figure 10 It is a simulation speed estimation diagram of the multi-source information fusion speed estimation method embodiment 2 of the application;

[0118] Figure 11 It is a simulation state estimation diagram of the multi-source information fusion speed estimation method embodiment 2 of the application;

[0119] Figure 12 Figure 2 is a simulation acceleration zero offset estimation diagram for the multi-source information fusion speed estimation method embodiment 2 of the present application;

[0120] Figure 13 Figure 1 is a flow chart of a rope free end winding and unwinding speed estimation method based on multi-sensor fusion according to the present application. DETAILED DESCRIPTION

[0121] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application is further described in detail below in combination with the drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application, that is, the specific embodiments described are only a part of the embodiments of the present application, but not all the specific embodiments. The components of the specific embodiments of the present application described and shown in the drawings herein can be arranged and designed in various different configurations, and the present application can also have other embodiments.

[0122] Therefore, the detailed description of the specific embodiments of the present application provided in the drawings below is not intended to limit the scope of the claimed present application, but only represents selected specific embodiments of the present application. Based on the specific embodiments of the present application, all other specific embodiments obtained by those skilled in the art without making creative efforts are within the scope of the present application.

[0123] In order to further understand the inventive content, characteristics and effects of the present application, the following specific embodiments are exemplified, and the drawings are Figure 1 - the drawings Figure 13 The detailed description is as follows:

[0124] Embodiment 1:

[0125] A rope free end winding and unwinding speed estimation method based on multi-sensor fusion includes the following steps:

[0126] S1. Based on different motion states, angle measuring sensors or speed measuring sensors are installed at the wire rope winding end or fixed end, and speed measuring sensors are installed at the wire rope free end to collect data under different motion states;

[0127] Further, the winding end or fixed end in step S1 is rotating motion, and an encoder is installed to measure the relative rotation angle and angular velocity of the stator and rotor;

[0128] The fixed end is linear motion, and one of a linear grating or a magnetic grating is installed to measure the relative linear displacement of the stator and mover;

[0129] An accelerometer is installed at the free end.

[0130] S2. Constructing a kinematic model based on the wire rope hoisting end or fixed end using an angle sensor, constructing a kinematic model based on the wire rope hoisting end or fixed end using a speed sensor, and comprehensively obtaining a state equation model of the system, then discretizing the state equation model of the system and introducing process noise to obtain a state equation model of the system with process noise introduced;

[0131] Further, the specific implementation method of constructing a kinematic model based on the hoisting end or fixed end using an angle sensor in step S2 is as follows:

[0132] The kinematic model of the free end is established as follows:

[0133]

[0134] wherein, x is the position of the free end, v is the speed of the free end, a is the accelerometer reading, and θ is the zero offset of the accelerometer, is the derivative of the position of the free end;

[0135] The state motion model of the kinematic model of the free end is recorded as:

[0136]

[0137] wherein, is the derivative of the state variable, is the system matrix, is the input matrix, and z1 is the state variable;

[0138] The observation equation is established based on the hoisting end or fixed end using an angle sensor as follows:

[0139]

[0140] wherein, y1 is the measurement of the rotation angle at the installation position of the hoisting end or fixed end using an angle sensor, c r is the transmission ratio, is the high-frequency measurement error, and ζ is the zero-mean white noise of the observation equation;

[0141] Set is generated by driving the following high-pass link, that is, a high-pass filter that suppresses low-frequency signals and allows high-frequency signals to pass normally, and the expression is as follows:

[0142]

[0143] wherein, ω a is the angular frequency of the driving link, s is the integral symbol, and w1 is the zero-mean white noise of the driving link;

[0144] The state space minimum realization is obtained by driving the following high-pass link:

[0145]

[0146] Further, the state equation model of the system obtained after the synthesis in step S2 is:

[0147]

[0148] y=Cz+D2w1+ζ

[0149] wherein z is an extended state vector, is an extended system matrix, is an extended input matrix, is an input matrix of white noise, C is an extended observation matrix, and D2 is an input coefficient corresponding to w1;

[0150]

[0151] C=[C1 -ω a ]

[0152] D=[0 D2]

[0153] D2=ω a

[0154] wherein C1 is an original system observation matrix, and D is a feedforward matrix of the extended system;

[0155] When the winch end (fixed end) uses an angle measuring element,

[0156] z=[x v θ α] T

[0157]

[0158] Then, the state equation model of the system obtained after the synthesis is discretized to obtain a discretized state equation model of the system:

[0159] z(k+1)=Az(k)+B1a(k)+B2w1(k)

[0160] wherein k is a discrete sampling time index, A is a discretized system matrix, B1 is a discretized input matrix, B2 is a discretized input matrix of white noise, and w1(k) is a zero-mean white noise of the driving link at the discrete sampling time;

[0161] Then, process noise is introduced into the discretized state equation model of the system to obtain:

[0162] z(k+1)=Az(k)+B1a(k)+B2w1(k)+B3η(k)

[0163] Wherein, B3 is the input matrix of process noise, η(k) is the process noise matrix at the discrete sampling time.

[0164]

[0165] S3. The state equation model of the system with introduced process noise obtained in step S2 is subjected to process noise and observation noise decorrelation processing, to obtain a decorrelated state equation model.

[0166] Further, the specific implementation method of step S3 includes the following steps:

[0167] S3.1. The covariance matrix of noise is defined as follows:

[0168]

[0169] Wherein, Q w ,Q2,R ζ are the covariance matrices of w1, η, ζ, E is the mathematical expectation symbol; E{w1} = 0, E{η} = 0, E{ζ} = 0 are set.

[0170] S3.2. The state equation of the discretized system with introduced process noise obtained in step S3 is introduced into the supplementary coefficient K1, to obtain a compensated state equation of the discretized system:

[0171]

[0172] Then, based on the above equation, the state term and the noise term are combined respectively, and the state equation expression of the system with combined state term and noise term is as follows:

[0173]

[0174] S3.3. Based on the process noise and observation noise decorrelation processing, the following objective is set:

[0175]

[0176] After expansion, the expansion formula is:

[0177]

[0178] Then, the covariance matrix of noise defined in step S3.1 is brought into the expansion formula, to obtain

[0179]

[0180] Based on R ζ is a positive definite matrix, D2Q w D2 T +R ζIf reversible, the calculation result of K1 is obtained:

[0181]

[0182] wherein, K 11 is the redefinition of K1 internal parameters;

[0183] S3.4. Substitute the calculation result of K1 obtained in step S3.3 into the state equation of the system obtained by combining the state term and the noise term obtained in step S3.2 respectively, to obtain the state equation of the decorrelated system, expressed as:

[0184]

[0185] y′=Cz+ξ

[0186] wherein, F is the state transition matrix after decorrelation, G is the noise input matrix after decorrelation, w4 is the process noise after decorrelation, ξ is the observation noise after decorrelation, and y′ is the measurement output vector after decorrelation;

[0187]

[0188] B=[B1 B2K 11 ]

[0189] G=[B2 B3]

[0190]

[0191] ξ=D2w1+ζ。

[0192] Further,

[0193]

[0194] S4. Design a steady-state Kalman filter algorithm based on the decorrelated state equation model obtained in step S3, for the estimation of the free-end speed of the rope based on multi-sensor fusion.

[0195] Further, the specific implementation method of step S4 includes the following steps:

[0196] S4.1. Set E{w4}=0 and E{ξ}=0, and construct the expression of the covariance matrix of the decorrelated process noise as:

[0197]

[0198] Then substitute the calculation result of K1 into the expression of the covariance matrix of the decorrelated process noise, to obtain:

[0199]

[0200] Based on R ζ Positive definite, so Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w Positive definite, the expression of the covariance matrix R of the decorrelated observation noise is set as:

[0201] R=E{ξξ T}=D2Q w D2 T +R ζ ;

[0202] E{w4ξ T}=0;

[0203] Further,

[0204]

[0205] S4.2. Based on the covariance matrix of the decorrelated process noise and the covariance matrix R of the decorrelated observation noise constructed in step S4.1, design a Kalman filtering algorithm for the state equation of the decorrelated system:

[0206] Calculate The corresponding mean square error is:

[0207] P(k|k-1)=FP(k-1)F T +GQG T

[0208] Wherein, is the Kalman filtering prediction value, P(k|k-1) indicates The corresponding mean square error, P(k-1) indicates The corresponding mean square error, F T Indicates the transpose matrix of the state transition matrix, G T Indicates the transpose matrix of the system noise driving matrix, F indicates the state transition matrix, and G indicates the system noise driving matrix.

[0209] Calculate the filtering gain, the expression is:

[0210] K(k)=P(k|k-1)C T (CP(k|k-1)C T +R) -1

[0211] Wherein, K(k) is the Kalman filter gain matrix of the discrete time index k;

[0212] Recursive calculation is obtained:

[0213] P(k) = (I - K(k)C)P(k|k-1)

[0214] Wherein, I is the unit matrix;

[0215] Then the state equation of the system after the design of the Kalman filter algorithm is obtained, and the update is:

[0216]

[0217] Then the state equation of the system after the design of the Kalman filter algorithm is obtained, and the update is:

[0218]

[0219] The steady-state solution of the state equation of the system after the design of the Kalman filter algorithm is obtained by Riccati equation:

[0220] P = FPF T -FPC T (CPC T +R) -1 CPF T +GQG T

[0221] The steady-state gain is as follows:

[0222] K = FPC T (CPC T +R) -1

[0223] The prediction state of the state equation of the system after the design of the Kalman filter algorithm is obtained, and the expression is:

[0224]

[0225] The expression of B is brought into the steady-state Kalman filter of one-step prediction, and the steady-state Kalman filter of one-step prediction is obtained:

[0226]

[0227] The method for estimating the speed of the free end of the rope based on multi-sensor fusion according to the embodiment is specifically implemented by simulating the feedback signal of the position sensor at the fixed end or the hoisting end of the rope without time drift. The amplitude of the simulated position signal is 10, and the frequency is 3, that is, a sine signal with an amplitude of 10 and a frequency of 3, as shown in Figure 2is shown; considering the uncertainty caused by the flexibility and dynamic characteristics of the transmission in the position measurement process, the real speed signal is designed as the sum of a cosine signal with an amplitude of 94.2 and a frequency of 3 and a sine signal with an amplitude of 47.1 and a frequency of 15, as shown in Figure 3 ; since there is a zero offset and noise in the accelerometer measurement feedback, the analog acceleration signal is designed as the sum of the derivative of the analog speed signal and a constant zero offset and high-frequency noise, as shown in Figure 4 ; see Figures 5 to 8 , respectively, the simulation speed estimation and estimation error of the multi-source information fusion speed estimation method of the embodiment, the state estimation simulation diagram, the position estimation simulation diagram, and the accelerometer zero offset estimation simulation diagram. From Figures 5 to 8 , it can be seen that for the multi-source information fusion speed estimation result of the relative position measurement sensor at the fixed end or the hoisting end, the speed estimation error value is between ±2.4, and the speed estimation converges at 4s. The accelerometer zero offset estimation value fluctuates around the true value. Figure 9 is the speed estimation simulation diagram obtained by weighting and averaging the position information differential processing and the acceleration information integral processing through low-pass and high-pass filters, respectively. The estimation result error is between ±20.5 and converges at 5.3s. It can be seen that the result of the multi-source information fusion speed estimation method of the embodiment is more accurate and converges faster.

[0228] Embodiment 2

[0229] A rope free end winding and unwinding speed estimation method based on multi-sensor fusion, comprising the following steps:

[0230] S1. Based on different motion states, installing an angle measurement sensor or a speed measurement sensor at the steel wire rope hoisting end or the fixed end, and installing a speed measurement sensor at the steel wire rope free end to collect data under different motion states;

[0231] Further, the hoisting end or the fixed end in step S1 is rotating motion, and one of an encoder, a circular grating, a magnetic grating, an inductive synchronizer, a rotary transformer or a speed measurement motor is installed to measure the relative rotation angle and angular velocity of the stator and the rotor;

[0232] The fixed end is linear motion, and one of a linear grating or a magnetic grating is installed to measure the relative linear displacement of the stator and the rotor;

[0233] An accelerometer is installed at the free end;

[0234] S2. Based on the steel wire rope hoisting end or the fixed end using the angle measurement sensor to construct a kinematic model, based on the steel wire rope hoisting end or the fixed end using the speed measurement sensor to construct a kinematic model, and comprehensively obtaining a system state equation model, then discretizing the system state equation model and introducing process noise to obtain a system state equation model with process noise;

[0235] Further, the specific implementation method of constructing the kinematic model based on the hoisting end or the fixed end using the speed sensor in step S2 is as follows:

[0236] The free end kinematic model is established as follows:

[0237]

[0238] wherein the free end position is x, the free end speed is v, the accelerometer reading is a, and the accelerometer zero offset is θ;

[0239] The observation equation is established based on the hoisting end or the fixed end using the speed sensor:

[0240]

[0241] wherein y2 is the measurement of the rotational angular velocity at the installation position of the hoisting end or the fixed end using the speed sensor;

[0242] Let be generated by driving the following high-pass link, that is, a high-pass filter that suppresses low-frequency signals and allows high-frequency signals to pass normally, and the expression is:

[0243]

[0244] wherein ω a is the angular frequency of the driving link, s is the integral symbol, and w1 is the zero-mean white noise of the driving link;

[0245] The state space minimum realization is obtained by driving the following high-pass link:

[0246]

[0247] Further, the state equation model of the system obtained by comprehensively integrating in step S2 is:

[0248]

[0249] y = Cz + D2w1 + ζ

[0250] wherein z is the extended state vector, is the extended system matrix, is the extended input matrix, is the input matrix of white noise, C is the extended observation matrix, and D2 is the input coefficient corresponding to w1;

[0251] z = [v θa] T

[0252]

[0253] C = [1 0 -ω a ];

[0254] D = [0 D2]

[0255] D2 = ω a

[0256] Then the state equation model of the system obtained after synthesis is discretized to obtain the discretized state equation model of the system:

[0257] z(k + 1) = Az(k) + B1a(k) + B2w1(k)

[0258] Wherein, k is the discrete sampling time index, A is the discrete system matrix, B1 is the discrete input matrix, B2 is the discrete input matrix of white noise, w1(k) is the zero mean white noise of the driving link at the discrete sampling time;

[0259] Then the state equation model of the discretized system is introduced into the process noise to obtain:

[0260] z(k + 1) = Az(k) + B1a(k) + B2w1(k) + B3η(k)

[0261] Wherein, B3 is the input matrix of process noise, η(k) is the process noise matrix at the discrete sampling time;

[0262]

[0263] S3. The state equation model of the system introduced into the process noise obtained in step S2 is processed to remove the correlation between the process noise and the observation noise, and the de-correlated state equation model is obtained;

[0264] Further, the specific implementation method of step S3 includes the following steps:

[0265] S3.1. Define the covariance matrix of noise as follows:

[0266]

[0267] Wherein, Q w ,Q2,R ζ The covariance matrix of w1, η, ζ, respectively, E is the mathematical expectation symbol; Set E{w1} = 0, E{η} = 0, E{ζ} = 0;

[0268] S3.2. The state equation of the discretized system introduced into the process noise obtained in step S3 is introduced into the supplementary coefficient K1 to obtain the compensated state equation of the discretized system:

[0269]

[0270] Then, the state term and the noise term are combined based on the above equation, and the state equation expression of the system after the state term and the noise term are combined is as follows:

[0271]

[0272] S3.3. Based on the process noise and the observation noise decorrelation processing, the following target is set:

[0273]

[0274] After expansion, the expansion expression is:

[0275]

[0276] Then, the covariance matrix of the defined noise defined in step S3.1 is brought into the expansion expression, and the following expression is obtained:

[0277]

[0278] Based on R ζ is a positive definite matrix, D2Q w D2 T +R ζ is reversible, and the calculation result of K1 is obtained:

[0279]

[0280] wherein, K 11 is a redefinition of the internal parameters of K1;

[0281] S3.4. The calculation result of K1 obtained in step S3.3 is substituted into the state equation of the system after the state term and the noise term are combined obtained in step S3.2, and the state equation of the decorrelated system is obtained, and the expression is:

[0282]

[0283] y′=Cz+ξ

[0284] wherein, F is the state transition matrix after decorrelation, G is the noise input matrix after decorrelation, w4 is the process noise after decorrelation, ξ is the observation noise after decorrelation, and y′ is the measurement output vector after decorrelation;

[0285]

[0286] B=[B1 B2K 11 ]

[0287] G=[B2 B3]

[0288]

[0289] ξ=D2w1+ζ;

[0290] Furthermore,

[0291]

[0292] S4. Based on the decorrelation-free state equation model obtained in step S3, design a steady-state Kalman filter algorithm for estimating the release and take-up velocity of the free end of the rope based on multi-sensor fusion.

[0293] Furthermore, the specific implementation method of step S4 includes the following steps:

[0294] S4.1. Setting E{w4} = 0 and E{ξ} = 0, the expression for constructing the covariance matrix of the decorrelated process noise is:

[0295]

[0296] Then, substituting the calculated result of K1 into the expression for the covariance matrix of the decorrelated process noise, we obtain:

[0297]

[0298] Furthermore, the Q matrix is ​​designed as follows:

[0299] Based on R ζ Positive definiteness, therefore Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w Positive definite, let the expression for the covariance matrix R of the decorrelated observation noise be:

[0300] R=E{ξξ T}=D2Q w D2 T +R ζ ;

[0301] E{w4ξ T} = 0;

[0302] S4.2. Based on the covariance matrix of the decorrelated process noise and the covariance matrix R of the decorrelated observation noise constructed in step S4.1, design a Kalman filtering algorithm for the state equation of the decorrelated system:

[0303] calculate The corresponding mean square error:

[0304] P(k|k-1) = FP(k-1)F T +GQG T

[0305] wherein, is the Kalman filter prediction value, P(k|k-1) refers to the corresponding mean square error, P(k-1) refers to the corresponding mean square error, F refers to the state transition matrix, and G refers to the system noise driving matrix; T represents the transpose matrix of the state transition matrix, G T represents the transpose matrix of the system noise driving matrix, F refers to the state transition matrix, and G refers to the system noise driving matrix;

[0306] The filter gain is calculated, and the expression is as follows:

[0307] K(k) = P(k|k-1)C T (CP(k|k-1)C T +R) -1

[0308] wherein, K(k) is the Kalman filter gain matrix of the discrete time index k;

[0309] The recursive calculation is as follows:

[0310] P(k) = (I-K(k)C)P(k|k-1)

[0311] wherein, I is the unit matrix;

[0312] The state equation of the system after the Kalman filter algorithm is designed to decorrelate is then updated as follows:

[0313]

[0314] The state equation of the system after the Kalman filter algorithm is designed to decorrelate is then updated as follows:

[0315]

[0316] The steady-state solution of the state equation of the system after the Kalman filter algorithm is designed to decorrelate is obtained from the Riccati equation as follows:

[0317] P = FPF T -FPC T (CPC T +R) -1 CPF T +GQG T

[0318] The steady-state gain is as follows:

[0319] K = FPC T (CPC T + R) -1

[0320] The state equation of the system after the one-step prediction design Kalman filter algorithm is predicted, and the expression is:

[0321]

[0322] The expression of B is brought into, and the one-step prediction steady-state Kalman filter is obtained:

[0323]

[0324] The method for estimating the speed of the free end of the rope based on multi-sensor fusion according to the embodiment is described in the following Figures 10 to 12 , which are the simulation speed estimation and estimation error, state estimation simulation diagram, and accelerometer zero offset estimation simulation diagram of the second embodiment of the multi-source information fusion speed estimation method of the application. From Figures 10 to 12 It can be seen that the multi-source information fusion speed estimation result of the fixed end or the hoisting end using the relative position measurement sensor has a speed estimation error value between ±2.4, which is close to the accuracy of the first embodiment. It can be seen that the application can achieve high-precision speed estimation for the fixed end or the hoisting end using the relative position measurement sensor and the speed measurement element. Through the comprehensive steady-state Kalman filter and multi-source data fusion, the speed of the free end of the rope is estimated by using the relative position measurement sensor or the speed measurement element at the hoisting end or the fixed end and the accelerometer at the free end of the rope. For the method of processing the position information and the acceleration information through filtering and weighted average, the estimation error for the speed peak value of 137 is ±20.5, while the simulation result of the application scheme can reduce the speed estimation error to within ±2.4, and the convergence speed is faster.

[0325] It should be noted that the relational terms, such as "first" and "second", and the like, are used solely to distinguish one entity or action from another entity or action, without necessarily requiring or implying any such actual relationship or order between such entities or actions. Moreover, the terms "comprises", "comprising", or any other variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements recited, but can also include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without more limitations, an element defined by the statement "comprising a" does not exclude the existence of additional identical elements in the process, method, article, or apparatus that includes the element.

[0326] Although the present application has been described with reference to the specific embodiments thereof, it should be understood by those skilled in the art that various changes can be made and equivalents can be substituted for elements thereof without departing from the scope of the present application. In particular, various features and aspects of the present application can be used individually or in any combination depending on the specific application and implementation. Therefore, it is expressly intended that the specific embodiments of the present application both as set forth and including any equivalents thereof should not limit the present application or scope of the claims herein, but rather the overall scope of pertaining solely to the methods and the articles of manufacture specifically recited in the following claims.

Claims

1. A method for estimating the speed of a free end of a rope based on multi-sensor fusion, characterized in that, Comprising the following steps: S1. Based on different motion states, install an angle measuring sensor or a speed measuring sensor at the wire rope winding end or the fixed end, and install a speed measuring sensor at the free end of the wire rope, and collect data under different motion states; Wherein, an accelerometer is installed at the free end; S2. Based on the wire rope winding end or the fixed end, use the angle measuring sensor to construct a kinematic model, based on the wire rope winding end or the fixed end, use the speed measuring sensor to construct a kinematic model, and then obtain a system state equation model after integration, and then discretize the system state equation model and introduce process noise to obtain a system state equation model with process noise introduced; Wherein, the measurement noise is composed of high-frequency error terms and zero-mean white noise; The process noise variance of each state is a positive definite adjustable term, which is used to attenuate the influence of unknown initial state; S3. Process noise and observation noise decoupling processing is performed on the system state equation model with process noise introduced obtained in step S2 to obtain a decoupled state equation model; S4. Based on the decoupled state equation model obtained in step S3, a steady-state Kalman filter algorithm is designed for wire rope free end speed estimation based on multi-sensor fusion; The specific implementation method of step S4 comprises the following steps: S4.

1. Set E{w4}=0, E{ξ}=0, and construct a covariance matrix Q of the decoupled process noise; w4 is the decoupled process noise, and ξ is the decoupled observation noise; S4.

2. Based on the covariance matrix of the decoupled process noise and the covariance matrix R of the decoupled observation noise constructed in step S4.1, a Kalman filter algorithm is designed for the decoupled system state equation: Computations Corresponding mean square error: P(k|k-1) = FP(k-1) F T +GQG T wherein is the Kalman filter prediction, P(k|k-1) denotes the corresponding mean square error, P(k-1) denotes the corresponding mean square error, F T denotes the transpose of the state transition matrix, G T denotes the transpose of the system noise driving matrix, F denotes the state transition matrix, G denotes the system noise driving matrix, z is the state equation of the decorrelated system, and k is the discrete sampling time index; The filter gain is calculated, and the expression is: K(k) = P(k|k-1)C T (CP(k|k-1)C T +R) -1 Wherein, K(k) is the Kalman filter gain matrix of the discrete time index k; C is the extended observation matrix, which is recursively calculated: P(k)=(I-K(k)C)P(k|k-1) Wherein, I is the unit matrix; Then the decoupled system state equation after designing the Kalman filter algorithm is updated as:

2. The method of claim 1, wherein, In step S1, the winding end or the fixed end is in rotary motion, and one of an encoder, a circular grating, a magnetic grating, an inductive synchronizer, a rotary transformer or a speed measuring motor is installed for measuring the relative rotation angle and angular velocity of the stator and the rotor; The fixed end is in linear motion, and one of a linear grating or a magnetic grating is installed for measuring the relative linear displacement of the stator and the mover.

3. The method of claim 1 or 2, wherein, The specific implementation method of step S2 for constructing a kinematic model based on the angle measuring sensor at the winding end or the fixed end is as follows: The free end kinematic model is established as follows: where free end position is x, free end velocity is v, accelerometer reading is a, and accelerometer zero offset is θ, is the derivative of the free end position; The state motion model of the free end kinematic model is denoted as: wherein is a derivative of a state variable, is a system matrix, is an input matrix, and z1 is a state variable; The observation equation is established based on the angle measuring sensor at the winding end or the fixed end: Wherein, y1 is the measurement of the rotation angle at the installation position by the angle sensor used at the hoisting end or the fixed end, c r is the transmission ratio, is the high-frequency measurement error, and ζ is the zero-mean white noise of the observation equation. Setting is generated by driving a high-pass element, that is, a high-pass filter that has an inhibitory effect on low-frequency signals and allows high-frequency signals to pass normally, and is expressed by the following expression: where ω a is the drive link angular frequency, s is the integral sign, and w1 is the zero-mean white noise of the drive link. The state space minimum realization is obtained by driving the following high-pass element:

4. The method of claim 3, wherein, The specific implementation method of step S2 for constructing a kinematic model based on the speed measuring sensor at the winding end or the fixed end is as follows: The free end kinematic model is established as follows: Wherein, the free end position is x, the free end speed is v, the accelerometer reading is a, and the accelerometer zero offset is θ; The observation equation is established based on the speed measuring sensor at the winding end or the fixed end: Wherein, y2 is the measurement of the rotational angular velocity at the installation position of the hoist end or fixed end using the speed sensor.

5. The method of claim 4, wherein, The state equation model of the system obtained after the integration in step S2 is: y=Cz+D2w1+ζ where z is the extended state vector, is the extended system matrix, is the extended input matrix, is the input matrix of white noise, C is the extended observation matrix, and D2 is the input coefficient corresponding to w1. C = [C1 - ω a ] D=[0D2] D2= ω a Wherein, C1 is the observation matrix of the original system, and D is the feedforward matrix of the extended system; Then, the state equation model of the system obtained after the integration is discretized to obtain the discretized state equation model of the system: z(k+1)=Az(k)+B1a(k)+B2w1(k) Wherein, k is the discrete sampling time index, A is the system matrix after the discretization, B1 is the input matrix after the discretization, B2 is the discrete input matrix of the white noise, and w1(k) is the zero-mean white noise of the driving link at the discrete sampling time; Then, the process noise is introduced into the discretized state equation model of the system to obtain: z(k+1)=Az(k)+B1a(k)+B2w1(k)+B3η(k) Wherein, B3 is the input matrix of the process noise, and η(k) is the process noise matrix at the discrete sampling time.

6. The method of claim 5, wherein, The specific implementation method of step S3 includes the following steps: S3.

1. The covariance matrix of the noise is defined as follows: wherein Q w , Q2, R ζ are the covariance matrices of w1, η, ζ, respectively, and E is the symbol for mathematical expectation; set E{w1} = 0, E{η} = 0, E{ζ} = 0; S3.

2. The discretized state equation of the system obtained in step S2 is introduced into the supplementary coefficient K1 to obtain the compensated discretized state equation of the system: Then, based on the above equation, the state term and the noise term are combined respectively to obtain the state equation expression of the system after the state term and the noise term are combined respectively: S3.

3. Based on the process noise and the observation noise decorrelation processing, the following target is set: After expansion, the expansion formula is obtained: Then, the definition noise covariance matrix defined in step S3.1 is brought into the expansion formula to obtain Based on R ζ is a positive definite matrix, D2Q w D2 T +R ζ is invertible, the result of the calculation of K1 is obtained: where K 11 is a redefinition of the K1 internal parameter; S3.

4. The calculation result of K1 obtained in step S3.3 is substituted into the state equation of the system obtained in step S3.2 after the state term and the noise term are combined respectively to obtain the decorrelated state equation of the system, and the expression is: y′=Cz+ξ Wherein, F is the state transition matrix after decorrelation, G is the noise input matrix after decorrelation, and y′ is the measurement output vector after decorrelation; B = [B1 B2 K 11 ] G=[B2 B3] ξ=D2w1+ζ.

7. The method of claim 6, wherein, The specific implementation method of step S4 includes the following steps: S4.

1. Set E{w4}=0, E{ξ}=0, and construct the expression of the covariance matrix of the decorrelated process noise as follows: Then, the calculation result of K1 is substituted into the expression of the decorrelated process noise covariance matrix to obtain Based on R ζ Positive definite, so Q w - Q w D2 T (D2Q w D2 T + R ζ ) -1 D2Q w Positive definite, the expression of the covariance matrix R of the observation noise after decorrelation is set as: R = E{ξξ T} = D2Q w D2 T + R ζ ; E{w4ξ T} = 0; S4.

2. Based on the decorrelated process noise covariance matrix and the decorrelated observation noise covariance matrix R constructed in step S4.1, the Kalman filtering algorithm is designed for the decorrelated state equation of the system: Computations Corresponding mean square error: P(k|k-1) = FP(k-1) F T + GQG T wherein, is the Kalman filter prediction, P(k|k-1) denotes the corresponding mean square error, P(k-1) denotes the corresponding mean square error, F T denotes the transpose of the state transition matrix, G T denotes the transpose of the system noise driving matrix, F denotes the state transition matrix, and G denotes the system noise driving matrix; The filter gain is calculated, and the expression is: K(k) = P(k|k-1)C T (CP(k|k-1)C T +R) -1 Wherein, K(k) is the Kalman filter gain matrix at the discrete time index k; Recursive calculation is performed to obtain: P(k)=(I-K(k)C)P(k|k-1) Wherein, I is the unit matrix; Then, the state equation of the system after the Kalman filtering algorithm is designed is updated as: Then the state equation of the system after decorrelation by the one-step prediction design Kalman filter algorithm is obtained as follows: The steady-state solution of the state equation of the system after decorrelation by the one-step prediction design Kalman filter algorithm is obtained by the Riccati equation as follows: P = FPF T - FPC T (CPC T + R) -1 CPF T + GQG T The steady-state gain is as follows: K = FPC T (CPC T +R) -1 The predicted state of the state equation of the system after decorrelation by the one-step prediction design Kalman filter algorithm is as follows: The steady-state Kalman filter of one-step prediction is obtained by substituting the expression of B into the following equation:

Images