阅读说明：本技术 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法 (Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm ) 是由 赵玉新 李帅阳 奔粤阳 吴磊 李倩 周广涛 臧新乐 魏廷枭 温官昊 于 2020-11-16 设计创作，主要内容包括：本发明提供一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法,完成初始对准后,利用实时采集三个轴上陀螺的输出信号和加速度计的输出信号进行捷联惯性导航解算；将GPS给出的位置信息作为量测,利用间接法卡尔曼滤波对实时解算的导航参数进行校正；利用校正的导航参数对运载体机动引起的线加速度和哥氏加速度进行补偿；利用互补滤波获得的陀螺积分误差作为量测量,再次利用卡尔曼滤波对已经校正过的捷联惯性导航解算的水平姿态信息进行校正,获得更高的测量精度。该方法有效利用加速度计的低频高精度特性,对陀螺仪进行不断修正,使水平姿态保持较高精度输出,保证系统在不同运动状态下具有较高的姿态测量精度,有一定的工程应用价值。(The invention provides a water surface vessel horizontal attitude measurement method based on a cascading Kalman filtering algorithm, which comprises the steps of collecting output signals of gyros on three axes and output signals of accelerometers in real time to carry out strapdown inertial navigation resolving after initial alignment is completed; taking the position information given by the GPS as measurement, and correcting the navigation parameters resolved in real time by using indirect Kalman filtering; compensating for linear acceleration and coriolis acceleration caused by vehicle maneuvers using the corrected navigation parameters; and (3) taking the gyro integral error obtained by complementary filtering as the measurement quantity, and correcting the corrected horizontal attitude information resolved by the strapdown inertial navigation by using Kalman filtering again to obtain higher measurement precision. The method effectively utilizes the low-frequency high-precision characteristic of the accelerometer to continuously correct the gyroscope, so that the horizontal attitude keeps higher-precision output, the system is ensured to have higher attitude measurement precision in different motion states, and the method has certain engineering application value.)

1. A method for measuring the horizontal attitude of a surface ship based on a cascaded Kalman filtering algorithm is characterized by comprising the following steps: the method comprises the following steps:

step 1: fully preheating an inertia measurement element of the strapdown inertial navigation system, and finishing initial alignment to enable the inertia measurement element to enter a navigation working state;

step 2: taking the position information given by the GPS as measurement, correcting the navigation parameters resolved in real time by using an indirect Kalman filter to obtain a strapdown matrix of the strapdown inertial navigation system after correctionVelocity V₁ ⁿAnd position P₁；

And step 3: utilizing the strapdown matrix of the strapdown inertial navigation system corrected in the step 2Velocity V₁ ⁿAnd position P₁Compensating linear acceleration and Coriolis acceleration caused by vehicle maneuvering according to a specific force equation to obtain theoretical output f of the accelerometer in a navigation system₁ ⁿ；

And 4, step 4: f obtained in step 3₁ ⁿProjected to the carrier system according to the actual output f of the accelerometer on the carrier system^bAttitude error after gyro integration obtained by complementary filteringObtaining a gyro integral error delta omega^b；

And 5: integrating the gyro integral error delta omega obtained in the step 4^bAs a measure of quantity, again using the strapdown matrix of the Kalman filter solution to the strapdown inertial navigation already correctedCorrecting to obtain corrected strapdown attitude matrix

2. The method for measuring the horizontal attitude of the surface vessel based on the cascaded kalman filter algorithm according to claim 1, wherein the method comprises the following steps: step 3, theoretical output f of the accelerometer in the navigation system₁ ⁿThe calculation of (A) is as follows:

wherein, V₁ ⁿIs the velocity corrected by the kalman filter,the rate of change of speed at the instant of time is solved for two consecutive attitudes,for the projection of the corrected rotational angular velocity of the earth on the navigation system,causing a navigation system to be equivalent to the angular velocity of rotation, g, of a terrestrial coordinate system for the horizontal velocity of the vehicleⁿThe corrected earth gravity vector is projected on a navigation system.

3. The surface vessel horizontal attitude measurement method based on the cascaded kalman filter algorithm according to claim 1 or 2, characterized in that: the attitude error after the step 4 gyro integrationIs calculated as:

wherein f is₁ ⁿFor the theoretical output of the accelerometer in the navigation system, f^bIs the actual output of the accelerometer carrier,is a strapdown attitude matrix corrected by Kalman filtering,^Trepresenting transpose to the matrix, | | is the corresponding vector modulus value.

4. The surface vessel horizontal attitude measurement method based on the cascaded kalman filter algorithm according to claim 1 or 2, characterized in that: selecting the quantity measurement in the step 5, and integrating the gyro integral error delta omega^bMeasurement of Z-measurements as a function of Kalman filtering for strapdown inertial navigation₂(t), measuring Z₂(t) and corresponding measurement matrix H₂(t) are respectively:

5. according to the rightThe method for measuring the horizontal attitude of the surface vessel based on the cascaded Kalman filtering algorithm according to claim 3, is characterized in that: selecting the quantity measurement in the step 5, and integrating the gyro integral error delta omega^bMeasurement of Z-measurements as a function of Kalman filtering for strapdown inertial navigation₂(t), measuring Z₂(t) and corresponding measurement matrix H₂(t) are respectively:

Technical Field

The invention relates to an attitude measurement system taking a micro-electromechanical inertial measurement unit as a core device, provides a horizontal attitude measurement method of a water surface ship based on a cascade Kalman filtering algorithm, and belongs to the field of navigation guidance and control.

Background

With the development of micro electro mechanical system technology, low-cost MEMS IMU has more and more applications in the navigation field, and the motion parameter measurement is carried out by utilizing an inertial sensor based on the micro electro mechanical system, so that the complex motion state of a ship in the sea can be detected, the angular motion parameter and the linear motion parameter of a carrier are output in real time, and the motion data acquisition of a user on a water surface ship is realized.

The micro-electromechanical gyroscope has the characteristic of random drift, the integral error of the micro-electromechanical gyroscope is accumulated along with time, and the accelerometer has no accumulated error but is easily influenced by vibration of a carrier. Common algorithms for fusing the two data are kalman filtering and complementary filtering, for example, in a patent document with the patent application number of 201811070907.X entitled "MEMS inertial navigation system horizontal attitude self-correction method based on maneuver state determination", the carrier motion is classified into low, medium and high dynamics by comparing the accelerometer output with the local gravity acceleration amplitude. And adjusting the measurement noise matrix in real time in low and medium dynamic states, and only performing time updating in high dynamic states. However, if the carrier is in high dynamic state for a long time, the attitude error will become larger and larger. As another example, in patent document with patent application No. 201911277173.7 entitled "a high-precision unmanned aerial vehicle system and intelligent control method", an adaptive function is established for the cutoff frequency of a complementary filter based on the precision factor and speed information of a GPS module receiving satellite signals to satisfy the resolving precision in different motion states. The method combines complementary filtering and a cascade Kalman filter, a navigation parameter obtained by the first-level Kalman filtering compensates the linear acceleration of a carrier and the Coriolis acceleration by means of a specific force equation to obtain theoretical accelerometer output, and then the theoretical accelerometer output is compared with the actual accelerometer output to obtain the gyro drift correction, which is used as the measurement of the second-level Kalman filtering to finally obtain the optimal navigation parameter. The method has the advantages that the defects that complementary filter gains are fixed and are not necessarily optimal are overcome through the two-stage Kalman filtering, the high-precision output of the horizontal attitude can be kept under different motion states, the system attitude measurement precision is effectively improved, and the method has a certain engineering application value.

Disclosure of Invention

The invention aims to improve the attitude measurement precision of a surface ship in a maneuvering scene, provides a horizontal attitude measurement method based on a cascading Kalman filtering algorithm, and provides accurate horizontal attitude information for the surface ship.

The purpose of the invention is realized as follows: after the strapdown inertial navigation system finishes initial alignment, the output signals of the gyros on the three axes are collected in real timeAnd output signal f of the accelerometer^bCarrying out strapdown inertial navigation resolving; taking the position information given by the GPS as measurement, and correcting the navigation parameters resolved in real time by using indirect Kalman filtering; compensating for linear accelerations and coriolis accelerations induced by the vehicle manoeuvre by means of specific force equations using the corrected navigation parameters; and (3) taking the gyro integral error obtained by complementary filtering as the measurement quantity, and correcting the corrected horizontal attitude information resolved by the strapdown inertial navigation by using Kalman filtering again to obtain higher measurement precision. The method comprises the following specific steps:

step 1, fully preheating an inertia measurement element of a strapdown inertial navigation system, and completing initial alignment to enable the inertia measurement element to enter a navigation working state;

step 2, taking the position information given by the GPS as measurement, correcting the navigation parameters resolved in real time by using an indirect Kalman filter to obtain a corrected strapdown matrix of the strapdown inertial navigation systemVelocity V₁ ⁿAnd position P₁；

Step 3, utilizing the strapdown matrix of the strapdown inertial navigation system corrected in the step 2Velocity V₁ ⁿAnd position P₁Compensating linear acceleration and Coriolis acceleration caused by vehicle maneuver by means of specific force equation to obtain theoretical output f of the accelerometer in a navigation system (n system, in the invention, the navigation coordinate system selects a geographic coordinate system)₁ ⁿ；

Step 4, converting f obtained in step 3₁ ⁿProjected onto a carrier system (system b) and the actual output f on the carrier system by means of an accelerometer^bAttitude error after gyro integration obtained by complementary filteringFurther obtain the integral error delta omega of the gyro^b；

Step 5, integrating the gyro integral error delta omega obtained in the step 4^bAs a measure of quantity, again using the strapdown matrix of the Kalman filter solution to the strapdown inertial navigation already correctedCorrecting to obtain corrected strapdown attitude matrix

The invention also includes such structural features:

1. said step 3 acceleratingTheoretical output f of the meter in the navigation system₁ ⁿThe calculation of (1):

wherein, V₁ ⁿIs the velocity corrected by the kalman filter,the rate of change of speed at the instant of time is solved for two consecutive attitudes,for the projection of the corrected rotational angular velocity of the earth on the navigation system,causing a rotation angular velocity, g, of a navigation system corresponding to a terrestrial coordinate system (e system) for the horizontal velocity of the vehicleⁿThe corrected earth gravity vector is projected on a navigation system.

2. The attitude error after the step 4 gyro integrationThe calculation of (2):

wherein f is₁ ⁿFor the theoretical output of the accelerometer in the navigation system, f^bIs the actual output of the accelerometer carrier,is a strapdown attitude matrix corrected by Kalman filtering,^Trepresenting transpose to the matrix, | | is the corresponding vector modulus value.

3. Selecting the quantity measurement in the step 4, and integrating the gyro integral error delta omega^bMeasurement values as a strapdown inertial navigation for Kalman filteringZ₂(t), measuring array Z₂(t) and corresponding measurement matrix H₂(t) are respectively:

compared with the prior art, the invention has the beneficial effects that: the invention comprehensively utilizes the acceleration and angular velocity information output by the micro-electromechanical inertia measurement unit to realize the high-precision horizontal attitude measurement of the system in different motion states. The linear acceleration and the Coriolis acceleration caused by the maneuvering of the carrier are compensated, the low-frequency high-precision characteristic of the accelerometer is effectively utilized, the gyroscope is continuously corrected, the horizontal attitude is kept to be output at high precision, even if the system has the motion acceleration, the optimal calculation of the misalignment angle is still kept, the system is guaranteed to have high attitude measurement precision in different motion states, the system attitude measurement precision is effectively improved, and the method has a certain engineering application value.

The invention compensates the linear acceleration and the Coriolis acceleration caused by the maneuvering of the carrier, effectively utilizes the low-frequency high-precision characteristic of the accelerometer, continuously corrects the gyroscope, keeps the horizontal attitude output with higher precision, still keeps the optimal calculation of the misalignment angle even if the system has the motion acceleration, ensures that the system has higher attitude measurement precision in different motion states, and has certain engineering application value.

Drawings

FIG. 1 is a flow chart of an implementation of the present invention.

FIG. 2 is a flow chart of the algorithm implementation of the present invention.

Detailed Description

In order to better understand the technical content of the invention, the following is explained and illustrated with reference to the attached drawings of the specification, and the main steps of the method are as follows:

step 1, fully preheating an inertia measurement element of a strapdown inertial navigation system;

step 2, initializing navigation parameters of the strapdown inertial navigation system, and acquiring the navigation parameters output by the accelerometer in real timeSpecific force f^bAnd angular velocity of gyroscope output

Step 3, carrying out initial alignment on the strapdown inertial navigation system to obtain an initial strapdown attitude matrix from a carrier system (system b) to a navigation coordinate system (system n, the navigation coordinate system in the invention selects a geographic coordinate system)Then starting to enter a navigation working state;

step 4, according to the initial strapdown attitude matrix of the gyro strapdown inertial navigation system obtained in the step 3And acquiring the angular velocity in real time in step 2And (3) obtaining an attitude quaternion by recursion updating and carrying out normalization calculation on the attitude quaternion, wherein a specific expression of the normalized attitude quaternion q is as follows:

q＝[q₁ q₂ q₃ q₄]^T (5)

wherein q is₁、q₂、q₃And q is₄Is an element of the normalized quaternion q;

step 5, calculating according to the normalized attitude quaternion q obtained in the step 4 to obtain a new strapdown attitude matrix

Step 6, utilizing the strapdown attitude matrix obtained in the step 5The specific force f output by the accelerometer of the strapdown inertial navigation system collected in the step 2 is used for^bAnd converting to a navigation coordinate system:

wherein f is^bRepresenting a projection of the accelerometer output specific force on a navigation coordinate system;

7, outputting the projection f of the specific force in the navigation coordinate system according to the accelerometer of the system in the step 6^bObtaining the acceleration of the system after removing the harmful acceleration, further updating and calculating to obtain the speed, and recording as Vⁿ(t)：

Wherein the content of the first and second substances,east speed calculated for the system,Is the north direction speed,The speed in the direction of the day is t represents the current moment;

step 8, updating the calculated speed V by using the step 7ⁿ(t) updating the calculated position p (t) of the strapdown inertial navigation system:

wherein the content of the first and second substances,is latitude, λ (t) is longitude, h (t) is altitude, and t represents the current time;

step 9, selecting position error(latitude error)Longitude error δ λ and altitude error δ h), and velocity error δ V ═ in the three northeast directions [ δ V ═ in the northeast direction_E δV_N δV_U]^TThe error phi of the platform misalignment angle is [ phi ]_x φ_y φ_z]^TZero offset delta a ═ delta a for carrier-based triaxial accelerometer_bx ΔA_by ΔA_bz]^TAnd constant drift of gyroscope_bxε_by ε_bz]^TState estimator X for Kalman filtering, i.e.

X＝[δr δV φ ΔA ε]^T (10)

Step 10, selecting a system noise vector W of Kalman filtering^BComprises the following steps:

W＝[w_ax w_ay w_az w_gx w_gy w_gz]^T (11)

wherein, w_ax、w_ayAnd w_azRandom noise, w, of triaxial accelerometers in a carrier system for strapdown inertial navigation systems_gx、w_gyAnd w_gzRandom noise of a three-axis gyroscope in a carrier system for the strapdown inertial navigation system is white Gaussian noise;

step 11, latitude provided by GPS is utilizedAnd longitude λ_A(t) as external auxiliary information for Kalman filtering by the strapdown inertial navigation system, and position information (latitude) calculated by navigation update of the strapdown inertial navigation systemAnd the longitude lambda (t)) are respectively subtracted, and the obtained position difference value is used as a measurement value Z when the strapdown inertial navigation carries out Kalman filtering₁(t), amountMeasured value Z₁(t) and corresponding measurement matrix H₁(t) are respectively:

step 12, estimating the error of the strapdown inertial navigation system in real time based on the Kalman filtering algorithm to obtain the position error of the strapdown inertial navigation systemError in velocityAnd state estimate of platform misalignment angle

Step 13, utilizing the position error estimated in step 12Error in velocityCorrecting the position and speed information resolved by the strapdown inertial navigation system;

wherein, V₁ ⁿIs the corrected inertial navigation velocity, P₁The corrected inertial navigation position is obtained;

step 14, the real-time estimated misalignment angle of the stage from step 12To construct a compensation quaternionTo strapdown inertial navigationStrap-down attitude matrix at current moment resolved by sexual navigation systemCorresponding attitude quaternion q ═ q₁ q₂ q₃ q₄]^TCompensation is performed to obtain a corrected quaternion q '═ q'₁ q′₂ q′₃ q′₄]^T：

Wherein, the elements of the modified quaternion q' are as follows:

step 19, normalizing the modified quaternion q' obtained in step 18:

the quaternion obtained after normalization is:

wherein the content of the first and second substances,q′₂、andis a normalized quaternionAn element of (1);

step 20, according to the normalized attitude quaternionCalculating to obtain a corrected strapdown attitude matrix

So far, the process of correcting the navigation parameters of the strapdown inertial navigation system by the Kalman filter 1 is completed;

step 21, compensating linear acceleration and Coriolis acceleration caused by vehicle maneuvering by means of a specific force equation by using the navigation parameters of the strapdown inertial navigation system corrected in the steps 17 to 20 to obtain theoretical output f of the accelerometer in the navigation system₁ ⁿ：

Wherein, V₁ ⁿIs the velocity corrected by the kalman filter,the rate of change of speed at the instant of time is solved for two consecutive attitudes,for the projection of the corrected rotational angular velocity of the earth on the navigation system,causing a navigation system to be equivalent to the angular velocity of rotation, g, of a terrestrial coordinate system for the horizontal velocity of the vehicleⁿThe corrected earth gravity vector is projected on a navigation system;

step 22, pairF obtained in step 21₁ ⁿThe projection of the system b and the actual output vector of the accelerometer are subjected to cross multiplication to obtain the attitude error after gyro integration

Wherein the content of the first and second substances,is a strapdown attitude matrix corrected by Kalman filtering,^Tthe transposition of the matrix is represented, and | is a corresponding vector module value;

step 23, utilizing the attitude error obtained in step 22Calculating integral error delta omega of gyro^b：

Wherein k is_p，k_iIs a proportionality coefficient, dt is a resolving period;

step 24, integrating the gyro integral error Δ ω obtained in step 23^bMeasurement value Z for Kalman filtering as strapdown inertial navigation₂(t), measured value Z₂(t) and corresponding measurement matrix H₂(t) are respectively:

step 24, estimating the error of the strapdown inertial navigation system in real time based on the Kalman filtering algorithm to obtain the position error of the strapdown inertial navigation systemError in velocityAnd state estimate of platform misalignment angle

Step 25, the real-time estimated misalignment angle of the stage from step 24To construct a compensation quaternionStrap-down attitude matrix at current moment resolved by strap-down inertial navigation systemCorresponding attitude quaternionCompensating to obtain a corrected quaternion q ═ q ″, and₁ q″₂q″₃q″₄]^T：

wherein, the elements of the modified quaternion q' are as follows:

step 26, normalizing the modified quaternion q' obtained in step 25:

the quaternion obtained after normalization is:

wherein the content of the first and second substances,andis a normalized quaternionAn element of (1);

27, according to the normalized attitude quaternionCalculating to obtain a corrected strapdown attitude matrix

So far, the navigation parameter correction process of the strapdown inertial navigation system by the Kalman filter 2;

therefore, the horizontal attitude updating and correction of the surface ship based on the cascaded Kalman filtering algorithm are completed.

In summary, the invention relates to an attitude measurement system taking a micro-electromechanical inertial measurement unit as a core device, and provides a water surface ship horizontal attitude measurement method based on cascade Kalman filtering. After the strapdown inertial navigation system finishes initial alignment, the output signals of the gyros on the three axes are collected in real timeAnd output signal f of the accelerometer^bPerforming strapdown inertial navigation solution(ii) a Taking the position information given by the GPS as measurement, and correcting the navigation parameters resolved in real time by using indirect Kalman filtering; compensating for linear accelerations and coriolis accelerations induced by the vehicle manoeuvre by means of specific force equations using the corrected navigation parameters; and (3) taking the gyro integral error obtained by complementary filtering as the measurement quantity, and correcting the corrected horizontal attitude information resolved by the strapdown inertial navigation by using Kalman filtering again to obtain higher measurement precision. The method effectively utilizes the low-frequency high-precision characteristic of the accelerometer to continuously correct the gyroscope, so that the horizontal attitude keeps higher-precision output, even if the system has motion acceleration, the optimal calculation of the misalignment angle is still kept, the system is ensured to have higher attitude measurement precision in different motion states, and the method has certain engineering application value.