Parameter-adaptive dynamic tilt angle sensor measuring system

Abstract

The invention relates to a parameter self-adaptive dynamic tilt angle sensor measuring system, which is used for realizing accurate and reliable attitude detection under a long-time vibration working condition, and comprises a triaxial accelerometer used for detecting and outputting X-axis, Y-axis and Z-axis acceleration data; the gyroscope is used for detecting and outputting angular velocity data of X, Y and Z axes; and the attitude resolving module is used for performing EKF attitude fusion of parameter adaptive adjustment on the acceleration data and the angular velocity data to obtain attitude angle data. Compared with the prior art, the device has the advantages of being suitable for long-time vibration working conditions, reliable and accurate in detection, rapid in adjustment and the like.

Description

Technical Field

[0001] The invention relates to the field of inclination sensor measurement, and in particular to a parameter-adaptive dynamic inclination sensor measurement system. Background Art

[0002] Dynamic inclination sensors mostly work in environments with long-term vibration and need to be able to operate stably and reliably under such environmental conditions.

[0003] At present, the EKF algorithm is used to solve the gyroscope attitude, which has a certain vibration resistance. The acceleration generated by intense movement can easily affect the accuracy of the filter. The chi-square test can identify the instantaneous vibration when the acceleration is too large. The chi-square test threshold is used to control or eliminate the degree of accelerometer participation in measurement when the acceleration is too large, which can improve the impact of short-term motion acceleration on the calculation results. However, under the condition of long-term motion acceleration, the EKF algorithm with chi-square test will always limit the accelerometer to participate in the measurement, and the system will trust the gyroscope more. The time drift of the gyroscope will cause the angle to drift. The more intense the movement, the faster the drift, which greatly reduces the detection accuracy. Therefore, it is necessary to develop a dynamic tilt sensor system and measurement method that can adapt to the condition of long-term motion acceleration. Summary of the invention

[0004] In order to solve the technical problems in the background technology, the present invention provides a parameter-adaptive dynamic inclination sensor measurement system to solve the influence of continuous motion acceleration on the dynamic inclination sensor using the gyroscope EKF attitude fusion algorithm, so that the inclination sensor can output stable and reliable angle data under long-term vibration conditions.

[0005] The system is used to achieve accurate and reliable posture detection under long-term vibration conditions. The system includes:

[0006] Three-axis accelerometer: used to detect and output acceleration data of X, Y, and Z axes;

[0007] Gyroscope: used to detect and output angular velocity data of X, Y, and Z axes;

[0008] Attitude calculation module: It is used to perform EKF attitude fusion to adaptively adjust the acceleration data and angular velocity data to obtain attitude angle data.

[0009] Furthermore, the system also includes a compensation calibration module, which is respectively connected to the three-axis accelerometer, gyroscope and attitude solution module to achieve initial zero bias calibration, offset error calibration, sensitivity error calibration and temperature compensation, and sends the compensated calibrated data to the attitude solution module.

[0010] Furthermore, the compensation calibration module performs initial zero bias calibration, offset error calibration, sensitivity error calibration and temperature compensation on the three-axis accelerometer, and performs initial zero bias calibration on the gyroscope.

[0011] Furthermore, the compensation calibration module also performs temperature compensation on the gyroscope.

[0012] Furthermore, the temperature compensation is supplemented by table lookup combined with linear interpolation, specifically:

[0013]

[0014] Among them, M T∈(T[i],T[i+1]) M is the output value after difference compensation when the temperature is T. T is the output value before compensation, M 0 is the compensation value, M i+1 、M i are the corresponding compensation values ​​when the temperature is T[i+1] and T[i] respectively, and T[i+1] and T[i] are adjacent step temperature values.

[0015] Furthermore, the attitude solution module adopts the EKF attitude fusion algorithm to fuse the three-axis acceleration data and the angular velocity data, and automatically and dynamically adjusts the EKF parameters according to the chi-square test value, specifically including the following steps:

[0016] 1) Construct state equation and observation equation;

[0017] 2) Perform the Kalman filtering process, including the prediction phase and the update phase;

[0018] 3) In the prediction stage of Kalman filtering, when the chi-square test value exceeds the set threshold, the variance matrix Q of the process noise is adjusted by setting the dynamic adjustment constants Gain_Q and Gain_R respectively. k and the variance matrix R of the observation noise k , then:

[0019] Q k =Q k Gain_Q

[0020] R k =R k Gain_R

[0021] Furthermore, in the step 1), the state equation is expressed as:

[0022] x k+1 =f(x k )+ω k

[0023]

[0024] Among them, x k is the state vector, f(·) represents the nonlinear function, ω k is process noise, subscript k represents time k, q=[q 0 ,q 1 ,q 2 ,q 3 ] T is the attitude quaternion of the body coordinate system relative to the inertial coordinate system, and ω bias =[ω bias_x ,ω bias_y ,ω bias_z ] T is the zero bias of the gyroscope corresponding to the X-axis, Y-axis, and Z-axis;

[0025] The expression of the observation equation is:

[0026] z k =h(x k )+v k

[0027]

[0028] Among them, z k is the measurement vector, which is the three-axis acceleration data after temperature compensation and zero bias calibration, h(·) represents the nonlinear function, v k is the observation noise.

[0029] Furthermore, the value ranges of the dynamic adjustment constants Gain_Q and Gain_R are respectively:

[0030] 0<Gain_Q≤1

[0031] 1≤Gain_R<r k .

[0032] Furthermore, the step 3) specifically includes the following steps:

[0033] 31) Determine the chi-square test value r of k at the current moment k Whether it exceeds the set threshold r 0 If not, set the values ​​of the dynamic adjustment constants Gain_Q and Gain_R to 1. At this time, the process noise ω k The variance matrix Q k and observation noise v k The variance matrix R k Remain unchanged; if yes, proceed to step 32);

[0034] 32) Determine the chi-square test value rk Is the difference between the dynamic adjustment constant Gain_R less than the set step Gap_R? If not, the dynamic adjustment constant Gain_R is gradually increased according to the set step Gap_R, and the dynamic adjustment constant Gain_Q is gradually reduced according to the proportional coefficient K, and K < 1; if so, the value of the dynamic adjustment constant Gain_R is r k , and set the dynamic adjustment constant Gain_Q value to 1.

[0035] Furthermore, in step 3), the threshold r 0 The value is 1.

[0036] Compared with the prior art, the present invention has the following advantages:

[0037] The present invention does not directly use the chi-square test value for measurement update, but automatically and dynamically adjusts the EKF parameters according to the chi-square test value. It can dynamically adjust in an environment where motion acceleration exists for a long time, prevent data mutations during the start and stop process, make the fusion output angle stable and reliable, and effectively avoid the problems of inaccuracy and drift in the attitude solution results in a long-term vibration environment, and have real-time, stability and vibration resistance. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a structural principle block diagram of the tilt sensor measurement system of the present invention;

[0039] Figure 2 It is a principle block diagram of the posture fusion algorithm of the present invention;

[0040] Figure 3 Flowchart for dynamically adjusting EKF parameters through chi-square test values;

[0041] Figure 4 A flow chart of the steps performed for the compensation calibration module;

[0042] Figure 5 The figure is a flow chart of the steps for temperature compensation using the linear interpolation method. DETAILED DESCRIPTION

[0043] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0044] It should be noted that in this specification, similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, it does not need to be further defined and explained in the subsequent drawings.

[0045] In the description of this embodiment, it should be noted that the terms "upper", "lower", "inner", "bottom", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, or are the orientations or positional relationships in which the inventive product is usually placed when used. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0046] The terms “first”, “second”, etc. are only used for distinguishing descriptions and should not be understood as indicating or implying relative importance.

[0047] In the description of this embodiment, it is also necessary to explain that, unless otherwise clearly specified and limited, the terms "set", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in this embodiment can be understood according to specific circumstances.

[0048] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0049] Example

[0050] The present invention provides a parameter-adaptive dynamic tilt sensor measurement system, which can realize dynamic adjustment in an environment where motion acceleration exists continuously for a long time, so that the angle data output by the tilt sensor fusion algorithm is more stable and reliable.

[0051] like Figure 1 As shown, the system includes an attitude solution module, a compensation calibration module and a data communication module. The compensation calibration module is connected to the three-axis accelerometer to realize temperature compensation and zero bias calibration of the output data of the three-axis accelerometer, and sends the compensated and calibrated three-axis acceleration data to the attitude solution module. The attitude collector is connected to the gyroscope to collect the angular velocity data output by the gyroscope in real time and send it to the attitude solution module. The attitude solution module runs the EKF attitude fusion algorithm and automatically and dynamically adjusts the EKF parameters according to the chi-square test value, finally solves the attitude angle, and outputs the attitude angle data through the data communication module.

[0052] In addition, this example uses a six-axis MEMS gyroscope chip with a three-axis accelerometer and a gyroscope, which can simultaneously collect three-axis acceleration and angular velocity data. The six-axis MEMS gyroscope chip sends the collected three-axis acceleration data to the attitude solution module after temperature compensation and zero bias calibration. In addition, the six-axis MEMS gyroscope chip sends the collected three-axis angular velocity data to the attitude solution module through the attitude collector.

[0053] The attitude calculation module, compensation calibration module and data communication module are described in detail below.

[0054] (1) Attitude calculation module

[0055] like Figure 2 As shown in the figure, in this example, the attitude solution module uses the EKF attitude fusion algorithm to fuse the three-axis acceleration and angular velocity data output by the six-axis MEMS gyroscope, and automatically and dynamically adjusts the EKF parameters according to the chi-square test value. First, the state equation and observation equation are constructed separately, specifically:

[0056] Define the state vector x k :

[0057]

[0058] Where q=[q 0 ,q 1 ,q 2 ,q 3 ] T is the attitude quaternion of the body coordinate system (b system) relative to the inertial coordinate system (n system), which is obtained by converting the three-axis angular velocity data, and ω bias =[ω bias_x ,ω bias_y ,ω bias_z ] T is the zero bias of the gyroscope corresponding to the X-axis, Y-axis, and Z-axis. Since the Z-axis of the body coordinate system is perpendicular to the ground and upward, it is difficult to estimate its zero bias by gravity acceleration. Therefore, in the present invention, ω is always considered bias_z =0;

[0059] The constructed state equation is:

[0060] x k+1 =f(x k )+ω k

[0061] Among them, x k is the state vector, f(·) represents the nonlinear function, ω k is process noise, subscript k represents time k;

[0062] The constructed observation equation is:

[0063] z k =h(x k )+v k

[0064]

[0065] Among them, z k is the measurement vector, which is the acceleration data output by the three-axis accelerometer after temperature compensation and zero bias calibration. h(·) represents the nonlinear function, and v k is the observation noise;

[0066] Then the Kalman filter process is performed. The prediction stage of the Kalman filter process includes state estimation, calculation of error covariance matrix, calculation of estimation error, calculation of chi-square test value and dynamic adjustment of parameters, which are as follows:

[0067] State Estimation:

[0068]

[0069] in, are the estimated state vectors at time k and k-1 respectively;

[0070] Compute the error covariance matrix:

[0071]

[0072] in, is the error covariance matrix estimated at time k, F k is the Jacobian matrix of the nonlinear function f(·), is the Jacobian matrix estimated at time k;

[0073] Calculate the estimated error:

[0074]

[0075] Among them, e k is the estimation error;

[0076] Calculate the chi-square test value:

[0077]

[0078] Among them, r k is the chi-square test value, H k is the Jacobian matrix of the nonlinear function h(·).

[0079] In order to avoid the environmental impact of long-term continuous motion acceleration, the present invention uses the chi-square test value to dynamically adjust Q k and Rk The parameters are:

[0080] Q k =Q k Gain_Q

[0081] R k =R k Gain_R

[0082] Among them, Gain_Q and Gain_R are dynamic adjustment constants, Q k is the process noise ω k The variance matrix, R k is the observation noise v k The variance matrix of

[0083] In actual testing conditions, the chi-square test value r k The larger the value, the greater the environmental motion acceleration. At this time, the error of the accelerometer will become larger, so it is necessary to adjust Q by dynamically adjusting the constant. k and R k , making the system more trustful of the gyroscope's output, thereby reducing the impact of long-term vibration.

[0084] like Figure 3 As shown, in this example, when the chi-square test value r at the current k moment k Does not exceed the set threshold r 0 When (in this case, r 0 The value is 1), at this time, the values ​​of the dynamic adjustment constants Gain_Q and Gain_R are both set to 1. When the chi-square test value r at the current k moment k Exceeding the set threshold r 0 When Q is adjusted, the dynamic adjustment constants Gain_Q and Gain_R are set accordingly. k and R k , the specific adjustment methods are as follows:

[0085] When the chi-square test value r k The difference r with the dynamic adjustment constant Gain_R k -When Gain_R is greater than step distance Gap_R, R k The set step size Gap_R is gradually increased, that is, Gain_R+=Gap_R, and Gain_Q is gradually reduced according to the proportional coefficient K, that is, Gain_Q*=K. The proportional coefficient K can be set according to the actual situation, such as 0.98, 0.99, 0.998, 0.999, etc.;

[0086] When the chi-square test value r k The difference r with the dynamic adjustment constant Gain_R k- When Gain_R is less than the step distance Gap_R, directly set Gain_R = r k , and Gain_Q=1;

[0087] According to the above adjustment process, the chi-square test value r k It will gradually decrease until it is less than the threshold r 0 , and tends to be stable, while the dynamic adjustment constant Gain_R tends to be stable at 1.

[0088] In addition, when the environmental motion acceleration changes again, the present invention will adjust Q according to the above adjustment method. k and R k Adjustment is made, and the chi-square test value r k and the dynamic adjustment constant Gain_R will tend to the same new value, as the chi-square test value r k As it continues to increase, Gain_R will also increase at the same time, and the system will trust the gyroscope more. Then the chi-square test value r k It will become smaller, Gain_R will also become smaller, and the parameters will be called back.

[0089] The update phase of the Kalman filter process includes calculating the Kalman filter gain, state update, and error covariance update, so:

[0090] Calculate the Kalman filter gain:

[0091]

[0092] Among them, K k is the Kalman filter gain;

[0093] Status Update:

[0094]

[0095] Error covariance update:

[0096]

[0097] At this point, the system estimates the optimal attitude quaternion and gyroscope zero bias value based on the Kalman filter, and finally converts the optimal attitude quaternion into attitude angle output.

[0098] (2) Compensation calibration module

[0099] like Figure 4As shown, the compensation calibration content of the compensation calibration module in the present invention includes initial zero bias calibration, offset error calibration, sensitivity calibration and temperature compensation. First, the six-axis MEMS gyroscope chip is subjected to power-on initial zero bias calibration, then the three-axis accelerometer is subjected to offset error calibration and sensitivity error calibration, and finally the six-axis MEMS gyroscope chip is subjected to temperature compensation, so that the acceleration and angular velocity data input into the attitude solution module are accurate and reliable.

[0100] The details are as follows:

[0101] 1. Initial zero bias calibration

[0102] Since the three-axis accelerometer and gyroscope of the six-axis MEMS gyroscope chip have zero bias, if they are not calibrated, it will have an adverse effect on the attitude fusion result. The initial zero bias calibration of the present invention refers to collecting a set number of data and calculating the average value as the zero bias of the gyroscope and the three-axis accelerometer when power is turned on. In subsequent measurements, the collected data will be subtracted from the initial value.

[0103] 2) Offset error and sensitivity error calibration

[0104] Since the six-axis MEMS gyroscope chip is mechanical in nature, stress and factory calibration limitations will cause offset error and sensitivity error to the three-axis accelerometer. The superposition of the two will cause greater errors. Therefore, it is necessary to calibrate the offset error and sensitivity error of the three-axis accelerometer. The specific steps are as follows.

[0105] After considering the influence of offset error and sensitivity error, the output acceleration value A of the triaxial accelerometer is out for:

[0106] A out =A offset +Sensitity*A actual

[0107] Among them, A offset is the accelerometer offset error, Sensitity is the accelerometer sensitivity, A actual True acceleration value.

[0108] Accelerometer offset error A offset The calculation method of accelerometer sensitivity is as follows:

[0109] Take the accelerometer output value in the ±1g gravity field:

[0110] Under +1g conditions:

[0111] A out +1g=A offset+1*Sensitivity

[0112] At -1g conditions:

[0113] A out -1g=A offset -1*Sensitivity

[0114] The solution of the two equations is:

[0115] A offset =0.5(A out +1g+A out -1g)

[0116] A offset =0.5(A out +1g-A out -1g)

[0117] Using the accelerometer offset error A offset The two parameters of accelerometer sensitivity can be used to calibrate the offset error and sensitivity error of the three-axis accelerometer.

[0118] 3) Temperature compensation

[0119] According to the correspondence between the output data of the six-axis MEMS gyroscope chip and the temperature, the output data is temperature compensated (in actual production, only the acceleration data output by the three-axis accelerometer can be temperature compensated, or the acceleration data output by the three-axis accelerometer and the angular velocity data output by the gyroscope can be temperature compensated separately to improve the output accuracy of the gyroscope). First, the correspondence between the acceleration and the temperature and the correspondence between the angular velocity and the temperature are saved. After reading the acceleration and / or angular velocity output by the six-axis MEMS gyroscope chip each time, temperature compensation is performed according to the corresponding temperature curve data. In the present invention, a linear interpolation method is used for temperature compensation. The specific method is as follows: Figure 5 shown.

[0120] Several temperature points are selected within the range of -40℃~85℃, and the temperature value, acceleration and angular velocity value of the product at each temperature point when it is at 0° are recorded respectively to form corresponding acceleration-temperature table and angular velocity-temperature table. The output data of the six-axis MEMS gyroscope chip is temperature compensated by table lookup combined with linear interpolation.

[0121] The calculation formula for temperature linear compensation is:

[0122]

[0123] Among them, M T∈(T[i],T[i+1]) M is the output value after difference compensation when the temperature is T. Tis the output value before compensation, M 0 is the compensation value, M i+1 、M i are the corresponding compensation values ​​when the temperature is T[i+1] and T[i] respectively, and T[i+1] and T[i] are adjacent step temperature values.

[0124] At this point, the calibrated and compensated acceleration and angular velocity can be put into the attitude calculation module to calculate the attitude angle.

[0125] In summary, the present invention can automatically adjust the EKF parameters according to the magnitude of the motion acceleration, can prevent data mutations during the start and stop processes, and prevent the inaccuracy and drift of attitude solution results in a long-term vibration environment.

[0126] The present invention can automatically identify motion acceleration, and automatically debug and adjust EKF parameters according to motion acceleration, so as to realize the self-adaptation of EKF parameters to the vibration environment. It can not only resist short-term vibration, but also operate stably and reliably under long-term continuous vibration conditions, and has real-time performance, stability and vibration resistance. Although it has limitations in long-term and particularly severe vibration conditions, the angular velocity of the gyroscope is seriously affected (for example, under conditions where the angular velocity exceeds the range of the gyroscope), it still has wide applicability.

[0127] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A parameter-adaptive dynamic tilt sensor measurement system for realizing accurate and reliable posture detection under long-term vibration conditions, characterized in that: The system includes: Three-axis accelerometer: used to detect and output acceleration data of X, Y, and Z axes; Gyroscope: used to detect and output angular velocity data of X, Y, and Z axes; Attitude calculation module: It is used to perform EKF attitude fusion to adaptively adjust the acceleration data and angular velocity data to obtain attitude angle data.

2. A parameter-adaptive dynamic tilt sensor measurement system according to claim 1, characterized in that: The system also includes a compensation calibration module, which is respectively connected to the three-axis accelerometer, the gyroscope and the attitude solution module to achieve initial zero bias calibration, offset error calibration, sensitivity error calibration and temperature compensation, and sends the compensated calibrated data to the attitude solution module.

3. A parameter-adaptive dynamic tilt sensor measurement system according to claim 2, characterized in that: The compensation calibration module performs initial zero bias calibration, offset error calibration, sensitivity error calibration and temperature compensation on the three-axis accelerometer, and performs initial zero bias calibration on the gyroscope.

4. A parameter-adaptive dynamic tilt sensor measurement system according to claim 3, characterized in that: The compensation calibration module also performs temperature compensation on the gyroscope.

5. A parameter-adaptive dynamic tilt sensor measurement system according to claim 3 or 4, characterized in that: The temperature compensation is specifically supplemented by table lookup combined with linear interpolation, specifically: Among them, M T∈(T[i],T[i+1]) M is the output value after difference compensation when the temperature is T. T is the output value before compensation, M0 is the compensation value, M i+1 、M i are the corresponding compensation values ​​when the temperature is T[i+1] and T[i] respectively, and T[i+1] and T[i] are adjacent step temperature values.

6. A parameter-adaptive dynamic tilt sensor measurement system according to claim 1, characterized in that: The attitude calculation module adopts the EKF attitude fusion algorithm to fuse the three-axis acceleration data and angular velocity data, and automatically and dynamically adjusts the EKF parameters according to the chi-square test value, which specifically includes the following steps: 1) Construct state equation and observation equation; 2) Perform the Kalman filtering process, including the prediction phase and the update phase; 3) In the prediction stage of Kalman filtering, when the chi-square test value exceeds the set threshold, the variance matrix Q of the process noise is adjusted by setting the dynamic adjustment constants Gain_Q and Gain_R respectively. k and the variance matrix R of the observation noise k , then: Q k =Q k Gain_Q R k =R k Gain_R。 7. A parameter-adaptive dynamic tilt sensor measurement system according to claim 6, characterized in that: In the step 1), the state equation is expressed as: x k+1 =f(x k )+ω k Among them, x k is the state vector, f(·) represents the nonlinear function, ω k is process noise, subscript k represents time k, q=[q0,q1,q2,q3] T is the attitude quaternion of the body coordinate system relative to the inertial coordinate system, and ω bias =[ω bias_x ,ω bias_y ,ω bias_z ] T is the zero bias of the gyroscope corresponding to the X-axis, Y-axis, and Z-axis; The expression of the observation equation is: z k =h(x k )+v k Among them, z k is the measurement vector, which is the three-axis acceleration data after temperature compensation and zero bias calibration, h(·) represents the nonlinear function, v k is the observation noise.

8. The parameter-adaptive dynamic tilt sensor measurement system according to claim 6, characterized in that: The value ranges of the dynamic adjustment constants Gain_Q and Gain_R are respectively: 0<Gain_Q≤1 1≤Gain_R<r k 。 9. A parameter-adaptive dynamic tilt sensor measurement system according to claim 8, characterized in that: The step 3) specifically comprises the following steps: 31) Determine the chi-square test value r of k at the current moment k Does it exceed the set threshold r0? If not, set the values ​​of the dynamic adjustment constants Gain_Q and Gain_R to 1. At this time, the process noise ω k The variance matrix Q k and observation noise v k The variance matrix R k Remain unchanged; if yes, proceed to step 32); 32) Determine the chi-square test value r k Is the difference between the dynamic adjustment constant Gain_R less than the set step Gap_R? If not, the dynamic adjustment constant Gain_R is gradually increased according to the set step Gap_R, and the dynamic adjustment constant Gain_Q is gradually reduced according to the proportional coefficient K, and K < 1; if so, the value of the dynamic adjustment constant Gain_R is r k , and set the dynamic adjustment constant Gain_Q value to 1.

10. The parameter-adaptive dynamic tilt sensor measurement system according to claim 9, characterized in that: In the step 3), the threshold r0 is set to 1.

Images