A rotational modulation inertial navigation system and method for unmanned boats adapted to challenging environments

Abstract

The present invention provides a rotational modulation inertial navigation system and method for unmanned boats adaptable to challenging environments. The system includes a micro-electromechanical inertial unit (MEMS-IMU), a servo motor, a servo drive, a single-axis rotary stage, an active code ring, a navigation and positioning solution board, an aviation plug, and a housing frame. The method includes: establishing an iterative cubature Kalman filter based on maximum a posteriori estimation; designing a filter to simplify the iterative update structure; and introducing a penalty weight function to rapidly reduce the weight of outlier measurements. The present invention improves positioning accuracy and navigation parameter compensation, enhances the adaptability of single-axis rotational MEMS strapdown inertial navigation systems in challenging GNSS / DVL environments, and further meets the high-precision, high-reliability positioning, attitude determination, and navigation requirements of unmanned boats operating in GNSS / DVL challenging environments.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and relates to a navigation and positioning system and method for an unmanned boat, and in particular to a rotational modulation inertial navigation system and method for an unmanned boat adapted to challenging environments. Background Art

[0002] With the rapid growth of demand for marine development and defense, intelligent unmanned boats are rapidly developing towards low cost, strong anti-interference, high speed, and intelligent features. Navigation and positioning, as a key element in achieving intelligent unmanned boats, determine their operational efficiency and accuracy.

[0003] Single-axis rotational MEMS strapdown inertial navigation systems (SINS) are ideal for unmanned underwater vehicle navigation systems due to their low cost, low power consumption, and efficient system-level inertial sensor error self-compensation capabilities. They utilize single-axis rotational modulation technology to effectively eliminate the gyro and accelerometer constant drift perpendicular to the rotation axis. A navigation filter then uses external information from the Global Navigation Satellite System (GNSS) and underwater Doppler velocimeter (DVL) to estimate the gyro and accelerometer constant drift parallel to the rotation axis, thereby achieving error compensation and accurate acquisition of MEMS strapdown inertial navigation parameters. However, many state estimation methods use the minimization of the sum of squared errors as a cost function. In harsh sea conditions, the frequently interrupted GNSS and DVL signals lead to large-deviation outlier measurements and time-varying noise, which reduces the filter's estimation accuracy and reliability. At this time, the existing traditional robust Kalman filter cannot guarantee the immediacy and accuracy of the down-weighting processing of harmful information, which hinders the positioning and attitude determination and navigation information output of the single-axis rotation MEMS inertial navigation system. As a result, the realization of precise operation of unmanned boats in harsh sea conditions faces huge challenges.

[0004] Therefore, designing a suitable robust estimation method to help the navigation filter quickly suppress the serious impact of outlier measurements on the system and improve the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in the GNSS / DVL challenging environment has become an important way to improve the navigation and positioning accuracy and reliability of unmanned boats. Summary of the Invention

[0005] In order to overcome the shortcomings of the existing technology, the present invention aims to solve the problem that the measurement outliers of the global navigation satellite system and underwater Doppler velocimeter in harsh sea conditions affect the accurate estimation effect of the navigation parameters of GNSS / DVL / SINS in the challenging environment of GNSS / DVL. The present invention provides an unmanned vehicle rotation modulation inertial navigation system adapted to the challenging environment and a novel robust estimation method. The maximum a posteriori estimation and nonlinear least squares regression ideas are introduced into the cubic Kalman filter (CKF) framework to establish an iterative cubic Kalman filter (ICKF) equation. Multiple iterations are used to improve the convergence speed and error compensation effect of the filter. At the same time, a simplified iterative update structure is proposed to reduce the computational cost of the integrated navigation system. In addition, a penalty weight function is introduced to quickly reduce the weight of the outlier measurement to suppress the influence of gross errors on the navigation filter, so as to achieve the purpose of improving positioning accuracy and navigation parameter compensation effect, and further meet the high-precision, high-reliability positioning, attitude determination and navigation requirements of unmanned vehicle operations in the challenging environment of GNSS / DVL.

[0006] In order to achieve the above object, the present invention provides the following technical solutions:

[0007] A rotational modulation inertial navigation system for an unmanned boat adapted to challenging environments comprises a micro-electromechanical system (MEMS) inertial unit (IMU), a servo motor, a servo driver, a single-axis rotary stage, an active code ring, a navigation and positioning solution board, an aviation plug, and a housing frame. The MEMS inertial unit integrates a three-axis MEMS gyroscope and a MEMS accelerometer. The acceleration and angular rate information output by the MEMS gyroscope are compensated for zero bias using single-axis rotational modulation technology and multi-source information fusion technology to calculate the real-time attitude, velocity, and position information of the unmanned boat. The servo motor and servo driver are used to drive the MEMS inertial unit to rotate back and forth around a celestial axis, eliminating constant drift and scale factor error of the gyroscope and accelerometer perpendicular to the rotation axis. The navigation and positioning solution board receives the output acceleration and angular rate information, as well as velocity and position information from an external global navigation satellite system and velocity information from an underwater Doppler velocimeter. It then performs strapdown navigation solution, integrated navigation multi-source information fusion, and robust estimation anti-error using an internal software algorithm to output navigation parameters. The aviation plug and housing frame are respectively used for electrical information access and hardware device assembly.

[0008] Furthermore, the built-in software algorithm includes the following steps:

[0009] S1: Considering the characteristics of MEMS strapdown inertial navigation, integrated navigation and the coupling relationship of multiple error sources, attitude angle error [ΔΦ], velocity error [Δv], position error [Δp], gyroscope zero bias [ε], gyroscope scale factor error [δK G ]、Add zero bias Add the scale factor error [δK A] as the 24-dimensional correction state quantity x of the combined filter, and the speed and position information of the global navigation satellite system [v GNSS ,p GNSS ] and the velocity information of underwater Doppler velocimeter [v DVL ] is used as the external measurement y of the combined filter; the strapdown inertial error differential equation f(x) is designed under the condition of large nonlinear misalignment angle:

[0010]

[0011] in, f b represents the gyroscope and adder output, Represents the earth's rotation angular velocity vector and the rotation vector of the navigation system relative to the earth system and the inertial system respectively, represents the navigation system rotation calculation error, δg represents the gravity error, represents the transformation matrix from the carrier system to the navigation system, Represents the misalignment angle error matrix between the calculated navigation system and the ideal navigation system, R M 、R N , h represents the principal curvature radius of the meridian circle, the principal curvature radius of the meridian circle and the altitude, and L represents the local latitude;

[0012] S2: Introducing the maximum a posteriori estimation and nonlinear least squares regression ideas, and establishing the iterative cubic Kalman filter ICKF equation;

[0013] In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated:

[0014]

[0015] Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimate covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time;

[0016] Considering the maximum a posteriori estimation MAP idea, the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows:

[0017]

[0018] According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares:

[0019]

[0020] Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix Hessian matrix

[0021]

[0022] Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter ICKF equation is obtained:

[0023]

[0024] Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix; in the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is:

[0025]

[0026] Rewrite it to get the optimal estimation equation of the ith iteration cubature Kalman filter ICKF equation at the current estimation time k:

[0027]

[0028] S3: Design an iterative cubature Kalman filter to simplify the iterative update structure;

[0029] The simplified iterative update structure is as follows:

[0030] ⑦Generate volume point x j,k-1|k-1

[0031]

[0032] Among them, m represents the number of volume points, Represents the basic volume point, [1] represents the n-dimensional unit direction

[0033] The complete set of fully symmetrical points generated by fully permuting the quantity and changing the element signs;

[0034] ⑧ Prior prediction update

[0035] Calculate one-step forecast state and the prior estimated covariance

[0036]

[0037] Among them, Q k represents the system noise covariance;

[0038] ⑨Posteriori measurement update

[0039] Computational measurement estimation

[0040]

[0041] Calculate the cross-covariance P xy,k|k-1 and measurement covariance P yy,k|k-1

[0042]

[0043] Loop calculation of the posterior state estimate x for the i-th iteration i+1

[0044]

[0045] Until the number of iterations reaches the set value N, let Calculate the posterior estimated covariance once

[0046]

[0047] S4: Introduce a penalty weight function to quickly reduce the weight of outlier measurements

[0048] Considering the construction of robust kernel function, the nonlinear damping term of robust estimation method is used to replace the square term of original error in traditional Kalman filter; the robust estimation target X established for measuring outliers is * as follows:

[0049]

[0050] Among them, W k ∈(0,1) represents the external information penalty weight matrix at time k, θ(W k ) represents the penalty weight function, Represents the Mahalanobis distance of a vector;

[0051] The GM loss function is selected as the robust kernel function as follows:

[0052]

[0053] Among them, s GM Represents the shape control factor of the GM loss function;

[0054] Since the robust estimation objective function X * As a convex function, in order to calculate the optimal penalty weight matrix, the robust estimation objective function X * Find the partial derivative and take the minimum point:

[0055]

[0056] The present invention also provides a robust estimation method adapted to GNSS / DVL challenging environments, which is used to output precise navigation parameters of an unmanned vehicle's rotation-modulated inertial navigation system, comprising the following steps:

[0057] S1: Considering the characteristics of MEMS strapdown inertial navigation, integrated navigation and the coupling relationship of multiple error sources, attitude angle error [ΔΦ], velocity error [Δv], position error [Δp], gyroscope zero bias [ε], gyroscope scale factor error [δK G ]、Add zero bias Add the scale factor error [δK A ] as the 24-dimensional correction state quantity x of the combined filter, and the speed and position information of the global navigation satellite system [v GNSS ,p GNSS ] and the velocity information of underwater Doppler velocimeter [v DVL ] is used as the external measurement y of the combined filter; the strapdown inertial error differential equation f(x) is designed under the condition of large nonlinear misalignment angle:

[0058]

[0059] in, f b represents the gyroscope and adder output, Represents the earth's rotation angular velocity vector and the rotation vector of the navigation system relative to the earth system and the inertial system respectively, represents the navigation system rotation calculation error, δg represents the gravity error, represents the transformation matrix from the carrier system to the navigation system, Represents the misalignment angle error matrix between the calculated navigation system and the ideal navigation system, R M 、R N , h represents the principal curvature radius of the meridian circle, the principal curvature radius of the meridian circle and the altitude, and L represents the local latitude;

[0060] S2: Introducing the maximum a posteriori estimation and nonlinear least squares regression ideas, and establishing the iterative cubic Kalman filter ICKF equation;

[0061] In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated:

[0062]

[0063] Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimate covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time;

[0064] Considering the maximum a posteriori estimation MAP idea, the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows:

[0065]

[0066] According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares:

[0067]

[0068] Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix Hessian matrix

[0069]

[0070] Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter ICKF equation is obtained:

[0071]

[0072] Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix; in the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is:

[0073]

[0074] Rewrite it to get the optimal estimation equation of the ith iteration cubature Kalman filter ICKF equation at the current estimation time k:

[0075]

[0076] S3: Design an iterative cubature Kalman filter to simplify the iterative update structure;

[0077] The simplified iterative update structure is as follows:

[0078] ⑩Generate volume point x j,k-1|k-1

[0079]

[0080] Among them, m represents the number of volume points, Represents the basic volume point, [1] represents the n-dimensional unit direction

[0081] The complete set of fully symmetrical points generated by fully permuting the quantity and changing the element signs;

[0082] Prior prediction update

[0083] Calculate one-step forecast state and the prior estimated covariance

[0084]

[0085] Among them, Q k represents the system noise covariance;

[0086] Posterior measurement update

[0087] Computational measurement estimation

[0088]

[0089] Calculate the cross-covariance P xy,k|k-1 and measurement covariance P yy,k|k-1

[0090]

[0091] Loop calculation of the posterior state estimate x for the i-th iteration i+1

[0092]

[0093] Until the number of iterations reaches the set value N, let Calculate the posterior estimated covariance once

[0094]

[0095] S4: Introduce a penalty weight function to quickly reduce the weight of outlier measurements

[0096] Considering the construction of robust kernel function, the nonlinear damping term of robust estimation method is used to replace the square term of original error in traditional Kalman filter; the robust estimation target X established for measuring outliers is * as follows:

[0097]

[0098] Among them, W k ∈(0,1) represents the external information penalty weight matrix at time k, θ(W k ) represents the penalty weight function, Represents the Mahalanobis distance of a vector;

[0099] The GM loss function is selected as the robust kernel function as follows:

[0100]

[0101] Among them, s GM Represents the shape control factor of the GM loss function;

[0102] Since the robust estimation objective function X * As a convex function, in order to calculate the optimal penalty weight matrix, the robust estimation objective function X * Find the partial derivative and take the minimum point:

[0103]

[0104]

[0105] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0106] Compared with the existing technology, the unmanned boat single-axis rotation MEMS inertial navigation system provided by the present invention adopts single-axis rotation modulation technology to effectively eliminate the gyroscope and accelerometer constant drift and scale factor error perpendicular to the rotation axis, and then adopts multi-source information fusion technology to effectively estimate the gyroscope and accelerometer constant drift and scale factor error parallel to the rotation axis based on the external measurement information assisted by the global navigation satellite system (GNSS) and the underwater Doppler velocimeter (DVL). Finally, a simplified iterative update structure is introduced to meet the real-time performance and a penalty weight function is introduced to quickly reduce the weight of the outlier measurement, thereby improving the adaptability of the single-axis rotation MEMS strapdown inertial navigation system in the GNSS / DVL challenging environment, promoting the further research and development of the single-axis rotation MEMS strapdown inertial navigation anti-interference technology, and providing technical support for the high-precision, high-reliability and efficient operation of unmanned boats under harsh sea conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 This is a perspective view of the overall structure of the single-axis rotation modulation MEMS inertial navigation hardware system of the present invention.

[0108] Figure 2 This is a simplified iterative update structure flow chart of the robust iterative cubic Kalman filter in an embodiment of the present invention. DETAILED DESCRIPTION

[0109] The technical solutions provided by the present invention will be described in detail below with reference to specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0110] The present invention provides an unmanned boat rotation modulation inertial navigation system suitable for challenging environments. Figure 1The overall structure of the single-axis rotation modulation MEMS inertial navigation hardware system of the present invention includes: a micro-electromechanical inertial unit (MEMS-IMU) 1, a servo motor 2, a servo driver 3, a single-axis rotation table 4, a movable code ring 5, a navigation and positioning solution board 6, an aviation plug 7 and a shell frame 8. The micro-electromechanical inertial unit integrates a three-axis MEMS gyroscope and a MEMS accelerometer. The acceleration and angular rate information output by the micro-electromechanical inertial unit are compensated for zero bias by using single-axis rotation modulation technology and multi-source information fusion technology to calculate the real-time attitude, speed and position information of the unmanned boat. The servo motor and servo driver are used to drive the micro-electromechanical inertial unit to rotate back and forth around the celestial axis, effectively eliminating the gyroscope and accelerometer constant drift and scale factor error perpendicular to the rotation axis. The driver serves as the motor control end and is used to adjust the motor's speed, position, torque and other parameters. The navigation and positioning solver receives output acceleration and angular rate information, as well as velocity and position information from an external global navigation satellite system and an underwater Doppler velocimeter. Using internal software algorithms, it performs strapdown navigation solution, integrated navigation multi-source information fusion, and robust estimation to output navigation parameters. A single-axis rotation stage houses a microelectromechanical inertial unit (MEMS-IMU) and is driven by a servo motor to reciprocate around its celestial axis. A movable code ring connects electrical cables to prevent entanglement caused by the turntable's rotation. An aviation plug and housing frame facilitate electrical information access and hardware assembly, respectively.

[0111] The software algorithm built into the navigation and positioning solution board is a robust estimation software algorithm provided by the present invention that is adapted to the challenging environment of GNSS / DVL. It is used to solve the problem that the measurement outliers of the global navigation satellite system and underwater Doppler velocimeter in harsh sea conditions affect the accurate estimation effect of the GNSS / DVL / SINS integrated navigation parameters. The algorithm includes the following steps:

[0112] S1: Considering the characteristics of MEMS strapdown inertial navigation, integrated navigation and the coupling relationship of multiple error sources, attitude angle error [ΔΦ], velocity error [Δv], position error [Δp], gyroscope zero bias [ε], gyroscope scale factor error [δK G ]、Add zero bias Add the scale factor error [δK A ] as the 24-dimensional correction state quantity x of the combined filter, and the speed and position information of the global navigation satellite system [v GNSS ,p GNSS ] and the velocity information of underwater Doppler velocimeter [v DVL ] is used as the external measurement y of the combined filter. Design the strapdown inertial error differential equation f(x) under the condition of nonlinear large misalignment angle:

[0113]

[0114] in, f b represents the gyroscope and adder output, Represents the earth's rotation angular velocity vector and the rotation vector of the navigation system relative to the earth system and the inertial system respectively, represents the navigation system rotation calculation error, δg represents the gravity error, represents the transformation matrix from the carrier system to the navigation system, Represents the misalignment angle error matrix between the calculated navigation system and the ideal navigation system, R M 、R N , h represents the principal curvature radius of the meridian circle, the principal curvature radius of the meridian circle and the altitude, and L represents the local latitude. represents velocity estimation, and sec represents the secant trigonometric function.

[0115] S2: The maximum a posteriori estimation and nonlinear least squares regression ideas are introduced to establish the iterative cubic Kalman filter (ICKF) equation.

[0116] In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated:

[0117]

[0118] Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimation covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time. Represents the one-step prediction estimate of the state quantity from time k-1 to time k.

[0119] Considering the idea of ​​maximum a posteriori estimation (MAP), the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows:

[0120]

[0121] According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares:

[0122]

[0123] Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix and the Hessian matrix Further h(x k ) is a nonlinear measurement differential equation.

[0124] Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter (ICKF) equation is obtained:

[0125]

[0126] Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix. In the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is:

[0127]

[0128] Rewrite it to get the optimal estimation equation of the i-th iterative cubature Kalman filter (ICKF) equation at the current estimation time k:

[0129]

[0130] S3: Design an iterative cubic Kalman filter to simplify the iterative update structure.

[0131] The large number of matrix accumulation and multiplication operations generated by the iterative process increases the computing pressure of the processor, which needs to meet the real-time performance of the navigation calculation. Since the state posteriors provide more information about the nonlinear measurement function, constraining the solved nonlinear regression parameters to the high likelihood interval of the posterior probability density function can obtain a better approximation effect, and the influence of the prior information on the maximum posterior estimation is very small. Therefore, it is considered to introduce multiple iterative updates in the posterior filtering process at each filtering moment and only iteratively estimate the state quantity, while the posterior estimated covariance and the prior prediction stage information are only calculated once to reduce the computational complexity of the iterative update. The simplified iterative update structure is as follows:

[0132] Generate volume point x j,k-1|k-1

[0133]

[0134] Among them, m represents the number of volume points, represents the basic volume point, [1] represents the complete set of fully symmetrical points generated by fully permuting the n-dimensional unit vector and changing the element sign

[0135] Prior prediction update

[0136] Calculate one-step forecast state and the prior estimated covariance

[0137]

[0138] Among them, Q k represents the system noise covariance.

[0139] Posterior measurement update

[0140] Computational measurement estimation

[0141]

[0142] Calculate the cross-covariance P xy,k|k-1 and measurement covariance P yy,k|k-1

[0143]

[0144] in, is the one-step prediction estimate of the observation x from time k-1 to time k;

[0145] Loop calculation of the posterior state estimate x for the i-th iteration i+1

[0146]

[0147] Until the number of iterations reaches the set value N, let Calculate the posterior estimated covariance once

[0148]

[0149] S4: Introduce a penalty weight function to quickly reduce the weight of outlier measurements.

[0150] Consider constructing a robust kernel function and using the nonlinear damping term of the robust estimation method to replace the square term of the original error in the traditional Kalman filter. * as follows:

[0151]

[0152] Among them, W k ∈(0,1) represents the penalty weight matrix of external information at time k. The closer the penalty weight is to 1, the better the quality of the external information is. The closer the penalty weight is to 0, the greater the influence of outliers on the external information is. θ(W k ) represents the penalty weight function, which is used to reduce the weight of external information including outlier measurements to suppress its impact on the filter. Represents the Mahalanobis distance of a vector.

[0153] The Geman McClure (GM) loss function is selected as the robust kernel function, which is:

[0154]

[0155] Among them, s GM Represents the shape control factor of the GM loss function, which determines the convergence speed and robust performance of the robust kernel function.

[0156] Since the robust estimation objective function X * As a convex function, in order to calculate the optimal penalty weight matrix, the robust estimation objective function X * Find the partial derivative and take the minimum point:

[0157]

[0158] The penalty weight function is introduced into the simplified iterative update structure of the iterative cubature Kalman filter according to S3, and the cross-covariance P is calculated in the posterior measurement update phase. xy,k|k-1 and measurement covariance P yy,k|k-1 After the step. In each iteration of the posterior estimation state, the external information penalty weight matrix W is calculated in advance k To help the navigation filter quickly suppress the serious impact of outlier measurements on the system.

[0159] The robust iterative cubature Kalman filter composed of S3 and S4 is as follows Figure 2 As shown in the figure, using the GM loss function to estimate the regression coefficient can suppress outliers while maintaining smoothness, resulting in a more robust least-squares solution. This rapidly reduces the weight of outlier measurements to suppress the impact of gross errors on the navigation filter, thereby improving positioning accuracy and navigation parameter compensation, and enhancing the adaptability of single-axis rotation MEMS strapdown inertial navigation systems in challenging GNSS / DVL environments.

[0160] The technical means disclosed in the solutions of the present invention are not limited to those disclosed in the above-mentioned embodiments, but also include technical solutions composed of any combination of the above-mentioned technical features. It should be noted that those skilled in the art may make various improvements and modifications without departing from the principles of the present invention, and such improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A rotational modulation inertial navigation system for unmanned boats adapted to challenging environments, characterized by: The invention comprises a micro-electromechanical inertial unit (MEMS-IMU), a servo motor, a servo driver, a single-axis rotary table, an active code ring, a navigation and positioning solution board, an aviation plug and a shell frame; the micro-electromechanical inertial unit integrates a three-axis MEMS gyroscope and a MEMS accelerometer, and the acceleration and angular rate information output by the micro-electromechanical inertial unit are compensated for zero bias by using single-axis rotation modulation technology and multi-source information fusion technology to calculate the real-time attitude, speed and position information of the unmanned boat; the servo motor and servo driver are used to drive the micro-electromechanical inertial unit to rotate back and forth around the celestial axis, eliminating the gyroscope and accelerometer constant drift and scale factor error perpendicular to the rotation axis; the navigation and positioning solution board is used to receive the output acceleration and angular rate information as well as the speed and position information of the external global navigation satellite system and the speed information of the underwater Doppler velocimeter, and outputs the navigation parameters after completing the strapdown navigation solution, the combined navigation multi-source information fusion and the robust estimation anti-error through the built-in software algorithm; The aviation plug and the housing frame are used for electrical information access and hardware equipment assembly respectively; The built-in software algorithm includes: The maximum a posteriori estimation and nonlinear least squares regression ideas are introduced to establish the iterative cubic Kalman filter ICKF equation; In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated: Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimate covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time; Considering the maximum a posteriori estimation MAP idea, the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows: According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares: Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix ▽V(x i )=J T (x i )r(x i ), Hessian matrix Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter ICKF equation is obtained: Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix; in the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is: Rewrite it to get the optimal estimation equation of the ith iteration cubature Kalman filter ICKF equation at the current estimation time k:

2. The unmanned boat rotation modulation inertial navigation system adapted to challenging environments according to claim 1 is characterized in that: The built-in software algorithm specifically includes the following steps: S1: Considering the characteristics of MEMS strapdown inertial navigation, integrated navigation and the coupling relationship of multiple error sources, attitude angle error [ΔΦ], velocity error [Δv], position error [Δp], gyroscope zero bias [ε], gyroscope scale factor error [δK G ]、Add zero bias Add the scale factor error [δK A ] as the 24-dimensional correction state quantity x of the combined filter, and the speed and position information of the global navigation satellite system [v GNSS ,p GNSS ] and the velocity information of the underwater Doppler velocimeter [v DVL ] is used as the external measurement y of the combined filter; the strapdown inertial error differential equation f(x) is designed under the condition of large nonlinear misalignment angle: in, f b represents the gyroscope and adder output, Represents the earth's rotation angular velocity vector and the rotation vector of the navigation system relative to the earth system and the inertial system respectively, represents the navigation system rotation calculation error, δg represents the gravity error, represents the transformation matrix from the carrier system to the navigation system, Represents the misalignment angle error matrix between the calculated navigation system and the ideal navigation system, R M 、R N , h represents the principal curvature radius of the meridian circle, the principal curvature radius of the meridian circle and the altitude, and L represents the local latitude; S2: Introducing the maximum a posteriori estimation and nonlinear least squares regression ideas, and establishing the iterative cubic Kalman filter ICKF equation; In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated: Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimate covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time; Considering the maximum a posteriori estimation MAP idea, the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows: According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares: Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix Hessian matrix Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter ICKF equation is obtained: Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix; in the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is: Rewrite it to get the optimal estimation equation of the ith iteration cubature Kalman filter ICKF equation at the current estimation time k: S3: Design an iterative cubature Kalman filter to simplify the iterative update structure; The simplified iterative update structure is as follows: ①Generate volume point x j,k-1|k-1 Among them, m represents the number of volume points, represents the basic volume point, [1] represents the complete set of fully symmetric points generated by fully permuting the n-dimensional unit vector and changing the element signs; ② Prior prediction update Calculate one-step forecast state and the prior estimated covariance Among them, Q k represents the system noise covariance; ③Posteriori measurement update Computational measurement estimation Calculate the cross-covariance P xy,k|k-1 and measurement covariance P yy,k|k-1 Loop calculation of the posterior state estimate x for the i-th iteration i+1 Until the number of iterations reaches the set value N, let Calculate the posterior estimated covariance once S4: Introduce a penalty weight function to quickly reduce the weight of outlier measurements Considering the construction of robust kernel function, the nonlinear damping term of robust estimation method is used to replace the square term of original error in traditional Kalman filter; the robust estimation target X established for measuring outliers is * as follows: Among them, W k ∈(0,1) represents the external information penalty weight matrix at time k, θ(W k ) represents the penalty weight function, Represents the Mahalanobis distance of a vector; The GM loss function is selected as the robust kernel function as follows: Among them, s GM Represents the shape control factor of the GM loss function; Since the robust estimation objective function X * As a convex function, in order to calculate the optimal penalty weight matrix, the robust estimation objective function X * Find the partial derivative and take the minimum point:

3. A robust estimation method adapted to GNSS / DVL challenging environments for outputting precise navigation parameters for a rotationally modulated inertial navigation system of an unmanned vehicle, comprising the following steps: S1: Considering the characteristics of MEMS strapdown inertial navigation, integrated navigation and the coupling relationship of multiple error sources, attitude angle error [ΔΦ], velocity error [Δv], position error [Δp], gyroscope zero bias [ε], gyroscope scale factor error [δK G ], plus zero bias [▽], plus scale factor error [δK A ] as the 24-dimensional correction state quantity x of the combined filter, and the speed and position information of the global navigation satellite system [v GNSS ,p GNSS ] and the velocity information of the underwater Doppler velocimeter [v DVL ] is used as the external measurement y of the combined filter; the strapdown inertial error differential equation f(x) is designed under the condition of large nonlinear misalignment angle: in, f b represents the gyroscope and adder output, Represents the earth's rotation angular velocity vector and the rotation vector of the navigation system relative to the earth system and the inertial system respectively, represents the navigation system rotation calculation error, δg represents the gravity error, represents the transformation matrix from the carrier system to the navigation system, Represents the misalignment angle error matrix between the calculated navigation system and the ideal navigation system, R M 、R N , h represents the principal curvature radius of the meridian circle, the principal curvature radius of the meridian circle and the altitude, and L represents the local latitude; S2: Introducing the maximum a posteriori estimation and nonlinear least squares regression ideas, and establishing the iterative cubic Kalman filter ICKF equation; In the framework of nonlinear sigma point filters, the posterior probability density function with Gaussian measurement noise is calculated: Where H represents the measurement matrix, R and P represent the measurement noise covariance and the optimal estimate covariance, p(·) represents the probability density function, exp[·] represents the exponential function with the natural constant e as the base, and the subscript k represents the estimation time; Considering the maximum a posteriori estimation MAP idea, the posterior probability density function is equivalent to solving the nonlinear regression parameters, which is equivalent to the filter update process as follows: According to the Newton-Raphson algorithm, the iterative update process of the estimated state quantity x is constructed as an iterative form of nonlinear least squares: Among them, i represents the number of iterations at the current estimation moment, and the Jacobian matrix Hessian matrix Through the inverse matrix lemma, the optimal estimation equation of the iterative cubic Kalman filter ICKF equation is obtained: Where K = P xy,k|k-1 (P yy,k|k-1 ) -1 Represents the filter gain matrix; in the framework of the cubic Kalman filter, the covariance expression in the filter gain matrix K is replaced by the uncertainty in the likelihood approximation, which is: Rewrite it to get the optimal estimation equation of the ith iteration cubature Kalman filter ICKF equation at the current estimation time k: S3: Design an iterative cubature Kalman filter to simplify the iterative update structure; The simplified iterative update structure is as follows: ④Generate volume point x j,k-1|k-1 Among them, m represents the number of volume points, represents the basic volume point, [1] represents the complete set of fully symmetric points generated by fully permuting the n-dimensional unit vector and changing the element signs; ⑤ Prior prediction update Calculate one-step forecast state and the prior estimated covariance Among them, Q k represents the system noise covariance; ⑥Posteriori measurement update Computational measurement estimation Calculate the cross-covariance P xy,k|k-1 and measurement covariance P yy,k|k-1 Loop calculation of the posterior state estimate x for the i-th iteration i+1 Until the number of iterations reaches the set value N, let Calculate the posterior estimated covariance once S4: Introduce a penalty weight function to quickly reduce the weight of outlier measurements Considering the construction of robust kernel function, the nonlinear damping term of robust estimation method is used to replace the square term of original error in traditional Kalman filter; the robust estimation target X established for measuring outliers is * as follows: Among them, W k ∈(0,1) represents the external information penalty weight matrix at time k, θ(W k ) represents the penalty weight function, Represents the Mahalanobis distance of a vector; The GM loss function is selected as the robust kernel function as follows: Among them, s GM Represents the shape control factor of the GM loss function; Since the robust estimation objective function X * As a convex function, in order to calculate the optimal penalty weight matrix, the robust estimation objective function X * Find the partial derivative and take the minimum point:

Images