Initial alignment method for movable base of underwater vehicle based on multi-navigation-system Kalman filtering

Abstract

The invention discloses an initial alignment method for a movable base of an underwater vehicle based on multi-navigation-system Kalman filtering, and the method comprises the steps: constructing N navigation systems which are uniformly distributed in the azimuth, enabling the included angles between the x axes of the N navigation systems and the x axis of an n system to be gradually increased, and splitting the three-dimensional attitude misalignment angle of an STEKF algorithm model on each navigation system; independently operating the STEKF algorithm on each navigation system to obtain an initial course angle estimated value of the carrier in each navigation system; and according to the initial course angle estimated value in each navigation system, obtaining an optimal navigation system by using a preset screening rule, and obtaining a final alignment result corresponding to the optimal navigation system. According to the method, a plurality of navigation systems covering all-directional angles are constructed, a large azimuth misalignment angle problem is converted into a small misalignment angle estimation problem which can be accurately processed by a linear model, and an optimal result is selected from a parallel filter based on three-dimensional attitude misalignment angle splitting; and the problem that the estimation precision of state transformation Kalman filtering and nonlinear filtering is reduced under the condition of a large misalignment angle can be solved.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation technology, specifically relating to an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering. Background Technology

[0002] Submersible vehicles are crucial for exploring underwater mysteries and developing marine resources. The accuracy and reliability of their navigation systems directly impact the efficiency and success of missions. Since satellite navigation signals are difficult to propagate over long distances underwater, submersibles generally employ autonomous navigation technology based on inertial navigation and aided by various acoustic sensors. The combined navigation system, consisting of a strapdown inertial navigation system (SINS) and a Doppler velocimeter (DVL), is the mainstream and highly efficient solution in submersible navigation. Before the combined navigation system can operate, the initial attitude values ​​of the inertial navigation system must be determined; this process is called initial alignment.

[0003] For underwater vehicles (UVs) docked on shore for navigation preparation, static pedestal alignment technology is quite mature. However, the pre-deployment alignment method limits the UV's rapid response capability. To improve its maneuverability, dynamic pedestal alignment is often used in engineering. The core challenge of dynamic pedestal alignment is that the linear vibration and angular sway of the vehicle itself drown out useful information such as the Earth's rotation angular velocity and gravitational acceleration required for alignment, making accurate estimation of inertial navigation attitude necessary with the assistance of external sensors. Specifically, when using GNSS (Global Navigation Satellite System) for alignment, the UV needs to navigate on the surface, but this compromises stealth. In practical applications, underwater motion alignment is often achieved only with the assistance of DVL (Direct Vehicle Lift), but the observability of its heading angle is usually weak.

[0004] To obtain high-precision alignment results, engineering generally employs a combined strategy of "coarse alignment using an inertial frame + fine alignment using Kalman filtering." After years of practice and refinement, this scheme has become the golden combination for achieving moving base alignment. The main drawback of the traditional "coarse alignment + fine alignment" scheme is that the initial alignment process is split into two sequential stages, resulting in a long alignment time, wasted alignment data, and hindering the rapid switching of the submersible from alignment mode to navigation mode. To address this issue, researchers designed a data storage and backtracking method, using the same alignment data segment simultaneously for both coarse and fine alignment, thereby improving data utilization and shortening alignment time. However, the filtering calculations in the backtracking scheme must be performed intensively at the end of the alignment process after all data acquisition is complete, leading to a surge in computational load at the end of the alignment process. If the navigation computer has limited computing power, this may delay the output of navigation parameters.

[0005] Another research hotspot in moving base alignment is initial alignment with large misalignment angles. This method can reduce the dependence of the Kalman filter on the accuracy of the initial attitude value, thereby greatly shortening the coarse alignment time. Common nonlinear filtering methods such as Unscented Kalman Filter (UKF), Volumetric Kalman Filter (CKF), and Particle Filter (PF) have all been applied to moving base alignment attempts. The computational cost of nonlinear filtering is several times or even tens of times that of the traditional EKF (Extended Kalman Filter) algorithm, which is the main obstacle limiting its widespread use in engineering. The State Transform Kalman Filter (STEKF) algorithm weakens the time-varying nature of the filtering system equations by defining the velocity error in a nonlinear form, and its internal logic is similar to that of the Right Invariant Kalman Filter. Although it cannot completely satisfy the invariant error equation, the STEKF algorithm can also achieve fast convergence of attitude errors under large misalignment angles. In addition, the navigation parameters of the STEKF algorithm are defined in the local geographic coordinate system, which is completely consistent with the standard navigation parameter representation method. However, when faced with extreme cases where the initial heading error is close to ±180°, both nonlinear filtering and invariant filtering significantly reduce the convergence speed of the attitude error, failing to achieve the same alignment accuracy as in cases with small misalignment angles. In other words, there is currently no unified solution for handling situations where the initial heading angle is completely unknown, relying solely on filtering methods. Summary of the Invention

[0006] To address the aforementioned problems in the existing technology, this invention provides an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering. The technical problem to be solved by this invention is achieved through the following technical solution: This invention provides an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering, comprising: S1: Constructed in a uniformly distributed orientation N A navigation system, the N A navigation system x shaft and n Department x The included angles of the axes increase sequentially, and the three-dimensional attitude misalignment angles of the STEKF algorithm model in each navigation system are decomposed; S2: Run the STEKF algorithm independently in each navigation system to obtain the estimated initial heading angle of the vehicle in each navigation system at the current moment; S3: Based on the initial heading angle estimate in each navigation system, obtain the optimal navigation system using a preset filtering rule and obtain the final alignment result corresponding to the optimal navigation system.

[0007] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention discloses an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering. Addressing the issues of traditional "coarse alignment + fine alignment" methods requiring data storage and backtracking, leading to a surge in computational load at the end of alignment, and the decreased estimation accuracy of State Transform Kalman Filtering (STEKF) and nonlinear filtering under large misalignment angles, this invention designs a multi-navigation system Kalman filtering method. By constructing multiple navigation systems covering all omnidirectional angles, the large azimuth misalignment angle problem is transformed into a small misalignment angle estimation problem that can be accurately handled by a linear model. Based on three-dimensional attitude misalignment angle decomposition, an optimal navigation system selection mechanism is proposed to select the optimal result from parallel filters. This overcomes the problem of decreased estimation accuracy under large misalignment angles for both State Transform Kalman Filtering and nonlinear filtering.

[0008] 2. To reduce computational load, this invention proposes an efficient inertial navigation integral data reuse mechanism. This mechanism decouples the computationally intensive sensor data integration process from navigation parameter updates, allowing all filters to share the same set of integration results for navigation parameter updates. This reduces computational load while maintaining performance. Monte Carlo simulation and experimental results show that this method can achieve stable and high-precision attitude angle estimation under initial azimuth misalignment angles of different magnitudes, and no position error correction is required at the end of alignment. This provides an effective technical path for improving the maneuverability and stealth deployment capabilities of underwater vehicles.

[0009] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0010] Figure 1 This is a flowchart of an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering, provided by an embodiment of the present invention; Figure 2 This is a schematic diagram of a multi-navigation system provided in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the selection of an optimal coordinate system provided by an embodiment of the present invention; Figures 4a to 4c This is an RMSE curve of the attitude error of different algorithms in the first set of simulation experiments provided by the embodiments of the present invention; Figures 5a to 5c This is an RMSE curve of the attitude error of different algorithms in the second set of simulation experiments provided in this embodiment of the invention; Figures 6a to 6c This is the RMSE curve of the attitude error of different algorithms in the third set of simulation experiments provided in the embodiments of the present invention. Detailed Implementation

[0011] To further illustrate the technical means and effects adopted by the present invention to achieve the intended purpose, the following describes in detail, with reference to the accompanying drawings and specific embodiments, a method for initial alignment of a moving base of a submarine based on multi-navigation system Kalman filtering proposed in accordance with the present invention.

[0012] The foregoing and other technical contents, features, and effects of the present invention will be clearly presented in the following detailed description of specific embodiments in conjunction with the accompanying drawings. Through the description of the specific embodiments, a more in-depth and concrete understanding can be gained of the technical means and effects adopted by the present invention to achieve its intended purpose. However, the accompanying drawings are for reference and illustration only and are not intended to limit the technical solutions of the present invention.

[0013] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations are intended to cover non-exclusive inclusion, such that an article or apparatus comprising a list of elements includes not only those elements but also other elements not expressly listed. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the article or apparatus that includes said element.

[0014] Please see Figure 1 , Figure 1 This is a flowchart of an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering, provided by an embodiment of the present invention. The method includes the following steps: S1: Constructed in a uniformly distributed orientation N There are several navigation systems, namely navigation coordinate systems. x shaft and n Department x The included angles of the axes increase sequentially, and the three-dimensional attitude misalignment angles of the STEKF algorithm model in each navigation system are decomposed.

[0015] Specifically, let the angle between the initial orientation of the submersible and the north direction be denoted as . In respectively N Filtering is performed independently on each navigation system, with each navigation system using... In other words, that is, Indicates the first i A navigation system. The coordinate system is the North-East-Down (NED) coordinate system. n The first navigation system, in other words. and System overlap. Each pair of adjacent navigation systems... x The included angle between the axes is: (1) System (i.e., the first) i (a navigation system) and n The attitude transformation relationship between the systems is as follows: (2) in, Indicates the first i Navigation system arrive n The coordinate transformation matrix of the system.

[0016] Please see Figure 2 , Figure 2 This is a schematic diagram of a multi-navigation system provided by an embodiment of the present invention. Among all navigation systems, there must be one navigation system... x The navigation frame with the smallest angle between the axis and the initial orientation of the submersible is the one that best satisfies the small-angle assumption of the initial three-dimensional attitude misalignment angle. Therefore, the attitude angle obtained by filtering based on this navigation frame has the highest accuracy. To establish a filtering mechanism and select this navigation frame, the strategy adopted in this invention is to decompose the three-dimensional attitude misalignment angle of the STEKF algorithm model on each navigation frame as follows: (3) in, Indicates in The current three-dimensional attitude misalignment angle in the system. Indicates in The three-dimensional attitude misalignment angle at the initial moment in the system. Indicates in The change in the three-dimensional attitude misalignment angle from the initial moment to the current moment in the system, with its initial state being 0.

[0017] S2: Run the STEKF algorithm independently in each navigation system to obtain the estimated initial heading angle of the vehicle (i.e., the underwater vehicle) in each navigation system at the current moment.

[0018] Specifically, step S2 in this embodiment includes: S2.1: Based on the three-dimensional attitude misalignment angle of each navigation frame, obtain the system equations of the filters in each navigation frame.

[0019] In traditional initial alignment and integrated navigation algorithms, in order to convert the attitude error into a vector representation, a first-order Taylor expansion of the attitude error matrix is ​​usually performed to obtain the three-dimensional attitude misalignment angle. Its definition and differential equation are as follows: (4) (5) in, The carrier coordinate system (i.e., the carrier coordinate system, also known as the carrier coordinate system) is a system that represents the carrier coordinate b (system) to n The coordinate transformation matrix of the system. Indicates the system to n The calculated value of the direction cosine matrix of the system in the navigation computer. Represents a third-order identity matrix. Indicates in n The current three-dimensional attitude misalignment angle in the system. express antisymmetric matrix, express The differential, express n The system is relative to the geocentric inertial coordinate system ( i The rotational angular velocity of the system is in n The projection of the system, express antisymmetric matrix, express n The calculation error of the navigation system is specifically expressed as follows: n System relative to i The rotational angular velocity of the system is n The calculation error of the projection of the system. This indicates that the gyroscope has zero bias. This indicates a random walk of the gyroscope. In the description of this invention, quantities marked with "~" represent their calculated values ​​in the navigation computer. Represents the antisymmetric matrix of a vector.

[0020] During filtering and All are modeled as state vectors, obtained after three-dimensional attitude misalignment angles. Characterizing the initial state, that is, in The three-dimensional attitude misalignment angle at the initial moment in the system does not change with time, therefore: (6) in, express The differential.

[0021] Based on equations (5) and (6), we can obtain The differential equation is: (7) in, express The differential, express n System relative toi The rotational angular velocity of the system is The projection of the system, express antisymmetric matrix, express The calculation error of the navigation system is as follows: n System relative to i The rotational angular velocity of the system is The calculation error of the projection of the system. Indicates the system to n i The coordinate transformation matrix of the system.

[0022] Furthermore, The expression is as follows: (8) (9) (10) in, Indicates the Earth's rotational angular velocity at The projection of the system, express n The system is relative to the geocentric coordinate system ( e The rotational angular velocity of the system is in The projection of the system; Indicates from n Tie The coordinate transformation matrix of the system. Represents the Earth's angular velocity of rotation. Indicates the latitude of the carrier. h Indicates the height of the carrier. , h and the longitude of the carrier Together they constitute the position of the STEKF algorithm ,Right now ; and These represent the radius of curvature of the meridian and the radius of curvature of the trochanter, respectively. The velocity of the carrier, that is, the velocity of the carrier relative to the Earth's coordinate system. n The projection of the system; , , , These are the northward, eastward, and celestial velocities of the carrier, respectively. The velocity of the carrier relative to the Earth's coordinate system is The projection of the system. As an intermediate variable, its expression is: (11) Next, and Taking the first-order partial derivatives with respect to the navigation parameters (including attitude, velocity, and position), we obtain: (12) (13) in, This indicates the positional error of the carrier. , , These represent longitude error, latitude error, and altitude error, respectively. express n i The calculation error of the Earth's rotation angular velocity is considered. express n i The calculation error of the rotational angular velocity under the system, express n i The speed error of the carrier under the system. and All are intermediate variables, and their expressions are: (14) (15) Furthermore, based on the STEKF algorithm, this invention uses vector... Indicates position, that is, the amount of change in the position of the carrier relative to the starting point. n i Projection of the system. Positional error of the carrier. The conversion relationship between the error states used in this invention and the actual error states is as follows: (16) in, Indicates position The corresponding error, This represents the calculated position of the vehicle within the navigation computer. This represents the calculated value of the location in the navigation computer. Rotate to real n i The error included after the system, As an intermediate variable, its expression is as follows: (17) In equation (7) Using the attitude error, velocity error, and position error from the STEKF algorithm, we can obtain: (18) After state amplification, The velocity error differential equation in the system is: (19) in, This indicates that the calculated value of the vehicle's speed in the navigation computer rotates to the actual value. The error included after the system, express The differential, Represents gravitational acceleration. express n i The download speed is calculated in the navigation computer. Indicates from b Tie n i The coordinate transformation matrix of the system. This indicates that the accelerometer has zero bias. This indicates a random walk of the accelerometer.

[0023] The differential equation for the position error in the system is: (20) in, express The differential, express Calculated values ​​in the navigation computer.

[0024] but The state definition of the STEKF algorithm in the system is as follows: (twenty one) Combining equations (6), (7), (18), (19), and (20), we can obtain The system equations for the filter in the system are as follows: (twenty two) (twenty three) (twenty four) in, express The differential, Here is the state transition matrix. For noise driving matrix, For process noise, express A matrix of all zeros express A matrix of all zeros express A matrix of all zeros; , , , , , , , and These are all intermediate variables, and their expressions are as follows: (25a) (25b) (25c) (25d) (25e) (25f) (25g) (25h) (25i) In each filter, the initial filter value is set to: (26a) (26b) (26c) (26d) in, Indicates from b Tie The calculated value of the coordinate transformation matrix of the system in the navigation computer. express The value at the initial moment, express The initial value of the variance, Representing state The initial value of the variance. S2.2: Within adjacent filtering cycles, the inertial sensor data output by the strapdown inertial navigation system is integrated uniformly. After the measurement update time is reached, the navigation parameters of all filters are updated in parallel using the integration result.

[0025] Because it needs to run NEach independent filter in a multi-navigation system Kalman filter faces the problem of a doubling of computational load due to parallel computing. For example, running 36 filters results in a computational load 36 times that of a traditional single filter, which is unacceptable in practical engineering. To address this issue, this invention designs an efficient inertial navigation data integration method. All filters can completely share a set of incremental velocity / angle integration results from SINS (Straightthrough Inertial Navigation System) for navigation parameter updates between two filtering cycles, thereby... N The integral calculation process, which used to be multiple times, is now reduced to only one iteration, greatly reducing the computational load of multi-navigation system filtering.

[0026] In other words, in multi-sensor fusion navigation systems (such as INS / DVL integrated navigation), to improve computational efficiency, time is divided into multiple "filtering cycles" (i.e., the time interval between two measurement updates). Within adjacent filtering cycles, the raw data (specific force, angular velocity) from the inertial sensors (IMU, including accelerometers and gyroscopes) are first integrated uniformly (rather than integrating individually for each filter) to generate intermediate results (such as position increment, velocity increment, and attitude increment). When the preset "measurement update time" (such as the time when DVL data is available) is reached, the integration result is used directly to update the navigation parameters (such as position, velocity, and attitude) of all filters in parallel. This avoids redundant integration calculations, reduces computational load, and is especially suitable for multi-filter architectures (such as multiple sub-filters working in parallel in a federated filter).

[0027] Specifically, let the measurement update period of the Kalman filter be... T Initial alignment is typically taken in applications During the measurement update cycle T The internal task is to complete the integral calculation of navigation parameters corresponding to each filter and the time update of the covariance matrix. If the data frequency of the strapdown inertial navigation system is 100... Hz If the single-sample inertial navigation system solution method is used, each 0.01 s A dead reckoning needs to be performed. To isolate the sensor-data-based integral calculations in each filter, in During the filtering period, the load system is... coordinate transformation matrix of the system By decomposing, we can obtain and The relationship is: (27) in, Indicates the period within the filter cycle The pose matrix at time step, i.e. From time to time b Tie The coordinate transformation matrix of the system. express The pose matrix at time step, Indicates from Time and The inertial connection of the fixed link time The coordinate transformation matrix of the system. Indicates from Time and b The inertial frame of the system coincides. Time and The coordinate transformation matrix of a fixed inertial frame. Indicates from time b Tie Time and b The coordinate transformation matrix of coincident inertial frames; Indicates within the filtering period Regarding the rotational change of the system, which is extremely small, the following approximation holds: (28) in, express time n Relative e The rotational angular velocity of the system is n The projection of the system, express An antisymmetric matrix.

[0028] The calculation formula is: (29) in, Indicates from Time e-system time The coordinate transformation matrix of the system. Indicates from The inertial frame that is always fixed to the e-frame The coordinate transformation matrix of the time frame e. Indicates from Time and The inertial connection of the fixed link The coordinate transformation matrix of the inertial frame fixed to the e-frame at time t. express From the e-series to The coordinate transformation matrix of the system. express From time to time The coordinate transformation matrix from the e-frame to the e-frame. express The coordinate transformation matrix from the n-frame to the e-frame at any given time. express The coordinate transformation matrix from the e-frame to the n-frame at any given time. and The calculation method is as follows: (30) (31) in, , They represent The longitude and latitude of the carrier.

[0029] Set time , Indicates from Time B system Time and b The coordinate transformation matrix of an inertial frame that coincides with another frame. The calculations rely solely on gyroscope measurement data, and The specific values ​​of the navigation parameters in the system are irrelevant; their differential equations and initial values ​​are as follows: (32) (33) in, express The differential, This indicates that the carrying system (b-frame) is relative to the geocentric inertial coordinate system (b-frame). i The projection of the rotational angular velocity of the system onto the geocentric inertial coordinate system. express The antisymmetric matrix; Indicates from time b Tie Time and b The coordinate transformation matrix of an inertial frame that coincides with another frame.

[0030] Based on equations (32) and (33), in By performing integral calculations within the range, we can obtain the result from... time b Tie Time and b Coordinate transformation matrix of coincident inertial frames The value of .

[0031] In summary, the gyroscope data integration process can be isolated by using the attitude matrix decomposition form shown in equation (27). and The value can be determined solely by The navigation parameters at any given time are obtained through step-by-step calculation. The calculation relies entirely on the integral extrapolation of sensor data; this decomposition mechanism ensures that the two are computationally decoupled. Therefore, Available for N The attitude matrix of each filter is updated, thus avoiding the duplication of the integral calculation process. N all over.

[0032] Next, we derive the decoupling process for the velocity integral calculation. The velocity integral calculation can be converted into the following form: (34) in, express The velocity of the carrier relative to the Earth's coordinate system at any given time The projection of the system, express The velocity of the carrier relative to the Earth's coordinate system at any given time The projection of the system, Indicates the specific force of the carrier. express At any given moment, the Earth's rotational angular velocity is The projection of the system, express time n The angular velocity of rotation of the system relative to the geocentric coordinate system is The projection of the system.

[0033] Coriolis acceleration and entrainment acceleration are both small quantities and change relatively slowly. Gravitational acceleration can be considered constant during alignment. Therefore, the velocity changes introduced by these three factors within the filtering period can be based solely on... Navigation parameters are calculated and updated step-by-step, while the integral calculation of the relative force must keep pace with the high-frequency rhythm of sensor data. The integral calculation of the relative force can be transformed into the following form: (35) in, Indicates from Time's up time The rotational changes of the system, This represents the projection of the Earth's rotational angular velocity onto the Earth system. express An antisymmetric matrix.

[0034] Therefore, only one needs to be completed within the filtering cycle. and The integral calculation of these two inertial data can then be performed in each navigation system by... The navigation parameters are updated at each moment. The speed of time.

[0035] Furthermore, the decoupling process for position integral derivation is as follows: (36) in, express The position, that is, the change in position of the carrier relative to the starting point. n i The projection of the system, express Location, time , Indicates from Time B system Time and b The coordinate transformation matrix of an inertial frame that coincides with another frame.

[0036] From the above formula, it can be seen that based on inertial data and These two double integral calculations can then be updated to obtain... The location of the time carrier in each navigation system.

[0037] S2.3: Using the Doppler velocimeter (DVL) velocity measurement as the observation, after filtering to obtain the optimal estimate of the error state, simultaneously... and Corrections were made, and the corrected values ​​were obtained respectively. and The correction process is as follows: (37a) (37b) During the alignment process, each filter can be solved in real time. That is, the representation of the vehicle's attitude in each navigation frame at the initial moment. The initial heading angle estimate of the carrier in each navigation system can be obtained by conversion. This value represents the initial orientation of the submarine. Tie x The included angle of the axis.

[0038] S3: Based on the initial heading angle estimate in each navigation system, obtain the optimal navigation system using a preset filtering rule and obtain the final alignment result corresponding to the optimal navigation system.

[0039] Specifically, to enhance the robustness of the screening criteria, the screening strategy designed in this invention is as follows: Figure 3 As shown. For a certain navigation system Its two adjacent navigation systems are respectively and Let the absolute values ​​of the estimated initial heading angles of the submarine calculated in the three navigation systems be respectively... , and If the three angles can satisfy: (38) Then, the current navigation system This is the optimal navigation system.

[0040] After filtering, the current navigation system will be... The navigation parameters calculated in the middle are converted to n The system is used as the final alignment result output.

[0041] The following Monte Carlo simulation experiments comprehensively evaluate the performance of the proposed method in initial alignment. For ease of representation, the STEKF-based multi-navigation Kalman filter algorithm used in this invention is abbreviated as MSTEKF. To compare the performance of different algorithms and examine the number of navigation frames... N To assess the impact of MSTEKF algorithm-based alignment methods on accuracy and computational cost, this embodiment compares the following seven alignment methods.

[0042] EKF: Extended Kalman Filter, a traditional precision alignment filtering method; STEKF: State Transition Kalman Filter; STUKF: The filtered state vector is consistent with the STEKF algorithm, except that it does not use a first-order approximation when establishing the filtering equation, but instead calculates the probability distribution propagation based on the UT transform; MSTEKF4: The method proposed in this invention, the number of navigation systems N The value of is 4; MSTEKF9: The method proposed in this invention N The value of is 9; MSTEKF 18 The method proposed in this invention, N The value is 18; MSTEKF 36 The method proposed in this invention, N The value is 36.

[0043] Three Monte Carlo simulation experiments were designed, with 100 independent simulations performed in each group. The initial position of the submersible was set as (latitude 34.343207°, longitude 108.939645°, elevation 0 m).

[0044] As shown in Table 1, the initial attitude angles of the submersible were set to random values ​​within a specific range in each simulation. The difference between the three sets of experiments lies in the different ranges of the initial heading angle settings to simulate different degrees of azimuth misalignment. Each algorithm sets the initial attitude angle and the three-dimensional attitude misalignment angle to 0 when starting the initial alignment calculation, indicating that the initial values ​​are completely unknown. The alignment time was set to 10 minutes for all simulations. During the alignment period, the submersible randomly performed maneuvers such as uniform speed, uniform acceleration, uniform deceleration, left turns, or right turns to simulate real motion. The sensor parameter settings in the simulation are shown in Table 2.

[0045] Table 1. Range of initial attitude angles in different experimental groups

[0046] Table 2 Sensor Performance Parameter Settings

[0047] In each simulation experiment, the zero bias of the gyroscope and accelerometer is set to a random value within the range of values ​​to simulate the random change of the zero bias value each time the device is powered on.

[0048] The root mean square error (RMSE) curves of attitude estimation errors for each algorithm in the first group of 100 Monte Carlo simulations. Figures 4a to 4c ,in, Figure 4a This is the RMSE curve of the roll angle error in the first set of simulation experiments. Figure 4b This is the RMSE curve of the pitch angle error in the first set of simulation experiments. Figure 4c This is the RMSE curve of the yaw angle error in the first set of simulation experiments. (In the first 110...) s Four MSTEKF algorithms (MSTEKF4, MSTEKF9, MSTEKF...) 18 and MSTEKF 36 The RMSE curves of all three navigation systems showed varying degrees of oscillation. This is because, in the initial alignment phase, the convergence trend of heading errors in each navigation system was not yet obvious, and the information fused by each filter was too limited, making it impossible for the selection criteria to stably determine the optimal navigation system. s Subsequently, the selection criteria can accurately identify the filter with the smallest initial heading error, and the RMSE curves of each MSTEKF algorithm tend to stabilize.

[0049] The pitch and roll angle alignment accuracy of STEKF, STUKF, and MSTEKF is slightly higher than that of EKF, but the horizontal angle error RMSE of all algorithms converges to within 0.01°, and the difference is not significant. In terms of heading angle alignment accuracy, STEKF, STUKF, MSTEKF9, and MSTEKF... 18 and MSTEKF 36The accuracy is basically the same, followed by MSTEKF4, with EKF having the lowest accuracy. MSTEKF4 divides the 360° heading area into four equal parts. The maximum initial heading error in the system is 45°. However, in MSTEKF9 and MSTEKF... 18 and MSTEKF 36 In the algorithms, the maximum initial heading errors are 20°, 10° and 5° respectively. The initial heading uncertainty faced by the STEKF and STUKF algorithms is 20°. Therefore, the alignment accuracy of these five methods is similar.

[0050] Table 3 lists the average horizontal position error at the end of alignment and the average time taken to complete the alignment calculation for different algorithms in 100 simulations. The experimental computer had an Intel Core i7-9750H CPU and 16GB of memory, and the simulation software was MATLAB R2024b. As shown in Table 3, the horizontal positioning errors at the end of alignment are not significantly different among the different algorithms. Because the EKF algorithm and the filtering method based on the left-invariant error of the Lie group contain ratio terms in their model structure parameters, the calculation frequency of the covariance matrix needs to be consistent with the IMU data frequency to fully capture the system dynamics. Therefore, in the simulation experiment, the covariance update frequency of EKF was 100Hz, while that of other algorithms was 1Hz. Comparing the time taken by EKF and STEKF shows that reducing the update frequency of the covariance matrix can significantly reduce the computational burden. MSTEKF 36 Although the computational cost is greater than that of STEKF, it is far less than 36 times that of STEKF and is lower than that of EKF, which uses high-frequency solutions.

[0051] Table 3. Position error and alignment time of different algorithms at the end of the alignment in the first set of simulations.

[0052] The RMSE curves for the pose estimation in the second set of simulations are as follows: Figures 5a to 5c As shown, where, Figure 5a This is the RMSE curve of the roll angle error in the second set of simulation experiments. Figure 5b This is the RMSE curve of the pitch angle error in the second set of simulation experiments. Figure 5c The RMSE curves for yaw angle error in the second set of simulation experiments are shown. The mean horizontal position error at the end of alignment and the average time consumed by each algorithm are shown in Table 4. When the initial heading error increases to 40° to 60°, the alignment accuracy of EKF, STEKF, and STUKF all decreases. Regarding heading estimation, MSTEKF9 and MSTEKF... 18 and MSTEKF 36 It maintained a similar level of accuracy as the first set of experiments, while the accuracy of STUKF degraded to be close to that of MSTEKF4.

[0053] Table 4. Position error and alignment time of different algorithms at the end of the alignment in the second set of simulations.

[0054] The attitude estimation RMSE curves and related performance indicators of the third set of simulations are as follows: Figures 6a to 6c And as shown in Table 5, Figure 6a This is the RMSE curve of the roll angle error in the second set of simulation experiments. Figure 6b This is the RMSE curve of the pitch angle error in the second set of simulation experiments. Figure 6c This shows the RMSE curve of the yaw angle error in the second set of simulation experiments. Combining the simulation results from all three sets, it can be seen that as the initial misalignment angle increases, the alignment accuracy of both STEKF and STUKF decreases, but the accuracy degradation of STUKF is more gradual, and its position error remains at a low level throughout. In the three sets of experiments, MSTEKF9 and MSTEKF... 18 and MSTEKF 36 All maintained good alignment accuracy, proving that converting the initial azimuth misalignment angle into a small angle by setting multiple navigation systems is effective.

[0055] Table 5. Position error and alignment time of different algorithms at the end of the alignment in the third group of simulations.

[0056] This invention discloses an initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering. It addresses the problems of traditional "coarse alignment + fine alignment" methods, which require data storage and backtracking, leading to a surge in computational load at the end of alignment. It also addresses the issues of decreased estimation accuracy in State Transform Kalman Filtering (STEKF) and nonlinear filtering under large misalignment angles. This invention designs a multi-navigation system Kalman filtering method. By constructing multiple navigation systems covering all omnidirectional angles, it transforms the large azimuth misalignment angle problem into a small misalignment angle estimation problem that can be accurately handled by a linear model. Based on three-dimensional attitude misalignment angle decomposition, it proposes an optimal navigation system selection mechanism to select the best result from parallel filters. This overcomes the problem of decreased estimation accuracy in State Transform Kalman Filtering and nonlinear filtering under large misalignment angles.

[0057] To reduce computational load, this invention proposes an efficient inertial navigation integral data reuse mechanism. This mechanism decouples the computationally intensive sensor data integration process from navigation parameter updates, allowing all filters to share the same set of integration results for navigation parameter updates. This reduces computational load while maintaining performance. Monte Carlo simulation and field experimental results demonstrate that this method achieves stable and high-precision attitude angle estimation under initial azimuth misalignment angles of different magnitudes, and eliminates the need for position error correction at the end of alignment. This provides an effective technical approach for improving the maneuverability and stealth deployment capabilities of underwater vehicles.

[0058] Another embodiment of the present invention provides a storage medium storing a computer program for executing the steps of the initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering described in the above embodiments. A further aspect of the present invention provides an electronic device including a memory and a processor. The memory stores a computer program, and the processor, when calling the computer program in the memory, implements the steps of the initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering as described in the above embodiments. Specifically, the integrated modules implemented as software functional modules can be stored in a computer-readable storage medium. These software functional modules, stored in a storage medium, include several instructions to cause an electronic device (which may be a personal computer, server, or network device, etc.) or processor to execute some steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0059] The above description, in conjunction with specific preferred embodiments, provides a further detailed explanation of the present invention. It should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, various simple deductions or substitutions can be made without departing from the concept of the present invention, and all such modifications and substitutions should be considered within the scope of protection of the present invention.

Claims

1. A method for initial alignment of a moving base of a submersible based on multi-navigation system Kalman filtering, characterized in that, include: S1: Constructed in a uniformly distributed orientation N A navigation system, the N A navigation system x shaft and n Department x The included angles of the axes increase sequentially, and the three-dimensional attitude misalignment angles of the STEKF algorithm model in each navigation frame are decomposed. n The coordinate system is northeast-northeast. S2: Run the STEKF algorithm independently in each navigation system to obtain the estimated initial heading angle of the vehicle in each navigation system at the current moment; S3: Based on the estimated initial heading angle in each navigation system, obtain the optimal navigation system using a preset filtering rule and obtain the final alignment result corresponding to the optimal navigation system.

2. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 1, characterized in that, S1 includes: Let the angle between the initial orientation of the underwater vehicle and the north direction be denoted as . Constructed in a uniformly distributed orientation N The first navigation system and Departments overlap; get System and n Attitude transformation relationships between systems: ， in, The series indicates the first i A navigation system. express Tie n The coordinate transformation matrix of the system. Indicates two adjacent navigation systems x Angle between axes; The 3D attitude misalignment angles of the STEKF algorithm model in each navigation frame are decomposed: ， in, Indicates in The current three-dimensional attitude misalignment angle in the system. Indicates in The three-dimensional attitude misalignment angle at the initial moment in the system. Indicates in The change in the three-dimensional attitude misalignment angle from the initial moment to the current moment in the system.

3. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 2, characterized in that, S2 includes: S2.1: Based on the three-dimensional attitude misalignment angle of each navigation system, obtain the system equations of the filters in each navigation system; S2.2: Within adjacent filtering cycles, the inertial sensor data output by the strapdown inertial navigation system is integrated uniformly. After reaching the measurement update time, the navigation parameters of all filters are updated in parallel using the integration results. S2.3: Using the Doppler velocimeter's velocity values ​​as observations, filtering is performed to obtain the optimal estimate of the error state. The carrier attitude at the initial moment for each navigation system is corrected, and the corrected values ​​are transformed to obtain the initial orientation of the submarine in each navigation system. Tie x The included angle of the axis.

4. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 3, characterized in that, The system equations for the filter in the system are: ， in, express The differential, express The state of the STEKF algorithm in the system, Here is the state transition matrix. For noise driving matrix, This is process noise.

5. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 4, characterized in that, The state of the STEKF algorithm in the system The expression is: ， in, This means rotating the calculated value of the vehicle's speed in the navigation computer to the actual value. The error included after the system, This means rotating the calculated value of the vehicle's position in the navigation computer to the actual value. n i The error included after the system, This indicates that the gyroscope has zero bias. This indicates that the accelerometer has zero bias. This represents the transpose of a matrix.

6. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 4, characterized in that, The state transition matrix and the noise driving matrix The expressions are as follows: ， ， in, express A matrix of all zeros express A matrix of all zeros express b Tie n i The coordinate transformation matrix of the system. express n i The download speed is calculated in the navigation computer. express n i The calculated value of the system's location in the navigation computer; , , , , , , , and All are intermediate variables. b The system represents the coordinate system of the carrier.

7. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 6, characterized in that, intermediate variables , , , , , , , and The expressions are as follows: ， ， ， ， ， ， ， ， ， in, express n System relative to i The rotational angular velocity of the system is The projection of the system, express antisymmetric matrix, express n Tie The coordinate transformation matrix of the system. express Tie n The coordinate transformation matrix of the system. express antisymmetric matrix, This represents the calculated position of the vehicle within the navigation computer. express antisymmetric matrix, Represents gravitational acceleration antisymmetric matrix, Indicates the Earth's rotational angular velocity at System projection antisymmetric matrix, express n System relative to e The rotational angular velocity of the system is System projection antisymmetric matrix, i The system is a geocentric inertial coordinate system; , , and All are intermediate variables, and their expressions are as follows: ， ， ， ， in, Indicates the latitude of the carrier. h Indicates the height of the carrier. and These represent the radius of curvature of the meridian and the radius of curvature of the trochanter, respectively. Represents the Earth's angular velocity of rotation. , These represent the northward and eastward velocities of the carrier, respectively.

8. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 7, characterized in that, S2.2 includes: Let the measurement update period of the Kalman filter be... T ,exist During the filtering period, the load system is... coordinate transformation matrix of the system Decompose to obtain and The relationship is: ， in, Indicates the period within the filtering cycle From time to time b Tie The coordinate transformation matrix of the system. express From time to time b Tie The coordinate transformation matrix of the system. Indicates from Time and The inertial connection of the fixed link time The coordinate transformation matrix of the system. Indicates from Time and b The inertial frame of the system coincides. Time and The coordinate transformation matrix of a fixed inertial frame. Indicates from time b Tie Time and b The coordinate transformation matrix of coincident inertial frames; Indicates within the filtering period The rotational changes of the system; Set time , Indicates from Moment b Tie Time and b The coordinate transformation matrix of coincident inertial frames. The differential equation is: ， in, express The differential, express b Relative i The rotational angular velocity of the system is i The projection of the system, express The antisymmetric matrix; The initial value is ; based on Differential equations in Integral calculations are performed within the range to obtain the result from... time b Tie Time and b Coordinate transformation matrix of coincident inertial frames The value; The decoupling process of velocity integral derivation is transformed into the following form: in, express Time carrier relative to e The speed of the system is The projection of the system, express Time carrier relative to e speed at The projection of the system, Indicates the specific force of the carrier. express At any given moment, the Earth's rotational angular velocity is The projection of the system, express time n System relative to e The rotational angular velocity of the system is The projection of the system, e The system represents the geocentric coordinate system; Completed within the filtering cycle and The integral calculation yields the value of each navigation system by... The navigation parameters are updated at each moment. The speed of time; The decoupled process of position integral derivation is transformed into the following form: ， in, express The position at a given moment, that is, the change in position of the carrier relative to the starting point. n i The projection of the system, express Location at any given moment Indicates from time b Tie Time and b The coordinate transformation matrix of the coincident inertial frames, at time... ; In each navigation system, we obtain The system integrates the position, velocity, and attitude of the carrier at all times, and uses the integrated results to update the navigation parameters of all filters in parallel.

9. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to claim 6, characterized in that, S2.3 includes: Using Doppler velocimeters as the observed value, the error state of the navigation system is estimated through a filtering algorithm. After obtaining the optimal estimate of the error state, the system is then... and Corrections were made, and the corrected values ​​were obtained respectively. and The correction process is as follows: ， ， in, Indicates from b Tie The calculated value of the coordinate transformation matrix of the system in the navigation computer. express The value at the initial moment; In the filters of each navigation system, for The conversion is performed to obtain the estimated initial heading angle of the carrier in each navigation system. That is, the initial orientation of the submarine and Tie x The included angle of the axis.

10. The initial alignment method for a moving base of a submarine based on multi-navigation system Kalman filtering according to any one of claims 1 to 9, characterized in that, S3 includes: For the current navigation system , Its two adjacent navigation systems are respectively and Let the absolute values ​​of the estimated initial heading angles of the submarine calculated in the three navigation systems be respectively... , and If the following conditions are met: ， Then determine the current navigation system The optimal navigation system; Current navigation system The navigation parameters calculated in the middle are converted to n The system is used as the final alignment result output.

Images