CN110566716B - Kalman filter-based rail valve position measuring system and method - Google Patents
Kalman filter-based rail valve position measuring system and method Download PDFInfo
- Publication number
- CN110566716B CN110566716B CN201910798322.8A CN201910798322A CN110566716B CN 110566716 B CN110566716 B CN 110566716B CN 201910798322 A CN201910798322 A CN 201910798322A CN 110566716 B CN110566716 B CN 110566716B
- Authority
- CN
- China
- Prior art keywords
- gyroscope
- measurement
- valve
- hall sensor
- current
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
- F16K—VALVES; TAPS; COCKS; ACTUATING-FLOATS; DEVICES FOR VENTING OR AERATING
- F16K37/00—Special means in or on valves or other cut-off apparatus for indicating or recording operation thereof, or for enabling an alarm to be given
- F16K37/0075—For recording or indicating the functioning of a valve in combination with test equipment
- F16K37/0083—For recording or indicating the functioning of a valve in combination with test equipment by measuring valve parameters
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Abstract
The application discloses track valve position measurement system and method based on Kalman filter, include: the gyroscope and the Hall sensor are arranged on the track valve, and the controller is connected with the gyroscope and the Hall sensor through a data collector; the data acquisition unit acquires measurement data of a gyroscope and a Hall sensor, the controller constructs a Kalman filter, and controls the data acquisition unit to measure and update the measurement data of the gyroscope to obtain updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix. According to the invention, the problem of poor measurement accuracy of a single sensor is solved by fusing data of the Hall sensor and the gyroscope.
Description
Technical Field
The application relates to a valve position detection technology in the field of valves, in particular to a system and a method for measuring a valve position of a track valve based on a Kalman filter.
Background
The track valve is used as a ball valve with a single valve seat and bidirectional sealing, integrates the advantages of a gate valve, a ball valve, a stop valve and a plug valve, and is widely applied to gas pipelines. Orbit valves are typically configured with a feedback valve position feedback. The feedback device can realize remote feedback of the valve opening and closing state by matching with a field PLC (programmable logic circuit) system, and provides corresponding valve position signal output for a monitoring system for managing the valve. However, the integrated feedback device is limited by the structure of the existing track valve, is mainly used for observation of field operators, is limited in sampling precision when being directly combined with a PLC system, and causes adverse effects on the assembly and disassembly of auxiliary facilities on a pipeline due to the fact that cable laying between the field PLC system and a sensor is interfered by operation of a control hand wheel.
The non-contact sensor is adopted to collect the rotation amount of the valve, and the current universal sensors comprise a gyroscope, a Hall sensor and the like. The bidirectional Hall sensor can count ferromagnetic objects placed at intervals and can also distinguish increase and decrease directions. And a rack-shaped disc is arranged on a hand wheel of the track valve, so that the rotation amount of the valve can be acquired. However, during the rotation of the valve, it is difficult to avoid the vibration interference perpendicular to the rotation surface, which causes the counting error of the hall sensor. The gyroscope has various errors such as temperature drift, time drift, random interference and the like, so that the gyroscope is difficult to be independently applied to a rotation measurement system. Therefore, how to realize efficient and accurate valve position measurement of the orbit valve becomes an important technical problem facing currently.
Disclosure of Invention
The embodiment of the application provides a system and a method for measuring the valve position of a track valve based on a Kalman filter, and solves the technical problem of efficient and accurate measurement of the valve position of the track valve.
The technical scheme of this application provides a track valve position measurement system based on kalman filter, includes: the gyroscope and the Hall sensor are arranged on the track valve, and the controller is connected with the gyroscope and the Hall sensor through a data collector; wherein the content of the first and second substances,
the data acquisition unit acquires gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant value deviation and gyroscope measurement noise, and acquires Hall sensor measurement data comprising a Hall sensor valve corner and Hall sensor measurement noise; the controller determines the next valve corner estimated value of the gyroscope according to the gyroscope measurement data, and the specific mode is as follows:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next gyroscope valve angle estimated value, θ (k) is a current gyroscope valve angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant value deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement;
according to the valve corner of the Hall sensor and the measurement noise of the Hall sensor, the valve corner measurement value of the Hall sensor is determined, and the specific mode is as follows:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is a valve corner measurement value of the Hall sensor after error correction, theta (k) is a detected true valve corner of the Hall sensor, and v (k) is measurement noise of the Hall sensor;
the method comprises the following steps of taking a gyroscope valve corner as a first state vector, taking a gyroscope constant value deviation obtained by estimation of a valve corner measured value of a Hall sensor as a second state vector, and constructing a Kalman filter, wherein the Kalman filter is as follows:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is angular velocity of the gyroscope at the previous time, and X (k-1) is the rotation angle of the gyroscope at the previous time
Z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
The gyro constant deviation b (k) can be estimated by using the following formula when the valve angle measurement value z (k) of the hall sensor is θ (k + 1):
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
the controller controls the data acquisition unit to measure and update the gyroscope measurement data based on a Kalman filter to obtain updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
Based on the above-mentioned orbit valve position measurement system, the technical scheme of this application still provides an orbit valve position measurement method based on kalman filter, includes:
obtaining gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant deviation and gyroscope measurement noise according to detection calculation, and determining a next valve corner estimated value of the gyroscope; determining a valve corner measured value of the Hall sensor according to the Hall sensor valve corner obtained by detection and calculation and the Hall sensor measurement noise; constructing a Kalman filter by taking a gyroscope valve corner as a first state vector and taking a gyroscope constant deviation estimated by adopting a valve corner measured value of a Hall sensor as a second state vector;
measuring and updating gyroscope measurement data based on a Kalman filter to obtain an updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
The method for determining the next gyroscope valve rotation angle estimated value specifically comprises the following steps:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next gyroscope valve angle estimated value, θ (k) is a current gyroscope valve angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement.
The method for determining the valve rotation angle measurement value of the Hall sensor specifically comprises the following steps:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is the valve rotation angle measurement value of the Hall sensor after error correction, theta (k) is the detected true valve rotation angle of the Hall sensor, and v (k) is the measurement noise of the Hall sensor.
The Kalman filter is as follows:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is angular velocity of the gyroscope at the previous time, and X (k-1) is the rotation angle of the gyroscope at the previous time
Z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
The gyro constant deviation b (k) can be estimated by using the following formula when the valve angle measurement value z (k) of the hall sensor is θ (k + 1):
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
the embodiment of the application adopts at least one technical scheme which can achieve the following beneficial effects:
the invention uses a multi-sensor fusion filtering method to fuse data of a Hall sensor and a gyroscope, inhibits noise interference, solves the problem of poor measurement accuracy of a single sensor, and particularly reduces the influence of vibration interference perpendicular to a rotating surface on rotating angle calculation by establishing a sensor error model to compensate random drift error and using an adaptive measurement noise matrix. According to the technical scheme, the valve position measuring system with low cost and high precision is realized, and a high-efficiency and accurate valve position measuring method is developed.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the application and together with the description serve to explain the application and not to limit the application. In the drawings:
FIG. 1 is a schematic diagram of a Kalman filter based rail valve position measurement system according to the present application;
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the technical solutions of the present application will be described in detail and completely with reference to the following specific embodiments of the present application and the accompanying drawings. It should be apparent that the described embodiments are only some of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
The technical solutions provided by the embodiments of the present application are described in detail below with reference to the accompanying drawings.
The kalman filter algorithm is an "optimized autoregressive data processing algorithm (optimal regression algorithm)" which has been widely used for over 30 years, and the application fields include robot navigation, control, sensor data fusion, radar systems, missile tracking, and the like. More recently, computer image processing has become more useful, such as for example, head and face recognition, image segmentation, image edge detection, and so forth. The Kalman Filter (The Kalman Filter) introduces a system of discrete control processes. The system can be described by a Linear Stochastic differential equation (Linear Stochastic differential equation) plus the system's measurements:
the linear equation: x (k) ═ A X (k-1) + B U (k) + w (k)
Measurement values: z (k) ═ H X (k) + V (k)
In the above two formulas, the kalman filter assumes that the true state at time k is evolved from the state at time (k-1), x (k) is the system state at time k, and u (k) is the control quantity of the system at time k. A and B are system parameters, and for multi-model systems, they are matrices. Z (k) is the measured value at time k, H is a parameter of the measurement system, and H is a matrix for a multi-measurement system. W (k) and v (k) represent process and measurement noise, respectively. The operation of the kalman filter comprises two phases: and (4) predicting and updating. In the prediction phase, the filter uses the estimate of the last state to make an estimate of the current state. In the update phase, the filter optimizes the predicted value obtained in the prediction phase using the observed value for the current state to obtain a more accurate new estimated value.
Aiming at the problem that the valve position of the rail valve cannot be efficiently and accurately measured due to the fact that a single sensor is poor in measurement accuracy in the prior art, the technical scheme of the application acquires detection data of a gyroscope and a Hall sensor, a Kalman filter is constructed, and data fusion is performed based on the Kalman filter, so that efficient and accurate valve position measurement of the rail valve is achieved.
The technical scheme of this application provides a track valve position measurement system based on kalman filter, as shown in fig. 1, includes: the gyroscope and the Hall sensor are arranged on the track valve, and the controller is connected with the gyroscope and the Hall sensor through a data collector; wherein the content of the first and second substances,
the data acquisition unit acquires gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant value deviation and gyroscope measurement noise, and acquires Hall sensor measurement data comprising a Hall sensor valve corner and Hall sensor measurement noise; the controller determines the next valve corner estimated value of the gyroscope according to the gyroscope measurement data, and the specific mode is as follows:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next gyroscope valve angle estimated value, θ (k) is a current gyroscope valve angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant value deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement;
according to the valve corner of the Hall sensor and the measurement noise of the Hall sensor, the valve corner measurement value of the Hall sensor is determined, and the specific mode is as follows:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is a valve corner measurement value of the Hall sensor after error correction, theta (k) is a detected true valve corner of the Hall sensor, and v (k) is measurement noise of the Hall sensor;
the method comprises the following steps of taking a gyroscope valve corner as a first state vector, taking a gyroscope constant value deviation obtained by estimation of a valve corner measured value of a Hall sensor as a second state vector, and constructing a Kalman filter, wherein the Kalman filter is as follows:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is angular velocity of the gyroscope at the previous time, and X (k-1) is the rotation angle of the gyroscope at the previous time
Z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
The gyro constant deviation b (k) can be estimated by using the following formula when the valve angle measurement value z (k) of the hall sensor is θ (k + 1):
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
the controller controls the data acquisition unit to measure and update the gyroscope measurement data based on a Kalman filter to obtain updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
Based on the above system for measuring the valve position of the rail valve, a method for measuring the valve position of the rail valve based on a kalman filter can be provided, which comprises:
obtaining gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant deviation and gyroscope measurement noise according to detection calculation, and determining a next valve corner estimated value of the gyroscope; determining a valve corner measured value of the Hall sensor according to the Hall sensor valve corner obtained by detection and calculation and the Hall sensor measurement noise; constructing a Kalman filter by taking a gyroscope valve corner as a first state vector and taking a gyroscope constant deviation estimated by adopting a valve corner measured value of a Hall sensor as a second state vector;
measuring and updating gyroscope measurement data based on a Kalman filter to obtain an updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
The method for determining the next gyroscope valve rotation angle estimated value specifically comprises the following steps:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next gyroscope valve angle estimated value, θ (k) is a current gyroscope valve angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement.
The method for determining the valve rotation angle measurement value of the Hall sensor specifically comprises the following steps:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is the valve rotation angle measurement value of the Hall sensor after error correction, theta (k) is the detected true valve rotation angle of the Hall sensor, and v (k) is the measurement noise of the Hall sensor.
The Kalman filter is as follows:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is angular velocity of the gyroscope at the previous time, and X (k-1) is the rotation angle of the gyroscope at the previous time
Z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
The gyro constant deviation b (k) can be estimated by using the following formula when the valve angle measurement value z (k) of the hall sensor is θ (k + 1):
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
the valve position measuring system and the valve position measuring method can be suitable for resolving valve positions of various manual and electric valves and are also suitable for valves rotating for multiple circles. The track valve is a manual and parallel multi-circle rotary valve, a rack-shaped disc is fixedly arranged on a hand wheel of the track valve, and a Hall type rotary sensing sensor, an MEMS inertial sensing sensor and an MEMS gyroscope are adopted. Different sensors have different interference and error characteristics, so that firstly, an error model of the sensor and a resolving model of related physical quantity are determined, and data fusion is carried out on the sampled measured data of the gyroscope and the Hall sensor by utilizing Kalman filtering to obtain rotation angle information (valve position) of the orbit valve.
Example (b): valve position measuring scheme of rail valve based on Kalman filtering algorithm
Step 1: the data collector collects the output data of the gyroscope and the Hall sensor
The STM32 standard data acquisition unit can be adopted to automatically distinguish the valve limit rotation angle and the current angle, and no person is required to grant the limit angle. The STM32 data acquisition unit is used for rapidly acquiring output data of the three-axis gyroscope and the Hall sensor, and the sampling frequency is 1 kHZ; meanwhile, in a sampling interval, the rotation angle data is processed, and the limit rotation angle of the valve is updated, wherein the frequency is 100 HZ. Wherein the content of the first and second substances,
gyro output signal yg,tContains the true angular rate omegatZero offsetg,tAnd white gaussian noise vg,tAs shown in the following formula: y isg,t＝ωt+g,t+νg,t
Through the quaternion resolving process, the gyroscope has zero offsetg,t is the main error source causing the three-dimensional misalignment angle, and the zero bias of the gyroscope belongs to the low-frequency and slow-changing process, including
νg,t is zero-mean white gaussian noise.
Step 2: the gyroscope can directly measure the angular velocity of the valve rotation and obtain angle information through integral operation, but a gyroscope measurement model has corresponding drift errors, so that the error drift characteristic of the gyroscope in a zero input state is obtained through experiments by combining the measurement model and a rail valve position measurement system, and a gyroscope error mathematical model is established by combining nonlinear least square method fitting experimental data according to a data fitting model. The gyroscope may be a MEMS gyroscope. Wherein the error model is:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k) is the valve rotation angle, ω (k) is the gyroscope angular velocity, b (k) is the gyroscope constant deviation,
w (k) is the gyroscope measurement noise and dt is the measurement period.
And step 3: the Hall sensor can calculate the current angle according to the initial position and the counting value in the rotating process, but the counting error caused by the vibration interference perpendicular to the rotating surface to the Hall sensor is difficult to avoid. Therefore, according to the measurement model of the Kalman filter, a mathematical model for performing corner calculation by using the Hall sensor is established, wherein the corner calculation model is as follows:
z(k)＝θ(k)+v(k)
wherein z (k) is a valve rotation angle measured by the Hall sensor, theta (k) is a real valve rotation angle, and v (k) is measurement noise of the Hall sensor.
And 4, step 4: due to the performance and characteristics of the inertial sensor, a gyroscope element is independently used for calculating the rotation angle of the valve, so that a serious error problem exists, and therefore the Kalman filter is constructed by utilizing an MEMS gyroscope error model and a rail valve position measurement model. And carrying out data fusion on the measured data of the system rotation angle, inhibiting noise interference and compensating errors. Discrete mathematical model from system
Obtaining a system state matrix:
and (3) system measurement vector:
H＝[1 0]
wherein the state
U (k) is the output angular velocity of the gyroscope, Z (k) is the rotation angle measured by the Hall sensor, W (k) and V (k) are respectively the noise measured by the gyroscope and the noise measured by the Hall sensor, the statistical characteristics of the two are zero mean Gaussian white noise, and Q and R are respectively used for representing the noise variance matrixes of the two. And T is the system sampling period.
And 5: and performing data fusion on the valve position measurement data of the track valve by using a Kalman filtering algorithm. The method comprises the following steps:
step 5-1, sampling measurement data of the rotating speed and the rotating angle of the gyroscope and the Hall sensor:
measuring and updating gyroscope measurement data, and sampling rotation speed and rotation angle measurement data of the gyroscope and the Hall sensor to obtain updated one-step prediction state quantity and one-step prediction system covariance quantity;
step 5-2, the sampling data time updating process:
step 5-3, measurement updating process:
kalman gain:
K(k)＝P(k/k-1)HT[HP(k/k-1)HT+R(k)]-1
updated state estimation:
X(k)＝X(k/k-1)+K(k)(Z(k)-HX(k/k-1))
updated covariance estimation:
P(k)＝(I-K(k)H)P(k/k-1)(I-K(k)H)T+K(k)R(k)K(k)T
through the implementation process, an error model of the sensor and a resolving model of the related physical quantity are determined, and data fusion is performed on the sampled measured data of the gyroscope and the Hall sensor by using a Kalman filter, so that the rotation angle information (valve position) of the rail valve is finally obtained.
It is to be noted that the terms "comprises," "comprising," or any other variation 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 but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. The use of the phrase "including a" does not exclude the presence of other, identical elements in the process, method, article, or apparatus that comprises the same element, whether or not the same element is present in all of the same element.
The above description is only an example of the present application and is not intended to limit the present application. Various modifications and changes may occur to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.
Claims (10)
1. A Kalman filter based rail valve position measurement system, comprising: the gyroscope and the Hall sensor are arranged on the track valve, and the controller is connected with the gyroscope and the Hall sensor through a data collector; wherein the content of the first and second substances,
the data acquisition unit acquires gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant value deviation and gyroscope measurement noise, and acquires Hall sensor measurement data comprising a Hall sensor valve corner and Hall sensor measurement noise;
the controller determines a next valve corner predicted value of the gyroscope according to the gyroscope measurement data; determining a valve corner measured value of the Hall sensor according to the Hall sensor valve corner and the Hall sensor measuring noise; constructing a Kalman filter by taking a gyroscope valve corner as a first state vector and taking a gyroscope constant deviation estimated by adopting a valve corner measured value of a Hall sensor as a second state vector;
the controller controls the data acquisition unit to measure and update the gyroscope measurement data based on a Kalman filter to obtain updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
2. The Kalman filter based rail valve position measurement system of claim 1,
the method for determining the next valve rotation angle estimated value of the gyroscope specifically comprises the following steps:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next valve rotation angle estimated value of the gyroscope, θ (k) is a current gyroscope valve rotation angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant value deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement;
the method for determining the valve rotation angle measurement value of the Hall sensor specifically comprises the following steps:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is the valve rotation angle measurement value of the Hall sensor after error correction, theta (k) is the detected true valve rotation angle of the Hall sensor, and v (k) is the measurement noise of the Hall sensor.
3. The kalman filter-based orbit valve position measurement system according to claim 2, wherein the gyro constant deviation b (k) is estimated by the following formula when using the valve angle measurement z (k) θ (k +1) of the hall sensor:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
4. the kalman filter-based orbit valve position measurement system of claim 1 or 2, wherein the kalman filter is:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is the angular velocity of the gyroscope at the previous time, X (k-1) is the rotation angle of the gyroscope at the previous time,
z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
5. A method for measuring a valve position of an orbit valve based on a kalman filter, which is performed by the system of claim 1, and comprises:
obtaining gyroscope measurement data comprising a gyroscope valve corner, a gyroscope angular velocity, a gyroscope constant deviation and gyroscope measurement noise according to detection calculation, and determining a next valve corner estimated value of the gyroscope; determining a valve corner measured value of the Hall sensor according to the Hall sensor valve corner obtained by detection and calculation and the Hall sensor measurement noise; constructing a Kalman filter by taking a gyroscope valve corner as a first state vector and taking a gyroscope constant deviation estimated by adopting a valve corner measured value of a Hall sensor as a second state vector;
measuring and updating gyroscope measurement data based on a Kalman filter to obtain an updated one-step prediction first state vector and one-step prediction system covariance quantity; obtaining an updated gain matrix of the Kalman filter according to the prediction system covariance quantity; and further updating the current first state vector and the current system covariance amount according to the gain matrix.
6. The Kalman filter-based rail valve position measuring method according to claim 5, characterized in that the manner of determining the next gyroscope valve rotation angle estimated value is specifically as follows:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next valve rotation angle estimated value of the gyroscope, θ (k) is a current gyroscope valve rotation angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant value deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement.
7. The Kalman filter-based rail valve position measurement method according to claim 5, characterized in that the manner of determining the valve rotation angle measurement value of the Hall sensor is specifically as follows:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is the valve rotation angle measurement value of the Hall sensor after error correction, theta (k) is the detected true valve rotation angle of the Hall sensor, and v (k) is the measurement noise of the Hall sensor.
8. The Kalman filter based rail valve position measurement method of claim 5,
the method for determining the next valve rotation angle estimated value of the gyroscope specifically comprises the following steps:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
wherein θ (k +1) is a next valve rotation angle estimated value of the gyroscope, θ (k) is a current gyroscope valve rotation angle, ω (k) is a current gyroscope angular velocity, b (k) is a current gyroscope constant value deviation, w (k) is current gyroscope measurement noise, dt is a measurement period, k represents a current kth measurement, and k +1 represents a current kth measurement;
the method for determining the valve rotation angle measurement value of the Hall sensor specifically comprises the following steps:
z(k)＝θ(k)+v(k)
wherein k represents the current k-th measurement, z (k) is the valve rotation angle measurement value of the Hall sensor after error correction, theta (k) is the detected true valve rotation angle of the Hall sensor, and v (k) is the measurement noise of the Hall sensor.
9. The kalman filter-based orbit valve position measurement method according to claim 8, wherein the gyro constant deviation b (k) is obtained by estimating the gyro constant deviation b (k) when using the valve angle measurement value z (k) θ (k +1) of the hall sensor by the following formula:
θ(k+1)＝θ(k)+(ω(k)-b(k)+w(k))dt
z(k)＝θ(k)+v(k)。
10. the Kalman filter based rail valve position measurement method according to claim 5 or 8, characterized in that the Kalman filter is:
wherein the system state matrix is:
the system measurement vector is: h ═ 10
The state vector matrix is:
k denotes the kth measurement, k-1 denotes the kth measurement,
u (k-1) is the angular velocity of the gyroscope at the previous time, X (k-1) is the rotation angle of the gyroscope at the previous time,
z (k) is the rotation angle measured by the Hall sensor, W (k) is the noise measured by the gyroscope, V (k) is the noise measured by the Hall sensor, and T is the system sampling period.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910798322.8A CN110566716B (en) | 2019-08-27 | 2019-08-27 | Kalman filter-based rail valve position measuring system and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910798322.8A CN110566716B (en) | 2019-08-27 | 2019-08-27 | Kalman filter-based rail valve position measuring system and method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110566716A CN110566716A (en) | 2019-12-13 |
| CN110566716B true CN110566716B (en) | 2020-12-11 |
Family
ID=68776384
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910798322.8A Active CN110566716B (en) | 2019-08-27 | 2019-08-27 | Kalman filter-based rail valve position measuring system and method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110566716B (en) |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2826447B1 (en) * | 2001-06-26 | 2003-09-19 | Sagem | HYBRID INERTIAL NAVIGATION METHOD AND DEVICE |
| CN104477052B (en) * | 2014-12-05 | 2016-08-17 | 浙江大学 | A kind of control method of Self-balance manned electric unicycle |
| CN105136145A (en) * | 2015-08-11 | 2015-12-09 | 哈尔滨工业大学 | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method |
| CN105691532B (en) * | 2016-04-14 | 2018-04-24 | 四川大学 | A kind of Double-wheel self-balancing electric car with hand brake system |
| CN106006384A (en) * | 2016-08-03 | 2016-10-12 | 湖南百特随车起重机有限公司 | Lorry crane six-axis gyroscope automatic level detection and leveling system |
-
2019
- 2019-08-27 CN CN201910798322.8A patent/CN110566716B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110566716A (en) | 2019-12-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108225308B (en) | Quaternion-based attitude calculation method for extended Kalman filtering algorithm | |
| KR101988786B1 (en) | Initial alignment of inertial navigation devices | |
| CN101726295B (en) | Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation | |
| KR100886340B1 (en) | Apparatus and method for calibrating gyro-sensor of mobile robot | |
| US6498996B1 (en) | Vibration compensation for sensors | |
| Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
| CN104132662B (en) | Closed loop Kalman filtering inertial positioning method based on zero-speed correction | |
| US20100076639A1 (en) | System for sensing state and position of robot | |
| Anjum et al. | Sensor data fusion using unscented kalman filter for accurate localization of mobile robots | |
| CN106153069B (en) | Attitude rectification device and method in autonomous navigation system | |
| KR101115012B1 (en) | Apparatus and Method for Compenating Angular Velocity Error for Robot | |
| CN111721288A (en) | Zero offset correction method and device for MEMS device and storage medium | |
| CN113203429B (en) | Online estimation and compensation method for temperature drift error of gyroscope | |
| KR101685151B1 (en) | Calibration apparatus for gyro sensor | |
| CN110567481A (en) | object displacement monitoring method | |
| WO2016165336A1 (en) | Navigation method and terminal | |
| EP3058311A1 (en) | Adjusted navigation | |
| CN110566716B (en) | Kalman filter-based rail valve position measuring system and method | |
| Kwak et al. | Improvement of the inertial sensor-based localization for mobile robots using multiple estimation windows filter | |
| CN110864684A (en) | User posture measuring and calculating method | |
| CN115290080A (en) | Pipeline robot positioning method based on unbiased finite impulse response filter | |
| Choi et al. | Calibration of inertial measurement units using pendulum motion | |
| JP4070879B2 (en) | Electronic magnetic compass | |
| Parviainen et al. | Using Doppler radar and MEMS gyro to augment DGPS for land vehicle navigation | |
| JPH0914962A (en) | Instrument for measuring position of construction vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |