Self-adaptive horizontal attitude measurement method based on motion state monitoring

The disclosure relates to a self-adaptive horizontal attitude measurement method based on motion state monitoring, and belongs to the technical field of inertia.

With the development of micro electro mechanical system technology, a low-cost micro electro mechanical system inertial measurement unit (MEMS-IMU) is more frequently applied in the field of navigation. By measuring motion parameters by using inertial sensors based on a micro electro mechanical system, a complex motion state of a vessel in the sea can be detected, and users can acquire motion data of a surface vessel. The MEMS-IMU is required to accurately output angular motion parameters and linear motion parameters of a carrier in real time.

Micro electro mechanical gyroscopes have random drift characteristics, and integral errors thereof are accumulated over time, while accelerometers do not have accumulated errors but are susceptible to carrier vibration. Common algorithms for fusing data of the gyroscopes and the accelerometers are Kalman filtering and complementary filtering. For example, in the patent application No. 201811070907.X, entitled “Horizontal Attitude Self-Correction Method for MEMS Inertial Navigation System based on Maneuvering State Judgment”, a carrier motion is divided into low, medium, and high dynamics by comparing an accelerometer output and a local gravity acceleration amplitude. In the low and medium dynamics, a measurement noise matrix is adjusted in real time. In the high dynamics, only time is updated. However, if the carrier is in the high dynamics for a long time, an attitude error will be increasing. For another example, in the patent application No. 202011092956.0, entitled “MARG Attitude Estimation Method for Small Unmanned Aerial Vehicle based on Self-Adaptive EKF Algorithm”, an adaptive filtering algorithm for an external acceleration analyzes residual errors of three axes and then self-adaptively adjusts corresponding measurement noises, thereby avoiding losing useful acceleration information and improving the precision of attitude estimation. According to most of the traditional methods of self-adaptive adjustment, by comparing a modulus output by an accelerometer of a carrier with a local gravity acceleration, a maneuvering state of the carrier can be judged without considering the interference of an angular motion of the carrier on acceleration measurement. It may be insufficient for some complex environments.

In view of the prior art, the technical problem to be solved by the disclosure is to provide a self-adaptive horizontal attitude measurement method based on motion state monitoring, which can improve the measurement precision of a horizontal attitude of a carrier in a maneuvering scene and provide accurate horizontal attitude information for the carrier.

In order to solve the above technical problem, a self-adaptive horizontal attitude measurement method based on motion state monitoring of the disclosure includes the following steps:

step 1: initially aligning a strapdown inertial navigation system, completing the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles, including a roll angle 0 and a pitch angle ϕ 0 , and then entering a navigation working mode; step 2: initializing a Kalman filter, and taking the initial horizontal attitude angles 0 and ϕ 0 obtained in step 1 as initial values of a Kalman filtering state quantity X 0 =[ 0 ϕ 0 ] T , an initial mean square error being P 0 ; step 3: sampling MEMS-IMU data at a k th time, and compensating a random constant zero offset thereof to obtain a compensated specific force f k b and angular velocity ω k b ; step 4: performing a Kalman filtering one-step prediction by using an angular velocity increment Δω k b at the k th time as a known deterministic input u k-1 , where Δω k b =ω k b ·T, T being a calculation period; step 5: judging a maneuvering state of a carrier by using the specific force f b and the angular velocity ω b from time k−N+1 to time k obtained in step 3, and self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R k , where N is the size of a data window; step 6: when the specific force is f k b =[f x,k f y,k f z,k ] T at the k th time, selecting a measurement vector Z k =[f x,k f y,k ] T to perform measurement update so as to realize the correction of the state quantity, where f x,k , f y,k , and f z,k are components of the specific force f k b n x-axis, y-axis, and z-axis directions of a carrier system respectively; and step 7: taking an estimated value of the state quantity at the k th time as an initial value of a state quantity at a next time, and repeatedly performing steps 3 to 6 until a navigation working state ends.

The method of the disclosure further includes the following operations:

1. The Kalman filtering one-step prediction in step 4 specifically is:

2. In step 5, the judging a maneuvering state of a carrier by using the specific force f b and the angular velocity ω b obtained in step 3 specifically is: calculating the maneuvering state of the carrier by using the specific force f b and the angular velocity ω b from time k−N+1 to time k obtained in step 3, and acquiring a maneuvering vector T k to realize maneuvering judgment, T k satisfying:

T k = ( 1 σ f 2 ⁢  f k b - g ⁢ f _ k  f _ k   + 1 σ ω 2 ⁢  ω _ k  ) where ⁢ ω _ k = 1 N ⁢ ∑ i = k - N + 1 k ω i b ⁢ and ⁢ f _ k = 1 N ⁢ ∑ i = k - N + 1 k f i b

are mean values obtained by performing moving average on the specific force f b and the angular velocity ω b , which are output by an inertial measurement unit and used for compensating the random constant zero offset, from time k−N+1 to time k in step 3, respectively, the size of a data window being N, g being a local gravitational acceleration, and σ f and σ ω being weighting coefficients respectively.

3. The self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R k in step 5 specifically is:

4. An update equation of the measurement update in step 6 is:

{ K k = P k / k - 1 ⁢ H k T ( H k ⁢ P k / k - 1 ⁢ H k T + R k ) - 1 X ^ k = X ^ k / k - 1 + K k ( Z k - H k ⁢ X ^ k / k - 1 ) P k = ( I - K k ⁢ H k ) ⁢ P k / k - 1 ⁢ where ⁢ H k = [ - g · cos k 0 - g · sin ⁢ ϕ k ⁢ cos k g · cos ⁢ ϕ k ⁢ cos k ]

is a measurement matrix at time k, g is a local gravity acceleration, ϕ k and k are one-step prediction values of a horizontal attitude at time k, K k is a filtering gain at time k, {circumflex over (X)} k is a state estimation at time k, and P k is a state estimation mean square error matrix at time k.

The disclosure has the following beneficial effects. The disclosure relates to an attitude measurement unit using an MEMS-IMU as a core device. In the disclosure, a new state space model is established, a horizontal attitude angle is taken as a state variable, an angular velocity increment Δω b output by the MEMS-IMU is taken as a control input of a system equation, and a specific force f b output by the MEMS-IMU is taken as a measurement quantity. Meanwhile, maneuvering judgment conditions for a carrier are improved, and maneuvering information of the carrier is judged by comprehensively utilizing acceleration information and angular velocity information output by the MEMS-IMU, whereby a measurement noise matrix of a filter can be automatically adjusted, thereby effectively reducing the influence of carrier maneuvering on horizontal attitude information. The method has no special requirement on the maneuvering state of the carrier, can ensure that the system has high attitude measurement precision in different motion states without an external information assistance, and has a certain engineering application value.

FIG. 1 is an implementation flowchart of the disclosure.

FIG. 2 is a flowchart of an implementation algorithm of the disclosure.

FIG. 3 is an algorithm calculation error according to the disclosure.

The disclosure will now be further described with reference to the accompanying drawings and specific embodiments.

The disclosure is implemented as follows:

An inertial measurement element of a strapdown inertial navigation system is fully pre-heated, and the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles are completed. Then a navigation working state may be entered. A horizontal attitude angle is taken as a state variable, an angular velocity increment Δω b output by an MEMS-IMU is taken as a control input of a system equation, and a specific force f b output by the MEMS-IMU is taken as a measurement quantity. A Kalman filtering equation is established, and maneuvering information of the carrier is judged by comprehensively utilizing acceleration information and angular velocity information output by the MEMS-IMU, whereby a measurement noise matrix of a filter can be automatically adjusted, thereby effectively reducing the influence of carrier maneuvering on the calculation of a horizontal attitude. The specific steps are as follows:

In step 1, an inertial measurement element of a strapdown inertial navigation system is fully pre-heated, the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles (a roll angle 0 and a pitch angle ϕ 0 ) are completed, and the element enters a navigation working state.

In step 2, the initial horizontal attitude angles 0 and ϕ 0 obtained in step 1 are taken as initial values of a Kalman filtering state quantity X 0 =[ 0 ϕ 0 ] T , an initial mean square error is P 0 , and a Kalman filter is initialized.

In step 3, MEMS-IMU data is sampled at a k th time, and a random constant zero offset thereof is compensated to obtain a compensated specific force f k b and angular velocity ω k b .

In step 4, a Kalman filtering one-step prediction is performed by using an angular velocity increment Δω k b at the k th time as a known deterministic input u k-1 , where Δω k b =ω k b ·T, T being a calculation period.

In step 5, a maneuvering state of a carrier is judged by using the specific force f b and the angular velocity ω b from time k−N+1 to time k obtained in step 2, and a Kalman filtering measurement noise covariance matrix R k is self-adaptively adjusted, where N is the size of a data window.

In step 6, the specific force f k b at the k th time is used as a measurement vector Z k for measurement update so as to realize the correction of the state quantity.

In step 7, an estimated value of the state quantity at the k th time is taken as an initial value of a state quantity at a next time, and steps 3 to 6 are repeatedly performed until a navigation working state ends.

A correlated equation for Kalman filtering one-step prediction in step 4 is established:

In step 5, the maneuvering state of the carrier is judged, the maneuvering state of the carrier is calculated according to the specific force f b and the angular velocity ω b obtained in step 3, and a maneuvering vector T k is acquired to realize maneuvering judgment:

T k = ( 1 σ f 2 ⁢  f k b - g ⁢ f ¯ k  f ¯ k   + 1 σ ω 2 ⁢  ω _ k  ) where ⁢ ω ¯ k = 1 N ⁢ ∑ i = k - N + 1 k ω i b ⁢ and ⁢ f ¯ k = 1 N ⁢ ∑ i = k - N + 1 k f i b

are mean values obtained by performing moving average on the specific force f b and the angular velocity ω b from time k−N+1 to time k in step 3, respectively, the size of a data window being N, g being a local gravitational acceleration, and σ f and σ ω are weighting coefficients respectively.

A self-adaptive rule for the Kalman filtering measurement noise covariance matrix R k in step 5 is as follows:

In step 6, measurement update is performed. The specific force f k b =[f x,k f y,k f z,k ] T at the k th time in step 3 is used as a measurement vector Z k =[f x,k f y,k ] T for measurement update. An update equation is as follows:

{ K k = P k / k - 1 ⁢ H k T ( H k ⁢ P k / k - 1 ⁢ H k T + R k ) - 1 X ˆ k = X ˆ k / k - 1 + K k ( Z k - H k ⁢ X ˆ k / k - 1 ) P k = ( I - K k ⁢ H k ) ⁢ P k / k - 1 ⁢ where ⁢ H k = [ - g · cos ⁢ ϑ k 0 - g · sin ⁢ ϕ k ⁢ cos ⁢ ϑ k g · cos ⁢ ϕ k ⁢ cos ⁢ ϑ k ]

is a measurement matrix at time k, ϕ k and k are one-step prediction values of a horizontal attitude at time k, K k is a filtering gain at time k, {circumflex over (X)} k is a state estimation at time k, and P k is a state estimation mean square error matrix at time k.

With reference to FIGS. 1 and 2 , a specific embodiment of the disclosure includes the following steps:

In step 1, an inertial measurement element of a strapdown inertial navigation system is fully pre-heated.

In step 2, data of MEMS-IMU under a static state during a period of time is acquired, and it is considered that an average output value of an MEMS-IMU during the period of time is a random constant zero offset of a device, constant zero offset errors for a gyroscope and an accelerometer are corrected and compensated, and a specific force f b and an angular velocity ω b after compensating the constant zero offset errors are obtained.

In step 3, the strapdown inertial navigation system is initially aligned to obtain initial horizontal attitude angles (a roll angle 0 and a pitch angle ϕ 0 ) of a carrier system (b system) relative to a navigation coordinate system (n system, the navigation coordinate system herein being a geographical coordinate system), and then a navigation working state starts to be entered.

In step 4, a Kalman filtering equation is established:

In step 5, horizontal attitude angles are selected as a state quantity X=[ ϕ] T (a roll angle and a pitch angle ϕ), the initial horizontal attitude angles 0 and ϕ 0 obtained in step 3 are taken as initial values of the state quantity X 0 =[ 0 ϕ 0 ] T , an initial mean square error is P 0 , and a Kalman filter is initialized.

In step 6, MEMS-IMU data is sampled at a k th time, and a random constant zero offset thereof is compensated to obtain a compensated specific force f k b and angular velocity ω k b .

In step 7, a Kalman filtering one-step prediction is performed by using an increment Δω k b =ω k b ·T (T being a calculation period) within a ω k b sampling interval at the k th time in step 6 as a known deterministic input u k-1 :

In step 8, a maneuvering state of a carrier is judged by using the specific force f b and the angular velocity ω b from time k−N+1 to time k obtained in step 6, and a maneuvering vector T k is acquired, thereby self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R k .

T k = ( 1 σ f 2 ⁢  f k b - g ⁢ f ¯ k  f ¯ k   + 1 σ ω 2 ⁢  ω _ k  ) ( 3 ) where ⁢ ω ¯ k = 1 N ⁢ ∑ i = k - N + 1 k ω i b ⁢ and ⁢ f ¯ k = 1 N ⁢ ∑ i = k - N + 1 k f i b

are mean values obtained by performing moving average on the specific force f b and the angular velocity ω b from time k−N+1 to time k in step 6, respectively, the size of a data window is N, g is a local gravitational acceleration, σ f is a weighting coefficient of the specific force (usually 0.5-5 for vessels and 1-3 for vehicles), and σ ω is a weighting coefficient of the angular velocity (usually 1-5 for vessels and 0.5-1 for vehicles).

A self-adaptive adjustment rule for the Kalman filtering measurement noise covariance matrix R k is as follows:

In step 9, the specific force f k b =[f x,k f y,k f z,k ] T at the k th time in step 6 is used as a measurement vector Z k =[f x,k f y,k ] T for measurement update. An update equation is as follows:

{ K k = P k / k - 1 ⁢ H k T ( H k ⁢ P k / k - 1 ⁢ H k T + R k ) - 1 X ˆ k = X ˆ k / k - 1 + K k ( Z k - H k ⁢ X ˆ k / k - 1 ) P k = ( I - K k ⁢ H k ) ⁢ P k / k - 1 ⁢ where ⁢ H k = [ - g · cos ⁢ ϑ k 0 - g · sin ⁢ ϕ k ⁢ cos ⁢ ϑ k g · cos ⁢ ϕ k ⁢ cos ⁢ ϑ k ] ( 5 )

is a measurement matrix at time k, g is a local gravity acceleration, ϕ k and k are one-step prediction values of a horizontal attitude at time k, K k is a filtering gain at time k, {circumflex over (X)} k is a state estimation at time k, and P k is a state estimation mean square error matrix at time k.

In step 10, an estimated value of the state quantity at the k th time is taken as an initial value of a state quantity at a next time, and steps 6 to 9 are repeatedly performed.

The self-adaptive horizontal attitude measurement method based on motion state monitoring has been completed so far.

In order to illustrate the effectiveness of an algorithm, the algorithm is simulated. Simulation conditions are set as follows. A zero offset of a gyroscope of each axis is set to 1°/h, and a zero offset of an accelerometer is set to 1×10 −4 g. A motion state is set as follows. A carrier is in an oscillating motion state. The oscillation of each axis follows the sine function law. Oscillation amplitudes of rolling, pitching, and heading are all 10°, oscillation periods are all 10 S , and initial attitude angles and phase angles are all 0°. The simulation results are shown in FIG. 3 . The horizontal attitude measurement error is small, and the precision is high.

Although examples and drawings of the disclosure have been disclosed for illustrative purposes, those skilled in the art will appreciate that: various substitutions, changes and modifications are possible without departing from the spirit and scope of the disclosure and the appended claims, and therefore the scope of the disclosure is not limited to the disclosure of the examples and drawings.