Rope free end winding and unwinding speed estimation method based on multi-sensor fusion

Abstract

The invention discloses a rope free end take-up and pay-off speed estimation method based on multi-sensor fusion, and belongs to the technical field of information physical systems. In order to solve the problem that real-time and accurate speed and position estimation is carried out in a mode of fusing an accelerometer and other sensors mounted at the winding end, the method comprises the following steps of: constructing a kinematic model by using an angle measurement sensor based on the winding end or the fixed end of the steel wire rope, and constructing a kinematic model by using a speed measurement sensor based on the winding end or the fixed end of the steel wire rope; performing integration to obtain a state equation model of the system, discretizing the state equation model of the system, introducing process noise, performing process noise and observation noise decorrelation processing on the obtained state equation model of the system introduced with the process noise, and obtaining a decorrelated state equation model; and designing a steady-state Kalman filtering algorithm by using the obtained decorrelated state equation model, wherein the steady-state Kalman filtering algorithm is used for estimating the winding and unwinding speed of the free end of the rope based on multi-sensor fusion. The method is accurate in estimation.

Description

Technical Field

[0001] The present invention belongs to the technical field of cyber-physical systems, and particularly relates to a method for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion. Background Art

[0002] The method for estimating the speed of the free end of a rope based on multi-sensor fusion is used in occasions where it is impossible to directly and accurately measure the movement speed and position of the free end of the rope in a winch system in real time. Since other workpieces and equipment that can be replaced at any time are often suspended at the lower end of the rope system, and there is a lack of reference coordinates around, it is impossible to directly measure the relative speed and position of the hook in real time. The winch end can be measured by angle measuring elements such as a code disk, but the intermediate transmission flexibility affects the measurement error of the high-frequency components when approximating the position and speed of the free end with the position and speed of the winch end. Especially in dynamic assembly systems and force control systems, in occasions where high requirements are placed on the measurement accuracy and real-time performance of the speed and position of the suspension point end, the excessive measurement error has a great impact on the system. Summary of the Invention

[0003] The problem to be solved by the present invention is to use the method of fusing an accelerometer with other sensors installed at the winch end to perform real-time and accurate speed and position estimation, and to propose a method for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion.

[0004] To achieve the above object, the present invention is realized through the following technical solutions:

[0005] A method for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion, comprising the following steps:

[0006] S1. Based on different motion states, install an angle measuring sensor or a speed measuring sensor at the winch end or the fixed end of the steel wire rope, and install a speed measuring sensor at the free end of the steel wire rope to collect data under different motion states;

[0007] S2. Based on the angle measuring sensor used at the winch end or the fixed end of the steel wire rope, construct a kinematic model, and based on the speed measuring sensor used at the winch end or the fixed end of the steel wire rope, construct a kinematic model. After synthesis, obtain the state equation model of the system, and then discretize the state equation model of the system and introduce process noise to obtain the state equation model of the system with introduced process noise;

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

[0009] S4. Design a steady-state Kalman filtering algorithm based on the decorrelated state equation model obtained in step S3 for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion.

[0010] Further, in step S1, the winching end or the fixed end is in rotational motion, and one of an encoder, a circular grating, a magnetic grating, an inductosyn, a resolver, or a tachogenerator is installed to measure the relative rotational angle and angular velocity of the stator and the rotor;

[0011] The fixed end is in 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 mover;

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

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

[0014] Establish the kinematic model of the free end as follows:

[0015]

[0016] where the position of the free end is x, the velocity of the free end is v, the accelerometer reading is a, and the accelerometer zero bias is θ, is the derivative of the free end position;

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

[0018]

[0019] where, is the derivative of the state variable, is the system matrix, is the input matrix, and z1 is the state variable;

[0020] Establish an observation equation based on the winching end or the fixed end using an angle sensor:

[0021]

[0022] where y1 is the measurement of the rotational angle at the installation position using the angle sensor at the winching 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;

[0023] 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 through normally, and the expression is:

[0024]

[0025] where ω ais the angular frequency of the driving link, s is the integral symbol, and w1 is the white noise with zero mean of the driving link;

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

[0027]

[0028] Furthermore, the specific implementation method of constructing the kinematic model based on the winch end or the fixed end using a speed sensor in step S2 is as follows:

[0029] Establish the kinematic model of the free end as follows:

[0030]

[0031] where the position of the free end is x, the velocity of the free end is v, the accelerometer reading is a, and the accelerometer zero - bias is θ;

[0032] Based on the winch end or the fixed end, use the speed sensor to establish the observation equation:

[0033]

[0034] where y2 is the measurement of the rotational angular velocity at the installation position using the speed sensor at the winch end or the fixed end.

[0035] Furthermore, the state - equation model of the system obtained after synthesis in 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 = [0 D2]

[0042] D2 = ω a

[0043] where C1 is the original system observation matrix and D is the feed - forward matrix of the extended system;

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

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

[0046] where k is the discrete sampling time label, A is the discrete system matrix, B1 is the discrete input matrix, B2 is the discrete input matrix of white noise, and w1(k) is the white noise with zero mean in the drive link at the discrete sampling time;

[0047] Then, 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] where B3 is the input matrix of process noise, and η(k) is the process noise matrix at the discrete sampling time.

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

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

[0052]

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

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

[0055]

[0056] Then, based on the above equation, the state terms and noise terms are merged respectively to obtain the state equation expression of the system with the state terms and noise terms merged as follows:

[0057]

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

[0059]

[0060] The expanded expression is:

[0061]

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

[0063]

[0064] Based on R ζ being a positive definite matrix, D2Q w D2 T +R ζ being invertible, the calculation result of K1 can be obtained:

[0065]

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

[0067] S3.4. Substitute the calculation result of K1 obtained in step S3.3 into the state equation of the system in which the state terms and noise terms obtained in step S3.2 are respectively combined to obtain the state equation of the decorrelated system, and the expression is:

[0068]

[0069] y′ = Cz + ξ

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

[0071]

[0072] B = [B1 B2K 11

[0073] G = [B2 B3]

[0074]

[0075] ξ = D2w1 + ζ.

[0076] Furthermore, 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 decorrelated process noise as:

[0078]

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

[0080]

[0081] Based on R ζ being positive definite, thus Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w is positive definite. Set the expression of the covariance matrix R of the decorrelated observation noise as:

[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] where 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 T represents the transpose matrix of the state transition matrix, G T represents the transpose matrix of the system noise driving matrix, F represents the state transition matrix, and G represents the system noise driving matrix;

[0088] Calculate the filtering gain, and the expression is:

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

[0090] Among them, \(K(k)\) is the Kalman filter gain matrix of the discretized time label \(k\);

[0091] It is obtained by recursive calculation:

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

[0093] Among them, \(I\) is the identity matrix;

[0094] Then, after obtaining the Kalman filter algorithm, the state equation of the decorrelated system is updated to:

[0095]

[0096] Then, for the one-step prediction design of the Kalman filter algorithm, the state equation of the decorrelated system is obtained:

[0097]

[0098] The steady-state solution of the state equation of the decorrelated system after designing the Kalman filter algorithm is obtained from the 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] For the state equation of the one-step prediction design of the Kalman filter algorithm for the decorrelated system, the predicted state expression is:

[0103]

[0104] After substituting the expression of \(B\), the steady-state Kalman filter for one-step prediction is obtained:

[0105]

[0106] The beneficial effects of the present invention:

[0107] A method for estimating the retracting and paying-out speed of the free end of a rope based on multi-sensor fusion according to the present invention establishes a state equation and a measurement equation for signal motion, wherein the measurement noise is a combination of a high-frequency error term and white noise with zero mean; the process noise variances of each state are positive definite adjustable terms for attenuating the influence of unknown initial states; output feedback is used to decouple the measurement noise and the motion noise; the zero bias and noise of the accelerometer are considered and compensated; a steady-state Kalman filtering algorithm is designed to estimate the speed, position at the accelerometer installation location, and the zero bias of the accelerometer. The present invention does not need to establish a complex dynamic model and can obtain high accuracy only with a low-order kinematic model. It has the characteristics that at low frequencies, it approaches the differentiation of signals such as encoders, and at medium and high frequencies, the motion details approach the integration of accelerometers, and it has low dependence on the high-frequency uncertainty of the transmission dynamic characteristics and the accuracy of the model. Description of the Drawings

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

[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 Embodiment 1 of the multi-source information fusion speed estimation method of the present invention;

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

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

[0115] Figure 8 It is a simulation acceleration zero bias estimation diagram of Embodiment 1 of the multi-source information fusion speed estimation method of the present invention;

[0116] Figure 9 It is a simulation diagram of speed estimation by performing low-pass, 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 Embodiment 2 of the multi-source information fusion speed estimation method of the present invention;

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

[0119] Figure 12 This is the simulation acceleration zero bias estimation diagram of Embodiment 2 of the multi-source information fusion speed estimation method of the present invention;

[0120] Figure 13 This is the flow chart of a method for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion according to the present invention. Detailed implementation manners

[0121] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and specific implementation manners. It should be understood that the specific implementation manners described herein are only used to explain the present invention and are not used to limit the present invention, that is, the specific implementation manners described are only a part of the implementation manners of the present invention, rather than all the specific implementation manners. Usually, the components of the specific implementation manners of the present invention described and shown in the accompanying drawings herein can be arranged and designed in various different configurations, and the present invention can also have other implementation manners.

[0122] Therefore, the detailed description of the specific implementation manners of the present invention provided in the accompanying drawings below is not intended to limit the scope of the claimed present invention, but only represents the selected specific implementation manners of the present invention. All other specific implementation manners obtained by those skilled in the art based on the specific implementation manners of the present invention without creative efforts belong to the scope of protection of the present invention.

[0123] To further understand the content, features and effects of the present invention, the following specific implementation manners are exemplified and combined with the attached Figure 1 - Attached Figure 13 The details are as follows:

[0124] Embodiment 1:

[0125] A method for estimating the retracting and extending speed of the free end of a rope based on multi-sensor fusion includes the following steps:

[0126] S1. Based on different motion states, install an angle sensor or a speed sensor at the winding end or the fixed end of the steel wire rope, and install a speed sensor at the free end of the steel wire rope to collect data under different motion states;

[0127] Further, in step S1, if the winding end or the fixed end is in rotational motion, install an encoder for measuring the relative rotational angle and angular velocity of the stator and the rotor;

[0128] If the fixed end is in linear motion, install one of a linear grating or a magnetic grating for measuring the relative linear displacement of the stator and the mover;

[0129] Install an accelerometer at the free end.

[0130] S2. Based on the use of angle sensors at the winching end or fixed end of the wire rope to construct a kinematic model, and based on the use of speed sensors at the winching end or fixed end of the wire rope to construct a kinematic model, the state equation model of the system is obtained through synthesis. 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;

[0131] Further, the specific implementation method of constructing a kinematic model based on the use of angle sensors at the winching end or fixed end in step S2 is as follows:

[0132] Establish the kinematic model of the free end as follows:

[0133]

[0134] where the position of the free end is x, the velocity of the free end is v, the accelerometer reading is a, and the zero bias of the accelerometer is θ, is the derivative of the free end position;

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

[0136]

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

[0138] Based on the use of angle sensors at the winching end or fixed end, establish an observation equation:

[0139]

[0140] where y1 is the measurement of the rotation angle at the installation position using an angle sensor at the winching end or 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;

[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 through normally. The expression is:

[0142]

[0143] where ω 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 obtained by driving the following high-pass link is:

[0145]

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

[0147]

[0148] y = Cz + D2w1 + ζ

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

[0150]

[0151] C = [C1 -ω a

[0152] D = [0 D2]

[0153] D2 = ω a

[0154] where C1 is the original system observation matrix and D is the feedforward matrix of the extended system;

[0155] When an angle measuring element is used at the hoisting end (fixed end),

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

[0157]

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

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

[0160] where k is the discrete sampling time label, A is the discrete system matrix, B1 is the discrete input matrix, B2 is the discrete input matrix of white noise, and w1(k) is the zero-mean white noise of the drive link at the discrete sampling time;

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

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

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

[0164]

[0165] S3. Perform decorrelation processing on the process noise and observation noise of the state equation model of the system with process noise obtained in step S2 to obtain the decorrelated state equation model;

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

[0167] S3.1. Define the covariance matrix expression of the noise as follows;

[0168]

[0169] Among them, Q w , Q2, R ζ are the covariance matrices of w1, η, and ζ respectively, and E is the symbol for calculating the mathematical expectation; set E{w1} = 0, E{η} = 0, E{ζ} = 0;

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

[0171]

[0172] Then, based on the above equation, combine the state terms and noise terms respectively to obtain the state equation expression of the system with the state terms and noise terms combined as follows:

[0173]

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

[0175]

[0176] After expansion, the expansion formula is:

[0177]

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

[0179]

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

[0181]

[0182] where K 11 is the redefinition of the internal parameter of K1;

[0183] S3.4. Substitute the calculation result of K1 obtained in step S3.3 into the state equation of the system where the state terms and noise terms obtained in step S3.2 are respectively combined, and the state equation of the decorrelated system is obtained. The expression is:

[0184]

[0185] y′ = Cz + ξ

[0186] where 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] Furthermore,

[0193]

[0194] S4. Design a steady-state Kalman filtering algorithm based on the decorrelated state equation model obtained in step S3 for estimating the retracting and releasing speed of the free end of the rope based on multi-sensor fusion.

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

[0196] S4.1. Set E{w4} = 0, 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 ζ is positive definite, so Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w is positive definite. Set the expression of the covariance matrix R of the decorrelated observation noise as:

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

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

[0203] Furthermore,

[0204]

[0205] S4.2. Design a Kalman filtering algorithm for the state equation of the decorrelated system 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:

[0206] Calculate the corresponding mean square error:

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

[0208] where, 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 T represents the transpose matrix of the state transition matrix, G T represents the transpose matrix of the system noise driving matrix, F represents the state transition matrix, and G represents the system noise driving matrix;

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

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

[0211] Among them, \(K(k)\) is the Kalman filter gain matrix of the discretized time label \(k\);

[0212] It is obtained by recursive calculation:

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

[0214] Among them, \(I\) is the identity matrix;

[0215] Then, after obtaining the designed Kalman filter algorithm, the state equation of the decorrelated system is updated to:

[0216]

[0217] Then, for the one-step prediction of the state equation of the decorrelated system after designing the Kalman filter algorithm, we get:

[0218]

[0219] The steady-state solution of the state equation of the decorrelated system after designing the Kalman filter algorithm is obtained from the 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] For the one-step prediction of the state equation of the decorrelated system after designing the Kalman filter algorithm, the predicted state expression is:

[0224]

[0225] After substituting the expression of \(B\), the one-step prediction steady-state Kalman filter is:

[0226]

[0227] A method for estimating the rope free-end retracting and extending speed based on multi-sensor fusion described in this embodiment is specifically implemented by simulating the feedback signal of a position sensor that does not drift with time at the fixed end or the winch end of the rope. The simulated position signal is designed as a sine signal with an amplitude of 10 and a frequency of 3, such as Figure 2As shown in the figure; considering the uncertainty caused by the flexibility of the transmission and the influence of dynamic characteristics during the position measurement process, the simulated 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 Figure 3 shown; since there are zero bias and noise in the measurement feedback of the accelerometer, the simulated acceleration signal is designed as the sum of the derivative of the simulated speed signal, a constant zero bias, and high-frequency noise, as Figure 4 shown. See Figures 5 to 8 , which are respectively the simulation speed estimation and estimation error, state estimation simulation diagram, position estimation simulation diagram, and accelerometer zero bias estimation simulation diagram of Example 1 of the multi-source information fusion speed estimation method in this embodiment. From Figures 5 to 8 , it can be seen that for the multi-source information fusion speed estimation results using relative position measurement sensors at the fixed end or the hoisting end, the speed estimation error value is between ±2.4, and the speed estimation converges in 4s. The accelerometer zero bias estimation value fluctuates around the true value. Figure 9 is the speed estimation simulation diagram obtained by weighted averaging after differential processing of position information and integral processing of acceleration information through low-pass and high-pass filters respectively. The estimation result error is between ±20.5 and converges in 5.3s. It can be seen that the multi-source information fusion speed estimation method in this embodiment has more accurate results and faster convergence speed.

[0228] Example 2:

[0229] A method for estimating the retracting and releasing speed of the free end of a rope based on multi-sensor fusion includes the following steps:

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

[0231] Further, in step S1, if the hoisting end or the fixed end is in rotational motion, install one of an encoder, a circular grating, a magnetic grating, an induction synchronizer, a resolver, or a tachogenerator to measure the relative rotational angle and angular velocity of the stator and the rotor;

[0232] If the fixed end is in linear motion, install one of a linear grating or a magnetic grating to measure the relative linear displacement of the stator and the mover;

[0233] Install an accelerometer at the free end;

[0234] S2. Based on the angle measurement sensor used at the hoisting end or the fixed end of the steel wire rope, construct a kinematic model, and based on the speed measurement sensor used at the hoisting end or the fixed end of the steel wire rope, construct a kinematic model. After synthesis, obtain the state equation model of the system, and then discretize the state equation model of the system and introduce process noise to obtain the state equation model of the system with introduced process noise;

[0235] Furthermore, the specific implementation method of constructing the kinematic model based on the winch end or the fixed end in step S2 is as follows:

[0236] Establish the kinematic model of the free end as follows:

[0237]

[0238] where the position of the free end is x, the velocity of the free end is v, the accelerometer reading is a, and the accelerometer zero bias is θ;

[0239] Based on the winch end or the fixed end, use a speed sensor to establish an observation equation:

[0240]

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

[0242] 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 through normally. The expression is:

[0243]

[0244] where ω 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 obtained by driving the following high-pass link is:

[0246]

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

[0248]

[0249] y = Cz + D2w1 + ζ

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

[0251] z = [v θα] T

[0252]

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

[0254] D = [0 D2]

[0255] D2 = ω a

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

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

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

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

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

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

[0262]

[0263] S3. Perform decorrelation processing on the process noise and the observation noise of the state equation model of the system with process noise obtained in step S2 to obtain the decorrelated state equation model;

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

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

[0266]

[0267] where Q w , Q2, R ζ are the covariance matrices of w1, η, and ζ respectively, and E is the symbol for taking the mathematical expectation; set E{w1} = 0, E{η} = 0, E{ζ} = 0;

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

[0269]

[0270] Then, based on the above equations, the state terms and noise terms are combined respectively to obtain the state equation expressions of the system with the state terms and noise terms combined respectively as follows:

[0271]

[0272] S3.3. Based on the decorrelation processing of process noise and observation noise, the following objectives are set:

[0273]

[0274] After expansion, the expansion formula is obtained as:

[0275]

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

[0277]

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

[0279]

[0280] where K 11 is the redefinition of the internal parameters of K1;

[0281] S3.4. Substitute the calculation result of K1 obtained in step S3.3 into the state equation of the system with the state terms and noise terms combined respectively obtained in step S3.2 to obtain the state equation of the decorrelated system, and the expression is:

[0282]

[0283] y′ = Cz + ξ

[0284] where 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. Design a steady-state Kalman filtering algorithm based on the decorrelated state equation model obtained in step S3 for estimating the retracting and extending speed 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. Set E{w4} = 0, E{ξ} = 0, and construct the expression for the covariance matrix of the decorrelated process noise as:

[0295]

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

[0297]

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

[0299] Based on R ζ being positive definite, so Q w -Q w D2 T (D2Q w D2 T +R ζ ) -1 D2Q w is positive definite. Set the expression for the covariance matrix R of the decorrelated observation noise as:

[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)=F P(k - 1) F T +G Q G T

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

[0306] Calculate the filtering gain, and the expression is:

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

[0308] where K(k) is the Kalman filter gain matrix at the discretized time label k;

[0309] Obtained by recursive calculation:

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

[0311] where I is the identity matrix;

[0312] Then obtain the state equation of the decorrelated system after designing the Kalman filter algorithm, and update it to:

[0313]

[0314] Then perform one-step prediction to obtain the state equation of the decorrelated system after designing the Kalman filter algorithm:

[0315]

[0316] The steady-state solution of the state equation of the decorrelated system after designing the Kalman filter algorithm is obtained from the Riccati equation:

[0317] P = F P F T - F P C T (C P C T +R) -1 C P F T +G Q G T

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

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

[0320] Predict the state of the decorrelated system's state equation after applying the one-step prediction design Kalman filtering algorithm. The expression is as follows:

[0321]

[0322] Substitute the expression of B into it to obtain the steady-state Kalman filtering for one-step prediction as follows:

[0323]

[0324] A method for estimating the retracting and paying-out speed of the free end of a rope based on multi-sensor fusion described in this embodiment is shown in Figures 10 to 12 , which are respectively the simulation speed estimation and estimation error, the state estimation simulation diagram, and the accelerometer zero-bias estimation simulation diagram of Embodiment 2 of the multi-source information fusion speed estimation method of the present invention. It can be seen from Figures 10 to 12 that for the multi-source information fusion speed estimation results using relative position measurement sensors at the fixed end or the winch end, the speed estimation error values are between ±2.4, which is close to the accuracy of Embodiment 1. It can be seen that the present invention can achieve high-precision speed estimation for both the fixed end and the winch end using relative position measurement sensors and speed measuring elements. Through comprehensive steady-state Kalman filtering and multi-source data fusion, the estimation of the speed of the free end of the rope is realized, where the relative position measurement sensor or the speed measuring element is used at the winch end or the fixed end, and the accelerometer is used at the free end of the rope. For the method of processing position information and acceleration information through filtering and weighted averaging, the estimation error for a speed peak of 137 is ±20.5, while the simulation results of the present invention can reduce the speed estimation error to within ±2.4, and the convergence speed is faster.

[0325] It should be noted that relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variation thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including a..." does not exclude the existence of additional identical elements in the process, method, article or device including the said element.

[0326] Although the present application has been described above with reference to specific embodiments, various modifications thereof can be made and components thereof can be replaced with equivalents without departing from the scope of the present application. In particular, as long as there is no structural conflict, the features in the specific embodiments disclosed in the present application can be combined with each other in any way, and the reason for not exhaustively describing the situations of these combinations in this specification is only to save space and resources. Therefore, the present application is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A method for estimating the speed of a rope's free end based on multi-sensor fusion, characterized in that: The steps include: S1. Based on different motion states, an angle sensor or a speed sensor is installed at the hoisting end or the fixed end of the wire rope, and a speed sensor is installed at the free end of the wire rope to collect data under different motion states; S2. A kinematic model is constructed based on the hoist end or fixed end of the wire rope using an angle sensor, and a kinematic model is constructed based on the hoist end or fixed end of the wire rope using a speed sensor, and a state equation model of the system is obtained after integration, and then the state equation model of the system is discretized and process noise is introduced to obtain a state equation model of the system with process noise introduced; S3. Decorrelation of the process noise and the observation noise of the state equation model of the system with process noise obtained in step S2 is performed to obtain a decorrelated state equation model; S4. Based on the decorrelated state equation model obtained in step S3, a steady-state Kalman filter algorithm is designed for estimating the rope free end retraction and release speed based on multi-sensor fusion.

2. The method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 1, characterized in that: In step S1, the hoisting end or the fixed end is in rotational motion, and an encoder, a circular grating, a magnetic grating, an induction synchronizer, a rotary transformer or a tachometer motor is installed to measure the relative rotation angle and angular velocity of the stator and the rotor; The fixed end is linearly moving, and is equipped with a linear grating or a magnetic grating to measure the relative linear displacement of the stator and the mover; An accelerometer is mounted on the free end.

3. A method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 1 or 2, characterized in that: The specific implementation method of constructing the kinematic model based on the hoist end or the fixed end using the angle measuring sensor in step S2 is: The kinematic model of the free end is established as follows: Where, the free end position is x, the free end velocity is v, the accelerometer reading is a, and the table zero bias is θ. is the derivative of the free end position; The state motion model of the free end kinematic model is recorded as: in, is the derivative of the state variable, is the system matrix, is the input matrix, z1 is the state variable; The observation equation is established based on the hoisting end or the fixed end using an angle sensor: Where y1 is the rotation angle measured by the angle sensor at the installation position at the hoist end or the fixed end, c r is the transmission ratio, is the high-frequency measurement error, ζ is the zero-mean white noise of the observation equation; set up It 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. The expression is: Among them, ω a is the angular frequency of the driving link, s is the integral sign, and w1 is the zero-mean white noise of the driving link; By driving the following high-pass links, the minimum state space implementation is obtained:

4. The method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 3 is characterized in that: The specific implementation method of constructing the kinematic model based on the winch end or the fixed end using the speed sensor in step S2 is: The kinematic model of the free end is established as follows: Where, the free end position is x, the free end velocity is v, the accelerometer reading is a, and the table zero bias is θ; The observation equation is established based on the winch end or fixed end using a speed sensor: Wherein, y2 is the measurement of the angular velocity of rotation at the installation position using a speed sensor at the hoisting end or the fixed end.

5. The method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 4, characterized in that: The state equation model of the system obtained after synthesis in step S2 is: y=Cz+D2w1+ζ Among them, z is the expanded state vector, is the expanded system matrix, is the expanded input matrix, is the input matrix of white noise, C is the expanded measurement matrix, and D2 is the input coefficient corresponding to w1; C=[C1 -ω a ] D=[0D2] D2=ω a Among them, C1 is the observation matrix of the original system, and D is the feedforward matrix of the expanded system; Then the state equation model of the system obtained after synthesis is discretized to obtain the state equation model of the discretized system: z(k+1)=Az(k)+B1a(k)+B2w1(k) Where k is the discrete sampling time index, A is the system matrix after discretization, B1 is the input matrix after discretization, B2 is the discrete input matrix of white noise, and w1(k) is the zero-mean white noise of the driving link under discrete sampling time; Then, process noise is introduced into the state equation model of the discretized system, and we get: z(k+1)=Az(k)+B1a(k)+B2w1(k)+B3n(k) Among them, B3 is the input matrix of process noise, and η(k) is the process noise matrix under discrete sampling time.

6. The method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 5, characterized in that: The specific implementation method of step S3 includes the following steps: S3.

1. The covariance matrix expression of the noise is defined as follows; Among them, Q w ,Q2,R ζ are the covariance matrices of w1, η, ζ respectively, and E is the sign of mathematical expectation; set E{w1}=0, E{η}=0, E{ζ}=0; 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 state equation of the compensated discretized system: Then, based on the above equation, the state term and the noise term are merged separately, and the state equation expression of the system where the state term and the noise term are merged separately is obtained as follows: S3.

3. Based on the decorrelation of process noise and observation noise, the following objectives are set: After expansion, the expanded formula is: Then the covariance matrix of the noise defined in step S3.1 is substituted into the expansion to obtain Based on R ζ is a positive definite matrix, D2Q w D2 T +R ζ Reversible, the calculation result of K1 is: Among them, K 11 Redefine the internal parameters of K1; S3.

4. Substitute the calculation result of K1 obtained in step S3.3 into the state equation of the system in which the state term and noise term obtained in step S3.2 are combined, and the state equation of the decorrelated system is obtained, which is expressed as: y′=Cz+ξ Where F is the state transfer 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; B=[B1 B2K 11 ] G=[B2 B3] ξ=D2w1+ζ。 7. The method for estimating the speed of a rope free end based on multi-sensor fusion according to claim 6, characterized in that: The specific implementation method of step S4 includes the following steps: S4.

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

2. Based on the covariance matrix of the process noise after decorrelation constructed in step S4.1 and the covariance matrix R of the observation noise after decorrelation, a Kalman filtering algorithm is designed for the state equation of the decorrelation system: calculate The corresponding mean square error is: P(k|k-1)=FP(k-1)F T +GQG T in, 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 T represents the transposed matrix of the state transfer matrix, G T represents the transposed matrix of the system noise driving matrix, F represents the state transfer matrix, and G represents the system noise driving matrix; Calculate the filter gain, 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 discretized time index k; Recursive calculation yields: P(k)=(IK(k)C)P(k|k-1) Where I is the identity matrix; Then the state equation of the system after decorrelation after designing the Kalman filter algorithm is obtained and updated as: Then, a one-step prediction is performed to design the state equation of the system after decorrelation with the Kalman filter algorithm, and the result is: After designing the Kalman filter algorithm, the steady-state solution of the state equation of the decorrelated system is obtained by the Riccati equation: P=FPF T -FPC T (CPC T +R) -1 CPF T +GQG T The steady-state gain is given by: K=FPC T (CPC T +R) -1 The state equation of the system that is decorrelated after the one-step prediction design Kalman filter algorithm is predicted, and the expression is: Substituting the expression of B into the steady-state Kalman filter for one-step prediction, we get:

Images