Inclinometer with integrity monitoring function and integrity monitoring method thereof

Abstract

The invention provides an inclinometer with an integrity monitoring function and an integrity monitoring method thereof. The inclinometer comprises: an inertial measurement unit, which is used for measuring to obtain inertial measurement data, the inertial measurement data comprising angular velocity data and acceleration data; the inclination angle resolving unit based on extended Kalman filtering is used for resolving inclination angle information according to the inertial measurement data and outputting the inclination angle information; the integrity monitoring data resolving unit is used for calculating a test statistic savg according to the innovation of the Kalman filter provided by the inclination angle resolving unit and the variance of the innovation, when the test statistic savg is smaller than a preset threshold value Td, it is considered that the inclination angle information currently output by the inclination angle resolving unit is credible, and when the test statistic savg is smaller than the preset threshold value Td, the integrity monitoring data resolving unit is used for determining that the inclination angle information currently output by the inclination angle resolving unit is credible. And when the test statistic savg is greater than a preset threshold Td, considering that the inclination angle information currently output by the inclination angle resolving unit is not credible. Therefore, not only the inclination angle information can be obtained, but also the integrity detection information of the inclination angle information can be obtained.

Description

[Technical Field]

[0001] This invention relates to the field of inclinometer technology, and more specifically to an inclinometer with integrity monitoring function and a method for monitoring its integrity. [Background Technology]

[0002] Inclinometers are fundamental sensors for measuring the roll and pitch angles of a vehicle, widely used in engineering machinery, robotics, drones, and other fields. Traditional inclinometers typically employ an extended Kalman filter (EKF) scheme that combines an accelerometer and a gyroscope for angle measurement. This scheme can be affected by linear acceleration and vibration, leading to rapidly changing step errors, and can also be affected by gyroscope bias estimation errors, resulting in slowly changing angle integration accumulation errors. Traditional inclinometer calculation methods focus primarily on reducing these errors, but rarely provide effective system integrity monitoring information, i.e., information on the reliability of the system's output angle results under complex operating conditions.

[0003] Therefore, it is necessary to provide an integrity monitoring method suitable for inclinometer applications to overcome the above problems. [Summary of the Invention]

[0004] One of the objectives of this invention is to provide an inclinometer with integrity monitoring function and its integrity monitoring method, which can self-assess the reliability of the output inclinometer information.

[0005] According to one aspect of the present invention, an inclinometer with integrity monitoring function is provided, comprising: an inertial measurement unit for measuring inertial measurement data, the inertial measurement data including angular velocity data and acceleration data; an inclinometer based on extended Kalman filtering for calculating and outputting inclinometer information based on the inertial measurement data; and an integrity monitoring data calculation unit for calculating a test statistic s based on the innovation of the Kalman filter provided by the inclinometer calculation unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle is not found, then the tilt angle information currently output by the tilt angle calculation unit is considered unreliable.

[0006] According to another aspect of the present invention, a method for monitoring the integrity of an inclinometer is provided, comprising: measuring inertial measurement data, wherein the inertial measurement data includes angular velocity data and acceleration data; preprocessing the inertial measurement data; an inclinometer calculation unit based on an extended Kalman filter calculating inclinometer information based on the inertial measurement data and outputting it; and calculating a test statistic s based on the innovation of the Kalman filter provided by the inclinometer calculation unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle is not found, then the tilt angle information currently output by the tilt angle calculation unit is considered unreliable.

[0007] Compared with the prior art, the integrity monitoring data processing unit in this invention calculates the test statistic s based on the innovation of the Kalman filter provided by the tilt angle processing unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle calculation unit outputs tilt angle information, it is considered unreliable, thus obtaining the reliability information of the tilt angle information. [Attached Image Description]

[0008] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein:

[0009] Figure 1 This is a schematic diagram of the inclinometer with integrity monitoring function in this invention;

[0010] Figure 2 This is a schematic flowchart of the inclinometer integrity monitoring method in this invention.

Detailed Implementation Methods

[0011] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0012] The term "an embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the invention. The phrase "in one embodiment" appearing in different places throughout this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0013] This invention provides an inclinometer with integrity monitoring function, which can self-evaluate the reliability of the output inclinometer information.

[0014] Figure 1 This is a schematic diagram of the inclinometer with integrity monitoring function in this invention. Figure 1 As shown, the inclinometer includes: an inertial measurement unit (IMU) 110, a data preprocessing unit 120, an inclinometer calculation unit 130 based on extended Kalman filtering, and an integrity monitoring data calculation unit 140.

[0015] The inertial measurement unit 110 is used to measure inertial measurement data, which includes angular velocity data and acceleration data. In one embodiment, the inertial measurement unit includes a gyroscope and an accelerometer.

[0016] Depending on the application scenario, the data preprocessing unit 120 can selectively preprocess the inertial measurement data. The preprocessing includes noise reduction, such as moving average or other low-pass filtering. In some application scenarios, the data preprocessing unit 120 may not be used to preprocess the inertial measurement data; in this case, the inertial measurement data output by the inertial measurement unit 110 is directly transmitted to the tilt angle calculation unit 130 based on extended Kalman filtering.

[0017] The tilt angle calculation unit 130 based on the extended Kalman filter calculates and outputs the tilt angle information according to the inertial measurement data. The extended Kalman filter technology is relatively mature, and its basic process will not be described in detail here.

[0018] After receiving the noise-reduced inertial measurement data, the tilt angle calculation unit 130 performs relevant calculation operations. Its main functions include: outputting the calculated tilt angle information to realize the basic function of the inclinometer; providing relevant data for the integrity monitoring data calculation unit, and the relevant formulas and details are given below; receiving integrity monitoring information and dynamically adjusting the Kalman filter parameters and related algorithm logic according to the integrity monitoring information to further improve the accuracy of the output tilt angle information.

[0019] The integrity monitoring data processing unit 140 is used to calculate the test statistic s based on the Kalman filter innovation provided by the tilt angle processing unit 130 and the variance of the innovation. avgWhen the test statistic s avg Less than the preset threshold T d If the system is considered intact and fault-free, the tilt angle information currently output by the tilt angle calculation unit is considered reliable. When the test statistic s... avg Greater than the preset threshold T d If the system malfunctions, the tilt angle information currently output by the tilt angle calculation unit is considered unreliable. The integrity monitoring data calculation unit outputs integrity detection information, which includes the test statistic s. avg And whether the currently output tilt angle information is reliable. Depending on the design requirements, the test statistic s avg Equal to the preset threshold T d In either case, it can be considered that the system has malfunctioned and the tilt angle information currently output by the tilt angle calculation unit is unreliable, or it can be considered that the system is intact and no malfunction has occurred, and the tilt angle information currently output by the tilt angle calculation unit is reliable.

[0020] In one embodiment, the test statistic s avg for:

[0021]

[0022] In the formula:

[0023]

[0024] Where r k-i For the Kalman filter's innovation at time ki, V k-i Let be the variance of the information at time ki, and m be a natural number greater than or equal to 2;

[0025] The information from the Kalman filter at time ki is:

[0026]

[0027] The variance of the information at time ki is:

[0028]

[0029] In one embodiment, the integrity monitoring data processing unit 140 uses the AIME algorithm (Autonomous Integrity Monitoring Extrapolation) for integrity monitoring. In the AIME algorithm, measurement information is not limited to a single epoch, and the test statistic is obtained based on the innovation updated by the Kalman filter. Wherein, the innovation r at time k... k for:

[0030]

[0031] Here, H is the system state vector, specifically the inclinometer attitude and gyroscope bias, and H is the measurement matrix that maps the system state vector to attitude angles. Z is the attitude angle recursively derived from the current epoch gyroscope. k These are the measured values ​​of roll and pitch angles calculated by the accelerometer;

[0032] r k The statistical characteristics, such as the significant changes in the mean and covariance of equation (2), are key features indicating system failure or model mismatch.

[0033]

[0034] E[r k ] for r k The mean, For r k The covariance matrix.

[0035] The new information at time k k Variance V k for:

[0036]

[0037] In the formula, P k / k-1 R is the mean square error of the one-step prediction of the state vector. k This represents the measurement variance used to calculate the attitude angle from the accelerometer.

[0038] The integrity monitoring data processing unit 140 calculates the test statistic s using the AIME method. avg At time km, assuming the system is fault-free, the tilt angle information currently output by the tilt angle calculation unit is considered reliable, based on the state vector at time km. and state variance matrix Recursively derive the state vector at time k and state error covariance matrix The recurrence relation is:

[0039]

[0040] In the formula, Φ k,k-1 Let Γ be the state transition matrix. k-1 Assign a noise matrix to the system, Q k-1 Let be the system noise variance matrix.

[0041] The inclinometer also includes a communication unit 150 for receiving inclinometer information and integrity detection information.

[0042] In this way, in the field of low-cost dynamic inclinometers / MEMS sensors, it is also possible to obtain both inclinometer information and integrity detection information simultaneously.

[0043] Figure 2 This is a schematic flowchart of the inclinometer integrity monitoring method of the present invention. Figure 2 As shown, the method for monitoring the integrity of the inclinometer includes the following steps:

[0044] Step 210: Measure inertial measurement data, wherein the inertial measurement data includes angular velocity data and acceleration data.

[0045] Step 220: Optionally, the inertial measurement data may be preprocessed. The preprocessing includes noise reduction, such as moving average or other low-pass filtering. In some applications, preprocessing of the inertial measurement data may be omitted.

[0046] Step 230: The tilt angle calculation unit based on the extended Kalman filter calculates the tilt angle information based on the inertial measurement data and outputs it.

[0047] Step 240: Calculate the test statistic s based on the innovation of the Kalman filter provided by the tilt angle calculation unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle is not found, the tilt angle information currently output by the tilt angle calculation unit is considered unreliable. The integrity monitoring data calculation unit outputs integrity detection information, which includes the test statistic s. avg And whether the currently output tilt angle information is reliable.

[0048] In one embodiment, the test statistic s avg for:

[0049]

[0050] In the formula:

[0051]

[0052] Where r k-i For the Kalman filter's innovation at time ki, V k-i Let be the variance of the information at time ki, and m be a natural number greater than or equal to 2;

[0053] The information from the Kalman filter at time ki is:

[0054]

[0055] The variance of the information at time ki is:

[0056]

[0057] Where the new information r at time k k for:

[0058]

[0059] Here, H is the system state vector, specifically the inclinometer attitude and gyroscope bias, and H is the measurement matrix that maps the system state vector to attitude angles. Z is the attitude angle recursively derived from the current epoch gyroscope. k The inertial measurement unit includes a gyroscope and an accelerometer, which are the measured values ​​of roll and pitch angles calculated by the accelerometer.

[0060] The new information at time k k The variance is:

[0061]

[0062] In the formula, P k / k-1 R is the mean square error of the one-step prediction of the state vector. k This represents the measurement variance used to calculate the attitude angle from the accelerometer.

[0063] The integrity monitoring data processing unit calculates the test statistic s. avg At time km, assuming the system is fault-free, the tilt angle information currently output by the tilt angle calculation unit is considered reliable, based on the state vector at time km. and state variance matrix Recursively derive the state vector at time k and state error covariance matrix The recurrence relation is:

[0064]

[0065] In the formula, Φ k,k-1 Let Γ be the state transition matrix. k-1 Assign a noise matrix to the system, Q k-1 Let be the system noise variance matrix.

[0066] When the system is operating under complex conditions, it can self-assess the reliability of the output tilt angle information.

[0067] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. In addition, those skilled in the art can combine and integrate the different embodiments or examples described in this specification.

[0068] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications and variations to the above embodiments within the scope of the present invention.

Claims

1. An inclinometer with integrity monitoring function, characterized in that, It includes: An inertial measurement unit (IMU) is used to measure and obtain inertial measurement data, which includes angular velocity data and acceleration data. The tilt angle calculation unit based on the extended Kalman filter calculates and outputs the tilt angle information based on the inertial measurement data; The integrity monitoring data processing unit is used to calculate the test statistic s based on the innovation of the Kalman filter provided by the tilt angle processing unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle is not found, then the tilt angle information currently output by the tilt angle calculation unit is considered unreliable.

2. The inclinometer with integrity monitoring function according to claim 1, characterized in that, The test statistic s avg for: In the formula: Where r k-i For the Kalman filter's innovation at time ki, V k-i Let be the variance of the information at time ki, and m be a natural number greater than or equal to 2; The information from the Kalman filter at time ki is: The variance of the information at time ki is: Where the new information r at time k k for: Here, H is the system state vector, specifically the inclinometer attitude and gyroscope bias, and H is the measurement matrix that maps the system state vector to attitude angles. Z is the attitude angle recursively derived from the current epoch gyroscope. k The inertial measurement unit includes a gyroscope and an accelerometer, which are the measured values ​​of roll and pitch angles calculated by the accelerometer. The new information at time k k The variance is: In the formula, P k / k-1 R is the mean square error of the one-step prediction of the state vector. k This represents the measurement variance used to calculate the attitude angle from the accelerometer.

3. The inclinometer with integrity monitoring function according to claim 2, characterized in that, The integrity monitoring data processing unit calculates the test statistic s. avg At time km, assuming the system is fault-free, the tilt angle information currently output by the tilt angle calculation unit is considered reliable, based on the state vector at time km. and state variance matrix Recursively derive the state vector at time k and state error covariance matrix The recurrence relation is: In the formula, Φ k,k-1 Let Γ be the state transition matrix. k-1 Assign a noise matrix to the system, Q k-1 Let be the system noise variance matrix.

4. The inclinometer with integrity monitoring function according to claim 1, characterized in that, It also includes a data preprocessing unit, which is used to preprocess the inertial measurement data. The integrity monitoring data processing unit outputs integrity detection information, which includes the test statistic s. avg And whether the currently output tilt angle information is reliable.

5. The inclinometer with integrity monitoring function according to claim 4, characterized in that, The preprocessing includes noise reduction.

6. A method for monitoring the integrity of an inclinometer, characterized in that, It includes: Inertial measurement data is obtained by measurement, wherein the inertial measurement data includes angular velocity data and acceleration data; The tilt angle calculation unit based on the extended Kalman filter calculates the tilt angle information based on the inertial measurement data and outputs it. The test statistic s is calculated based on the innovation of the Kalman filter provided by the tilt angle calculation unit and the variance of the innovation. avg When the test statistic s avg Less than the preset threshold T d When the tilt angle information currently output by the tilt angle calculation unit is considered reliable, and the test statistic s is... avg Greater than the preset threshold T d If the tilt angle is not found, then the tilt angle information currently output by the tilt angle calculation unit is considered unreliable.

7. The method for monitoring the integrity of an inclinometer according to claim 6, characterized in that, The test statistic s avg for: In the formula: Where r k-i For the Kalman filter's innovation at time ki, V k-i Let be the variance of the information at time ki, and m be a natural number greater than or equal to 2; The information from the Kalman filter at time ki is: The variance of the information at time ki is: Where the new information r at time k k for: Here, H is the system state vector, specifically the inclinometer attitude and gyroscope bias, and H is the measurement matrix that maps the system state vector to attitude angles. Z is the attitude angle recursively derived from the current epoch gyroscope. k The inertial measurement unit includes a gyroscope and an accelerometer, which are the measured values ​​of roll and pitch angles calculated by the accelerometer. The new information at time k k The variance is: In the formula, P k / k-1 R is the mean square error of the one-step prediction of the state vector. k This represents the measurement variance used to calculate the attitude angle from the accelerometer.

8. The method for monitoring the integrity of an inclinometer according to claim 7, characterized in that, The integrity monitoring data processing unit calculates the test statistic s. avg At time km, assuming the system is fault-free, the tilt angle information currently output by the tilt angle calculation unit is considered reliable, based on the state vector at time km. and state variance matrix Recursively derive the state vector at time k and state error covariance matrix The recurrence relation is: In the formula, Φ k,k-1 Let Γ be the state transition matrix. k-1 Assign a noise matrix to the system, Q k-1 Let be the system noise variance matrix.

9. The method for monitoring the integrity of an inclinometer according to claim 6, characterized in that, It also includes: Used for preprocessing the inertial measurement data. The integrity monitoring data processing unit outputs integrity detection information, which includes the test statistic s. avg And whether the currently output tilt angle information is reliable.

10. The method for monitoring the integrity of an inclinometer according to claim 9, characterized in that, The preprocessing includes noise reduction.

Images