A SINS self-aided navigation method based on maneuver constraint and incremental filtering

Abstract

A kind of SINS self-aided navigation method based on maneuvering constraint and incremental filtering, 1 drives AUV to do periodic steady motion in horizontal direction, while maneuvering to specified direction, adds high-frequency component to the velocity in navigation coordinate system;2 obtains real-time data of inertial sensor installed on underwater AUV;3 input the horizontal velocity in navigation coordinate system obtained in step 2 into high-pass filter, obtains a set of horizontal velocity in navigation coordinate system with low-frequency error component removed;4 establishes Kalman filter, the increment of velocity component obtained in step 3 is input into Kalman filter as the measurement of Kalman filter, the obtained velocity error is compensated, and high-precision positioning value after self-aided compensation is obtained.The application effectively solves the problem that SINS positioning error diverges with time, compared with the positioning scheme of pure SINS solution, the velocity error can be limited in a very small range, and the positioning accuracy is greatly improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of underwater autonomous vehicle navigation and positioning, especially to the field of strapdown inertial navigation system, aiming at completing high-precision navigation and positioning under the condition of using only inertial sensors (gyroscope, accelerometer) and strapdown inertial navigation system, especially to a SINS self-aided navigation method based on maneuvering constraint and incremental filtering. BACKGROUND

[0002] Ocean, which accounts for more than 70% of the area on the earth's surface, is the focus of human exploration and competition in a new round. As an important underwater unmanned carrier, Autonomous Underwater Vehicle (AUV) plays an important application in topographic exploration, resource search, hydrological data collection and other work, and has been widely concerned. For underwater positioning of AUV, due to the blockage of seawater to radio signals and the lack of single features in underwater environment, satellite navigation and visual navigation commonly used on land are difficult to play.

[0003] Strapdown Inertial navigation system (SINS) measures the angular velocity and acceleration of the carrier relative to the inertial space through gyroscopes and accelerometers, and obtains the attitude, velocity and position information of the carrier after navigation integration, which is a completely autonomous navigation method. It has the characteristics of high output frequency, strong adaptability, high short-time precision, etc. However, when used for a long time, the error caused by initial alignment error and instrument error will be accumulated with the integration process, so that the error becomes unacceptable.

[0004] For this problem, a common solution is to add other sensors to correct it regularly, but the commonly used acoustic positioning equipment usually needs to be arranged in advance, and the method based on terrain matching or geomagnetic matching also needs a database to assist. In order to obtain high-precision correction, these two methods need to limit the activity range of AUV in advance, which greatly reduces the flexibility of AUV. Therefore, how to reduce the accumulated error only by using SINS and its error characteristics without adding additional sensors or measurement value input has high research and application value.

[0005] Therefore, our project team applied for a Chinese patent application (CN110345941A), disclosing a SINS self-assisted navigation method for deep-sea manned submersibles. Targeting the descent / ascent scenario of manned submersibles, a high-pass, zero-delay digital filter was designed. This filter obtains a set of velocities that filter out low-frequency errors, and these velocities are then fused using a Kalman filter to achieve SINS self-assisted navigation without the need for external reference sensors. However, this method requires the submersible to perform circular motion in place, limiting its application to manned submersible descent / ascent.

[0006] Therefore, this invention proposes a self-assisted SINS scheme based on maneuver constraints and incremental filtering. The method involves: first, designing a high-frequency maneuver scheme for the AUV, enabling it to possess forward velocity while introducing high-frequency components into the navigation system; then, filtering out low-frequency error components using a high-pass filter; and finally, using incremental observation to address the issue of filtering out constant components in the velocity. This effectively suppresses the error divergence problem of the SINS system without adding other external reference information.

[0007] The difference between the solution proposed in this invention and other inventions lies in the following:

[0008] 1. The motion constraint designed in this invention is a composite high-frequency motion, so it can be used in ordinary AUV operations, no longer limiting its application range to spiral diving / surfacing, thus broadening the applicable conditions.

[0009] 2. This invention designs an incremental observation method to solve the problem of filtering out constant values ​​in the velocity of the navigation system. It takes the increment of output information at adjacent time points as the measurement and redesigns the measurement matrix in the Kalman filter. This method can effectively eliminate the influence of unknown constant velocity values ​​on velocity estimation. Summary of the Invention

[0010] Objective of the Invention: The technical problem this invention aims to solve is that for underwater positioning of AUVs based on strapdown inertial navigation systems (SINS), as navigation time increases, errors caused by the initial alignment error and instrument errors of the SINS accumulate during the integration process, making the errors unacceptable. Furthermore, using other sensors or measurements to correct positioning errors limits the AUV's operating range and reduces its operational flexibility. Therefore, it is necessary to find a solution that utilizes maneuver constraints and the error characteristics of SINS to reduce the accumulation of positioning errors, thus meeting the requirements for long-term navigation.

[0011] Technical Solution: To achieve the objectives of this invention, the technical solution adopted by this invention is as follows: This invention provides an algorithm based on maneuver constraints and incremental filtering. By restricting the maneuver of the AUV, high-frequency components are added to the speed of the navigation system. Then, a digital high-pass filter is used to remove low-frequency error components from the high-frequency speed components. Finally, Kalman filtering based on incremental observation is used to complete the SINS self-assisted navigation, thereby reducing accumulated errors and achieving high-precision navigation over a long period of time.

[0012] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0013] A self-assisted navigation method for SINS based on maneuver constraints and incremental filtering is described below:

[0014] (1) Drive the AUV to perform periodic steady-state motion in the horizontal direction, and add high-frequency components to the velocity in the navigation coordinate system while maneuvering in the specified direction;

[0015] (2) Based on the maneuvering in step (1), real-time data from the inertial sensors installed on the underwater AUV are obtained, specifically including the output data from the gyroscope and accelerometer of the inertial measurement element, and the attitude calculation algorithm is executed using the sensor data to obtain the velocity in the navigation coordinate system.

[0016] (3) Set the parameters (f) of the digital high-pass filter. p f s A p A s Then, the horizontal velocity in the navigation coordinate system obtained in step (2) is input into the high-pass filter to obtain a set of horizontal velocity quantities in the navigation coordinate system that have eliminated low-frequency error components.

[0017] (4) Establish a Kalman filter, and use the increment of the velocity component obtained in step (3) as the quantity input of the Kalman filter to compensate for the obtained velocity error, so as to obtain a high-precision positioning value after self-aid compensation.

[0018] As a further improvement of the present invention, the method for introducing high-frequency navigation system information in step (1) is as follows:

[0019] For an AUV, firstly, it maintains a constant forward velocity v and moves forward in a uniform straight line. Then, while maintaining the forward velocity v, a constant driving force is applied to the AUV using the rudder fin propeller, causing it to rotate at an angular velocity ω for t seconds while moving forward. Subsequently, the opposite driving force is applied to the AUV using the rudder fin propeller, causing it to rotate in the opposite direction at an angular velocity of -ω for t seconds. In this way, the ideal navigation system velocity is a triangular wave high-frequency information within one cycle.

[0020] As a further improvement to the present invention, the method for selecting the high-pass filter in step (3) is as follows:

[0021] S3.1: Select a finite impulse (IIR) filter as the digital filter, where the following parameters need to be set: ω p ω represents the passband cutoff circle frequency, which is set according to the frequency of the periodic maneuver constraint in step (2) and is lower than the frequency of the periodic maneuver; s Represents the stopband cutoff angular frequency, set to 0.0015~0.002; A p Represents the maximum passband attenuation, ranging from 0.1 to 1; A s This represents the minimum stopband attenuation, ranging from 40 to 100.

[0022] S3.2: The time delay bit depth of the digital filter is obtained by calculating the frequency of the high-frequency maneuver. Then, the time delay bit depth is subtracted from the obtained data to obtain the time delay compensated speed value. The calculation method for the time delay bit depth N is as follows:

[0023]

[0024] Among them, T sp The sampling period is The filter's lead time is calculated using the following formula:

[0025]

[0026] in, H(z) is the angular frequency of the periodic motion introduced in step (1), and H(z) is the transfer function of the IIR filter in step (3).

[0027] As a further improvement to the present invention, the method for selecting the measurement variables and measurement matrix of the Kalman filter in step (4) is as follows:

[0028] Selection of measurement variables:

[0029]

[0030] in, and These represent the eastward and northward velocities obtained after passing through the digital filter in step (3), respectively. and The eastward and northward velocities obtained from the inertial element and the inertial calculation algorithm in step (2) are respectively represented by h. SINR This represents the altitude information obtained by the inertial calculation algorithm; h pressure This indicates the altitude information obtained from the altimeter sensor.

[0031] Selection of the measurement matrix:

[0032]

[0033] Where F 21 (k)(1:2, 1:2) represents a new matrix composed of rows 1 and 2 and columns 1 and 2 of the submatrix in the Kalman filter at time k, and Δt is the time interval between two Kalman filters.

[0034] Technical Principle: Based on the error propagation characteristics of the SINS system, the velocity error of SINS exhibits low-frequency oscillations. Therefore, high-frequency components in the navigation system velocity can be introduced through maneuver constraints. At this point, the ideal navigation system velocity output is a high-frequency component, while the calculation errors caused by instrument errors and initial alignment errors are low-frequency errors. Therefore, low-frequency errors can be filtered out from the high-frequency ideal velocity by constructing a digital filter. However, the filter also filters out the constant component in the ideal velocity, so the velocity after passing through the filter cannot be directly output. By subtracting adjacent velocities in the filtered velocity, the influence of the constant can be masked. Therefore, by designing a Kalman filter and using the increment as a measurement for fusion, the error of SINS can be effectively estimated, thereby compensating for the velocity output and obtaining a higher-precision positioning solution.

[0035] Beneficial effects: The SINS self-assisted navigation scheme adopted in this invention can effectively filter out velocity errors caused by initial alignment errors and instrument errors by using only inertial sensors (gyroscopes, accelerometers) and navigation algorithms through maneuver constraints, thereby improving the accuracy of navigation and positioning. Attached Figure Description

[0036] Figure 1 This is a framework diagram of the SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering according to the present invention;

[0037] Figure 2 The figure shows a comparison of the eastward velocity error between the SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering and the pure SINS solution of this invention.

[0038] Figure 3 This figure shows a comparison of the northbound velocity error between the SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering and the pure SINS solution. Detailed Implementation

[0039] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0040] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] For the underwater positioning problem of AUVs based on strapdown inertial navigation systems (SINS), as navigation time increases, the errors caused by the initial alignment error and instrument errors of the SINS accumulate during the integration process, making the errors unacceptable. Meanwhile, using other sensors or measurements to correct positioning errors limits the AUV's operating range and reduces its operational flexibility. For the self-calibration technology problem of SINS without external information assistance, this invention employs an algorithm based on maneuver constraints and incremental filtering. This algorithm adds high-frequency components to the navigation system velocity by restricting AUV maneuvers, then uses a digital high-pass filter to remove low-frequency error components from the high-frequency velocity components, and finally completes SINS self-assisted navigation through incremental observation Kalman filtering. This achieves the goal of reducing accumulated errors and realizing high-precision navigation over long periods. The specific implementation scheme of this invention is as follows, where the framework diagram of the SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering is shown below. Figure 1 As shown:

[0042] Step 1: Drive the AUV to perform periodic steady-state motion in the horizontal direction. While maneuvering in the specified direction, add high-frequency components to the velocity in the navigation coordinate system.

[0043] First, define the following coordinate system:

[0044] Navigation coordinate system: Select the local geographic coordinate system as the navigation coordinate system, pointing to the east, north, and sky respectively, and denote the navigation coordinate system as n.

[0045] Carrier coordinate system: The origin of the carrier coordinate system is located at the center of gravity of the carrier, and the three axes point to the right, front and top of the carrier respectively. The volume coordinate system is recorded as b.

[0046] Inertial coordinate system: A constant coordinate system, denoted as i.

[0047] Earth coordinate system: The origin is located at the center of the Earth, the x and y axes are located in the equatorial plane of the Earth and the two axes are perpendicular to each other, and the z axis points to the prime meridian, which is denoted as e.

[0048] 1.1: For an AUV, firstly, maintain a constant forward velocity v and move forward in a uniform straight line. Then, while maintaining the forward velocity v, apply a constant driving force to the AUV using the rudder fin propeller, causing it to rotate at an angular velocity ω for t seconds while moving forward. Subsequently, apply the opposite driving force to the AUV using the rudder fin propeller, causing it to rotate in the opposite direction at an angular velocity of -ω for t seconds. Thus, the ideal navigation system velocity is a triangular wave high-frequency information with a period of ω.

[0049] Step 2: Based on the maneuvering in Step 1, acquire real-time data from the inertial sensors installed on the underwater AUV, specifically including the output data from the gyroscope and accelerometer of the inertial measurement unit, and use the sensor data to execute the attitude calculation algorithm to obtain the velocity in the navigation coordinate system.

[0050] 2.1: The inertial measurement units (gyroscope, accelerometer) are fixed to the AUV carrier, with their OX, OY, and OZ coordinate axes pointing to the right, front, and top of the AUV carrier, respectively, denoted as the body coordinate system b. The geographical east, north, and sky directions are selected as the navigation coordinate system, denoted as the n system. The inertial coordinate system is denoted as i, and the Earth coordinate system as e. After the initial alignment algorithm, the initial velocity v of the AUV carrier can be obtained. n (0), initial position [L(0) λ(0) h(0)] T And the initial attitude q(0), and then according to equations (1) to (4), the latest position of the carrier can be obtained recursively:

[0051]

[0052] Where q represents the rotation quaternion from the vehicle coordinate system to the navigation coordinate system; This represents the projection of the relative angular velocity between the carrier system b and the navigation system n onto the carrier system b. This represents the projection of the relative angular velocity between Earth frame e and inertial frame i onto navigation frame n; V represents the projection of the relative angular velocity between navigation frame n and Earth frame e onto navigation frame n; n =[v E v N v U ] T f is the projection of the ground velocity in navigation frame n; b This indicates the specific force information output by the accelerometer; g n Represents the projection of gravitational acceleration in navigation frame n; L, λ, and h represent latitude, longitude, and altitude, respectively; R n R e These represent the radii of curvature of the Earth's meridian and circumference, respectively.

[0053] Step 3: Set the parameters (f) of the digital high-pass filter. p f s A p A s Then, the horizontal velocity in the navigation coordinate system obtained in step (2) is input into the high-pass filter to obtain a set of horizontal velocity quantities in the navigation coordinate system that have eliminated low-frequency error components.

[0054] 3.1: Selecting a finite impulse response (IIR) filter as the digital filter requires setting the following parameters: ω p ω represents the passband cutoff circle frequency, which is set according to the frequency of the periodic maneuver constraint in step (2), and is generally slightly lower than the frequency of the periodic maneuver; s This represents the stopband cutoff angular frequency, typically set to 0.0015–0.002; A p Represents the maximum passband attenuation, typically taken as 0.1 to 1; A s This represents the minimum stopband attenuation, typically taken as 40 to 100.

[0055] 3.2: The time delay bit depth of the digital filter is calculated using the frequency of the high-frequency maneuver. Then, the time delay bit depth is subtracted from the obtained data to obtain the time-delay compensated speed value. The calculation method for the time delay bit depth N is as follows:

[0056]

[0057] Among them, T sp The sampling period is The filter's lead time can be calculated using the following formula:

[0058]

[0059] Step 4: Establish a Kalman filter. Use the increment of the velocity component obtained in step (3) as the quantity input of the Kalman filter to compensate for the obtained velocity error and obtain a high-precision positioning value after self-aided compensation.

[0060] 4.1: Select the SINS system attitude misalignment angle, SINS system navigation system velocity error, SINS system position error, gyroscope constant error and speedometer constant error as state variables, and select the difference between the velocity components after filtering out the errors obtained in step (3) at adjacent time moments as the measurement, to obtain the discretized Kalman filter state equation and measurement equation;

[0061] 4.2: Initial values ​​for the given state estimate and the variance of the estimate error And P0, based on the observation Z at time k k The state estimate at time k is obtained through real-time recursive calculation.

[0062] 4.3: Subtract the error state quantity estimate obtained in step 4.2 from the SINS output to obtain the corrected SINS navigation solution value.

[0063] Furthermore, the 15-dimensional state variables in S4.1 are selected as follows:

[0064]

[0065] The discretized Kalman filter equation in section 4.1 is as follows:

[0066]

[0067] Where X(k+1) is the state estimate at time k+1, X(k) is the state estimate at time k, Z(k+1) is the observation at time k+1, F(k) is the state transition matrix at time k, Γ(k) is the system process noise input matrix, H(k+1) is the observation matrix at time k+1, W(k) is the random process noise in the state transition, and V(k+1) is the random measurement noise;

[0068] 4.1 The measurement measurements and measurement matrix are represented as follows: Selection of measurement variables: 3

[0069]

[0070] in, and These represent the eastward and northward velocities obtained after passing through the digital filter in step (3), respectively. and The eastward and northward velocities obtained from the inertial element and the inertial calculation algorithm in step (2) are respectively represented by h. SINS This represents the altitude information obtained by the inertial calculation algorithm; h pressure This indicates the altitude information obtained from the altimeter sensor.

[0071] Selection of the measurement matrix:

[0072]

[0073] Where F 21 (k)(1:2, 1:2) represents a new matrix composed of rows 1 and 2 and columns 1 and 2 of the submatrix in the Kalman filter at time k, and Δt is the time interval between two Kalman filters.

[0074] The recursive process in 4.2 is as follows:

[0075]

[0076] Where I represents the identity matrix.

[0077] To verify the algorithm, we used the simulation software Matlab to simulate the trajectory of an underwater vehicle and conducted experiments to compare the combined Kalman filter algorithm with the ordinary Kalman filter algorithm.

[0078] The simulation lasted for 1200 seconds. The initial attitude angle of the AUV was set to [0°, 0°, 120°], and the initial velocity was 0. It first moved at a uniform forward velocity of 5 m / s, and then moved periodically in a cycle of 40 seconds. In the first half of the 20-second cycle, it rotated in the direction of increasing heading angle at an angular velocity of 1° / s. In the second half of the 20-second cycle, it rotated in the direction of decreasing heading angle at a angular velocity of 1° / s.

[0079] Add errors to the SINS system. The added data is as follows:

[0080] Table 1 Simulation Error Settings

[0081]

[0082]

[0083] The SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering used in this invention will be compared with the pure SINS navigation scheme, such as... Figure 2 and Figure 3 It was found that the SINS self-assisted navigation scheme based on maneuver constraints and incremental filtering used in this invention, compared with ordinary SINS solution, can limit the speed error to a certain range and has higher navigation and positioning accuracy without adding additional information sources.

[0084] The above description is merely one of the preferred embodiments of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A SINS self-assisted navigation method based on maneuver constraints and incremental filtering, characterized in that, The specific method is as follows: (1) Drive the AUV to perform periodic steady-state motion in the horizontal direction, and add high-frequency components to the velocity in the navigation coordinate system while maneuvering in the specified direction; The method for introducing high-frequency navigation system information in step (1) is as follows: For an AUV, firstly, maintain a constant forward velocity v and move forward in a uniform straight line. Then, while maintaining the forward velocity v, apply a constant driving force to the AUV using the rudder fin propeller, so that it rotates at an angular velocity ω for t seconds while moving forward. Then, apply the opposite driving force to the AUV using the rudder fin propeller, so that it rotates in the opposite direction at an angular velocity of -ω for t seconds. In this way, the ideal navigation system velocity is a triangular wave high-frequency information within one cycle. (2) Based on the maneuvering in step (1), real-time data from the inertial sensors installed on the underwater AUV are obtained, specifically including the output data from the gyroscope and accelerometer of the inertial measurement element, and the attitude calculation algorithm is executed using the sensor data to obtain the velocity in the navigation coordinate system. (3) Set the parameters (f) of the digital high-pass filter. p ,f s A p A s Then, the horizontal velocity in the navigation coordinate system obtained in step (2) is input into the high-pass filter to obtain a set of horizontal velocity quantities in the navigation coordinate system that have eliminated low-frequency error components. (4) Establish a Kalman filter, and use the increment of the velocity component obtained in step (3) as the quantity input of the Kalman filter to compensate for the obtained velocity error, so as to obtain a high-precision positioning value after self-aid compensation.

2. The SINS self-assisted navigation method based on maneuver constraints and incremental filtering according to claim 1, characterized in that: The method for selecting the high-pass filter in step (3) is as follows: S3.1: Select a finite impulse (IIR) filter as the digital filter, where the following parameters need to be set: ω p ω represents the passband cutoff circle frequency, which is set according to the frequency of the periodic maneuver constraint in step (2) and is lower than the frequency of the periodic maneuver; s Represents the stopband cutoff angular frequency, set to 0.0015~0.002; A p Represents the maximum passband attenuation, ranging from 0.1 to 1; A s This represents the minimum stopband attenuation, ranging from 40 to 100. S3.2: The time delay bit depth of the digital filter is obtained by calculating the frequency of the high-frequency maneuver. Then, the time delay bit depth is subtracted from the obtained data to obtain the time delay compensated speed value. The calculation method for the time delay bit depth N is as follows: Among them, T sp The sampling period is The filter's lead time is calculated using the following formula: in, H(z) is the angular frequency of the periodic motion introduced in step (1), and H(z) is the transfer function of the IIR filter in step (3).

3. The SINS self-assisted navigation method based on maneuver constraints and incremental filtering according to claim 1, characterized in that: Step (4) The method for selecting the measurement variables and measurement matrix of the Kalman filter is as follows: Selection of measurement variables: in, and These represent the eastward and northward velocities obtained after passing through the digital filter in step (3), respectively. and The eastward and northward velocities obtained from the inertial element and the inertial calculation algorithm in step (2) are respectively represented by h. SINS This represents the altitude information obtained by the inertial calculation algorithm; h pressure This indicates the altitude information obtained from the altimeter sensor; Selection of the measurement matrix: Where F 21 (k)(1:2, 1:2) represents a new matrix composed of rows 1 and 2 and columns 1 and 2 of the submatrix in the Kalman filter at time k, and Δt is the time interval between two Kalman filters.

Images