A launch system inertial navigation satellite guidance combined method based on quaternion error model

Abstract

The application relates to the technical field of combined navigation, in particular to a launch system inertial navigation satellite navigation combined method based on a quaternion error model, which comprises the following steps: establishing a quaternion error model; establishing an inertial navigation model and an inertial navigation error model under a launch system; establishing an inertial navigation satellite navigation combined navigation model and a combined navigation error model based on the quaternion error model; starting an inertial navigation system, completing preparation work including air alignment according to a conventional work flow and a work mode of the inertial navigation system, until an aircraft enters an inertial navigation state; updating time-varying values in a measurement error model, the inertial navigation error model and the combined navigation error model in real time according to navigation parameters of the inertial navigation system, and performing time updating; when satellite navigation signals are effective, performing satellite navigation measurement updating; and correcting the inertial navigation system according to error estimation results of Kalman filtering. The method effectively improves the stability and precision of the launch system inertial navigation satellite navigation combination.

Description

Technical Field

[0001] The embodiments of this application relate to the field of integrated navigation technology, and in particular to a launch system inertial navigation satellite navigation integration method based on a quaternion error model. Background Technology

[0002] Strapdown Inertial Navigation Systems (SINS) possess outstanding advantages such as complete autonomy, short-term high accuracy, and resistance to external electromagnetic interference, making them the core navigation equipment for various aircraft and spacecraft. Global Navigation Satellite Systems (GNSS) can provide long-term stable absolute position and velocity observation information. The loosely coupled combination of SINS and GNSS can complement each other's weaknesses and is the mainstream navigation solution in launch scenarios. Currently, in engineering applications, SINS-GNSS combined filtering commonly employs a Kalman filter framework based on a small misalignment angle error model using Euler angles and rotation vectors. The derivation forcibly introduces the assumption of small-angle perturbation, assuming that the carrier's attitude misalignment angle is extremely small. After first-order Taylor truncation of the attitude differential equation, higher-order nonlinear terms are ignored, thereby linearizing the system state equation and significantly reducing the complexity of the filter solution. This allows for meeting basic navigation accuracy requirements under initial precise alignment and stable low-dynamic flight conditions.

[0003] However, launch scenarios present unique challenges that differ significantly from those of conventional aircraft carriers. At the moment of launch, there are no sufficiently precise alignment conditions. The combined effects of pre-launch coarse alignment, ground reference deviations, and initial attitude disturbances can easily generate large misalignment angles on the order of tens of degrees. Simultaneously, the launch phase involves strong overload, high-speed rotation, and violent maneuvers, resulting in rapid changes in attitude angles and large attitude disturbances. In these conditions, the assumptions of traditional linear error models with small misalignment angles no longer hold. Neglected second-order and higher-order nonlinear terms become fixed model errors, directly causing bias in filtered state estimation. Attitude and velocity errors diverge rapidly, severely weakening the effectiveness of integrated navigation corrections.

[0004] Existing improved solutions for large misalignment angles mostly employ multiplicative quaternion error models. While these models overcome the singularity constraint of Euler angles, the iterative update logic of multiplicative errors is complex, and the error differential equations have numerous coupling terms. When solving in the launch coordinate system (hereinafter referred to as the launch system), additional multi-dimensional matrix transformations between the launch system, the geocentric coordinate system, and the carrier coordinate system are required, significantly increasing the computational load and placing a heavy burden on embedded hardware for real-time calculation. Other teams have used nonlinear filtering algorithms such as unscented Kalman filtering and particle filtering to adapt to large misalignment angle scenarios, but these methods involve a large number of sampling points and complex matrix operations, making it difficult for small airborne processors to support high-frequency navigation calculations, resulting in poor engineering feasibility.

[0005] Furthermore, the launch system is fixed to the Earth and is affected by the Earth's rotation, local gravitational field, and geographical parameters of the launch point. This results in significant differences in the propagation patterns of velocity and position errors compared to the geocentric-fixed coordinate system. Existing large misalignment error models are all derived in the geocentric-fixed coordinate system. Directly transferring these models to launch system navigation requires repeated coordinate system error transformations, introducing additional transformation errors and failing to accurately characterize the coupled propagation characteristics of attitude, velocity, position, and inertial device drift during launch. Simultaneously, traditional linear models struggle to effectively suppress calculation errors caused by conical motion and rowing motion during the high-dynamic launch process, further reducing the stability of navigation output.

[0006] In summary, existing linear error models with small misalignment angles have limited applicability to various scenarios, while existing nonlinear filtering and multiplicative quaternion models suffer from drawbacks such as high computational cost and poor adaptability to the launch system. There is an urgent need in this field for an error modeling method for inertial navigation and satellite navigation systems that does not require small angle assumptions, adapts to the launch coordinate system, and balances solution accuracy and real-time performance. Summary of the Invention

[0007] To address the aforementioned technical problems, embodiments of this application propose a launch-system inertial navigation and satellite navigation integration method based on a quaternion error model. This method aims to accurately describe the error propagation characteristics under large misalignment angles, making the error model closer to the actual physical process, thereby effectively improving the stability and accuracy of launch-system inertial navigation and satellite navigation integration.

[0008] To achieve the above objectives, embodiments of this application propose a launch-system inertial navigation and satellite navigation integration method based on a quaternion error model. The method includes the following steps: S1, establishing a quaternion error model; S2, establishing an inertial navigation model and an inertial navigation error model under the launch system; S3, establishing an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the quaternion error model; S4, starting the inertial navigation system and completing preparatory work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state; S5, updating the time-varying values ​​in the measurement error model, the inertial navigation error model, and the integrated navigation error model in real time according to the navigation parameters of the inertial navigation system, and performing time updates; S6, updating satellite navigation measurements when the satellite navigation signal is valid; S7, correcting the inertial navigation system based on the error estimation results of Kalman filtering.

[0009] To achieve the above objectives, embodiments of this application also propose a launch-system inertial navigation and satellite navigation integrated system based on a quaternion error model. The system includes: a quaternion establishment module for establishing a quaternion error model; an inertial navigation establishment module for establishing an inertial navigation model and an inertial navigation error model under the launch system; an integrated navigation establishment module for establishing an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the quaternion error model; a preparation module for starting the inertial navigation system and completing preparation work, including airborne alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state; a time update module for updating the time-varying values ​​in the measurement error model, the inertial navigation error model, and the integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performing time updates; a measurement update module for updating satellite navigation measurements when the satellite navigation signal is valid; and a correction module for correcting the inertial navigation system based on the error estimation results of Kalman filtering.

[0010] Optionally, in the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system. The geocentric coordinate system, i.e. The system, whose coordinate system origin. Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis Construct a right-handed rectangular coordinate system; Launch coordinate system, i.e., launch system The system, whose coordinate system origin As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and the Earth; Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and All are inertial coordinate systems; The carrier coordinate system, i.e. The system, whose coordinate system origin At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.

[0011] Optionally, a surrounding axis, axis, Axis rotation The rotation matrices are respectively , , ; (3-1); (3-2); (3-3); based on , , ,get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: (3-4); (3-5); in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.

[0012] Optionally, the construction of the attitude error equation for the quaternion error model includes: Representing attitude information using quaternions, the attitude quaternion error equation can be derived as follows: (4-1); in, It is a real quaternion. To calculate quaternions, It is an additive error quaternion; Tie attitude quaternions during system rotation The differential equation is: (4-2); The differential equation for attitude quaternion update during inertial navigation system (INS) calculation is: (4-3); in, For the real System relative to The angular velocity of the system, , For the Earth's rotational angular velocity in The projection on the system, , , These are gyroscope measurements. This refers to the measurement error of the gyroscope, also known as gyroscope drift. ; Subtracting equation (4-3) from equation (4-2) yields the attitude quaternion error equation: (4-3); Neglecting second-order minor quantities in equation (4-3), and since The system is fixedly connected to the Earth, so It is a fixed value, therefore Then the linear differential equation is obtained as follows: (4-4); in, This represents the random constant drift of the equivalent gyroscope.

[0013] Optionally, the construction of the velocity error equation for the quaternion error model includes: The velocity differential equation in the system is: (5-1); in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is ​​obtained by gyroscope measurement; In fact, the velocity differential equation used by inertial navigation for navigation calculations is: (5-2); in, The value is the accelerometer measurement. , , , Calculate the values ​​for the attitude matrix. , Calculate the error for the attitude matrix; Subtracting equation (5-2) from equation (5-1), and ignoring second-order minor quantities, the final velocity error equation is: (5-3); Because the alignment time is short, the influence of position error can be ignored, and the following is taken: ; set up for The matrix after removing the first row is: (5-4); set up for The matrix after removing the first row is: (5-5); Obviously there is The corresponding measured values ​​are Subtracting them, we get: (5-6); because ,therefore: (5-7); in, This represents the nonlinear part of the equation.

[0014] Optionally, the construction of the position error equation and the inertial device error equation of the quaternion error model includes: The position differential equation in the system is: (6-1); Based on equation (6-1), the position error equation can be obtained as follows: (6-2); Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. The model is created using random constants plus white noise, i.e. , This represents the random constant drift of the equivalent gyroscope. To measure the white noise of the gyroscope's angular velocity, and taking the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: (6-3); Accelerometer drift The model is created using random constants plus white noise, i.e. , To achieve zero bias for the equivalent accelerometer random constant, For accelerometer force measurement using white noise, and taking the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: (6-4).

[0015] Optionally, based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the quaternion loosely coupled integrated navigation state equation is derived as follows: (7-1); remember ,in: (7-2); (7-3); In equation (7-1), the state equation is divided into a linear part and a nonlinear part to facilitate model processing and make the model more intuitive. It is a nonlinear term in the state equation, specifically: (7-4); System noise vector It consists of accelerometer zero bias and gyroscope zero bias, and is as follows: (7-5); System noise driving matrix for: (7-6); Here Take only the last three columns; for: (7-7); for: (7-8); The quaternion velocity error term is: (7-9); The velocity-position measurement vector is: (7-10); set up The Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of inertial navigation and satellite navigation is: (7-11); The expression is: (7-12).

[0016] Optionally, the discretized forms of the state equation and the measurement equation are as follows: (8-1); (8-2); in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence; For the processing of nonlinear state equations, the idea of ​​extended Kalman filtering is applied. The nonlinear terms are linearized through Taylor expansion, and then the Kalman filtering framework is used to process the linearized function, resulting in the following state transition matrix: (8-3); (8-4); and satisfy: (8-5); in, Let be the covariance matrix of the system noise. The covariance matrix for measuring noise; The state is predicted in one step as follows: (8-6); The state is estimated as follows: (8-7); in, It is called the filter gain matrix, which is the weight of the observation information during state update; (8-8); in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. (8-9); in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: (8-10); (8-11).

[0017] Optionally, the quaternion error is calculated using a navigation algorithm. The speed is The location is ; The attitude was corrected to: (9-1); in, The attitude quaternion obtained from the inertial navigation system solution. The corrected attitude quaternion; Speed ​​correction: (9-2); The position is corrected to: (9-3).

[0018] This application proposes a launch-system inertial navigation and satellite navigation integration method based on a quaternion error model. Traditional small-error equations typically assume a small misalignment angle and perform linearization. However, in the initial launch phase or during high-dynamic maneuvers of the carrier, the misalignment angle can be large, introducing model errors through linearization. This application eliminates this assumption, accurately describing the error propagation characteristics under large misalignment angles, making the model closer to actual physical processes. This method has no singularities in attitude calculation, can operate across all attitudes, and is more suitable for launch-system environments. Furthermore, it can suppress errors caused by conic and rowing effects within the launch system environment. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments or related technologies of this application, the accompanying drawings used in the description of the embodiments or related technologies of this application will be briefly introduced below. The following drawings are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. The drawings described herein are only used to explain this application and are not intended to limit this application.

[0020] Figure 1 This is a flowchart of a launch system inertial navigation satellite navigation combination method based on a quaternion error model provided in one embodiment of this application; Figure 2 This is a detailed schematic diagram of a launch system inertial navigation satellite navigation combination method based on a quaternion error model provided in one embodiment of this application; Figure 3 This is a schematic diagram of the launch coordinate system provided in one embodiment of this application; Figure 4 This is a schematic diagram of the structure of a launch system inertial navigation satellite navigation combined system based on a quaternion error model, provided in another embodiment of this application. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the various embodiments of this application will be described in detail below with reference to the accompanying drawings. Those skilled in the art will understand that many technical details have been presented in the embodiments of this application to facilitate better understanding. However, the technical solutions claimed in this application can be implemented even without these technical details and various variations and modifications based on the following embodiments. The division of the following embodiments is for ease of description and should not constitute any limitation on the specific implementation of this application. The following embodiments can be combined with and referenced by each other without contradiction.

[0022] One embodiment of this application proposes a launch system inertial navigation and satellite navigation combination method based on a quaternion error model. The implementation details of the launch system inertial navigation and satellite navigation combination method based on a quaternion error model proposed in this embodiment are described in detail below. The following implementation details are provided for ease of understanding only and are not necessary for implementing this solution.

[0023] The specific process of the launch system inertial navigation satellite navigation combination method based on the quaternion error model proposed in this embodiment can be described as follows: Figure 1 As shown, its implementation details are as follows: Figure 2 As shown, the method includes: S1, Establish a quaternion error model.

[0024] S2, establish the inertial navigation model and inertial navigation error model under the launch system.

[0025] S3. Establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on a quaternion error model.

[0026] S4. Activate the inertial navigation system and complete preparatory work, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state.

[0027] S5 updates the time-varying values ​​in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performs time updates.

[0028] S6 updates satellite navigation measurements when the satellite navigation signal is valid.

[0029] S7, the inertial navigation system is corrected based on the error estimation results of the Kalman filter.

[0030] The implementation details of each part of the launch system inertial navigation satellite navigation combination method based on the quaternion error model proposed in this embodiment are described in detail below.

[0031] 1) Coordinate system and coordinate system transformation.

[0032] In the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system.

[0033] The geocentric coordinate system, i.e. The system, whose coordinate system origin Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis This forms a right-handed rectangular coordinate system.

[0034] Launch coordinate system, i.e., launch system The system, whose coordinate system origin As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and Earth (please refer to...) Figure 3 ).

[0035] Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and Both are inertial coordinate systems, but their coordinate origins and coordinate axis orientations differ.

[0036] The carrier coordinate system, i.e. The system, whose coordinate system origin At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.

[0037] 2) Rotation matrix.

[0038] Let's surround axis, axis, Axis rotation The rotation matrices are respectively , , , respectively represented as: (3-1); (3-2); (3-3).

[0039] based on , , You can get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: (3-4); (3-5); in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.

[0040] 3) Quaternion error model.

[0041] 3-1) Attitude error equation.

[0042] Representing attitude information using quaternions, the attitude quaternion error equation can be derived as follows: (4-1); in, It is a real quaternion. To calculate quaternions, It is an additive error quaternion.

[0043] Tie attitude quaternions during system rotation The differential equation is: (4-2).

[0044] The differential equation for attitude quaternion update during inertial navigation system (INS) calculation is: (4-3); in, For the real System relative to The angular velocity of the system, , For the Earth's rotational angular velocity in The projection on the system, , , These are gyroscope measurements. This refers to the measurement error of the gyroscope, also known as gyroscope drift. .

[0045] Subtracting equation (4-3) from equation (4-2) yields the attitude quaternion error equation: (4-3).

[0046] Neglecting second-order minor quantities in equation (4-3), and since The system is fixedly connected to the Earth, so It is a fixed value, therefore Then the linear differential equation is obtained as follows: (4-4); in, This represents the random constant drift of the equivalent gyroscope.

[0047] 3-2) Velocity error equation.

[0048] The velocity differential equation in the system is: (5-1); in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is ​​obtained by measuring the gyroscope.

[0049] In fact, the velocity differential equation used by inertial navigation for navigation calculations is: (5-2); in, The value is the accelerometer measurement. , , , Calculate the values ​​for the attitude matrix. , Calculate the error for the attitude matrix.

[0050] Subtracting equation (5-2) from equation (5-1), and ignoring second-order minor quantities, the final velocity error equation is: (5-3); Because the alignment time is short, the influence of position error can be ignored, and the following is taken: .

[0051] set up for The matrix after removing the first row is: (5-4).

[0052] set up for The matrix after removing the first row is: (5-5).

[0053] Obviously there is The corresponding measured values ​​are Subtracting them, we get: (5-6).

[0054] because ,therefore: (5-7); in, This represents the nonlinear part of the equation.

[0055] 3-3) Position error equation.

[0056] The position differential equation in the system is: (6-1).

[0057] Based on equation (6-1), the position error equation can be obtained as follows: (6-2).

[0058] 3-4) Error equations for inertial devices.

[0059] Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. The model is created using random constants plus white noise, i.e. , This represents the random constant drift of the equivalent gyroscope. To measure the white noise of the gyroscope's angular velocity, and taking the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: (6-3).

[0060] Accelerometer drift The model is created using random constants plus white noise, i.e. , To achieve zero bias for the equivalent accelerometer random constant, For accelerometer force measurement using white noise, and taking the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: (6-4).

[0061] 4) Quaternion loosely coupled SINS / GNSS integrated navigation algorithm.

[0062] 4-1) Quaternion loosely coupled integrated navigation state equations.

[0063] Based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the quaternion loosely coupled integrated navigation state equation is derived as follows: (7-1).

[0064] remember ,in: (7-2); (7-3).

[0065] In equation (7-1), the state equation is divided into a linear part and a nonlinear part to facilitate model processing and make the model more intuitive. It is a nonlinear term in the state equation, specifically: (7-4).

[0066] System noise vector It consists of accelerometer zero bias and gyroscope zero bias, and is as follows: (7-5).

[0067] System noise driving matrix for: (7-6); Here Only the last three columns are taken.

[0068] for: (7-7).

[0069] for: (7-8).

[0070] The quaternion velocity error term is: (7-9).

[0071] 4-2) Quaternion loosely coupled velocity-position measurement equation.

[0072] The velocity-position measurement vector is: (7-10).

[0073] set up The Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of inertial navigation and satellite navigation is: (7-11).

[0074] The expression is: (7-12).

[0075] 4-3) Quaternion combination navigation filtering algorithm.

[0076] The discretized forms of the state equation and the measurement equation are as follows: (8-1); (8-2); in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence.

[0077] For the processing of nonlinear state equations, the idea of ​​extended Kalman filtering is applied. The nonlinear terms are linearized through Taylor expansion, and then the Kalman filtering framework is used to process the linearized function, resulting in the following state transition matrix: (8-3); (8-4).

[0078] and satisfy: (8-5); in, Let be the covariance matrix of the system noise. It is a non-negative definite matrix. To measure the covariance matrix of the noise, It is a positive definite matrix.

[0079] The state is predicted in one step as follows: (8-6).

[0080] The state is estimated as follows: (8-7); in, It is called the filter gain matrix, which is the weight of the observation information during state updates.

[0081] (8-8); in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. (8-9); in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: (8-10); (8-11).

[0082] The relevant information in the off-diagonal elements combines the observation vector with those that cannot be passed through The matrix is ​​coupled with the measurement-related state, therefore the matrix It describes the degree of correlation between the uncertainty of the state estimation and the estimation error.

[0083] After repeating equations (8-6) to (8-11), the calculations were performed. After that, we can conclude that The convergence value, i.e., the error values ​​of attitude, velocity, position, gyroscope drift and accelerometer drift.

[0084] 4-5) Feedback correction.

[0085] The quaternion error calculated using the navigation algorithm is: The speed is The location is .

[0086] The attitude was corrected to: (9-1); in, The attitude quaternion obtained from the inertial navigation system solution. This is the corrected attitude quaternion.

[0087] Speed ​​correction: (9-2).

[0088] The position is corrected to: (9-3).

[0089] This embodiment proposes a launch-system inertial navigation and satellite navigation combination method based on a quaternion error model. Traditional small-error equations typically assume a small misalignment angle for linearization. However, in the initial launch phase or during high-dynamic maneuvers, the misalignment angle can be large, introducing model errors through linearization. This embodiment eliminates this assumption, accurately describing error propagation characteristics under large misalignment angles, making the model closer to actual physical processes. This method has no singularities in attitude calculation, can operate across all attitudes, and is more suitable for launch-system environments. Furthermore, it can suppress errors caused by conic and rowing effects within the launch system environment.

[0090] The steps described above are merely for clarity in describing the technical solution. In actual implementation, they can be combined into one step, or certain steps can be broken down into multiple steps, as long as they involve the same logical relationship, they are all within the scope of protection of this application. Any insignificant modifications or designs added to the algorithm or process, as long as they do not change the core of the algorithm or process, are also within the scope of protection of this application.

[0091] Accordingly, another embodiment of this application proposes a launch system inertial navigation and satellite navigation combined system based on a quaternion error model. The details of the launch system inertial navigation and satellite navigation combined system based on a quaternion error model proposed in this embodiment are described in detail below. The following details are provided for ease of understanding only and are not necessary for implementing this solution.

[0092] Figure 4 This is a schematic diagram of the structure of a launch system inertial navigation satellite navigation integrated system based on a quaternion error model proposed in this embodiment, including: quaternion establishment module M1, inertial navigation establishment module M2, integrated navigation establishment module M3, preparation module M4, time update module M5, measurement update module M6, and correction module M7.

[0093] The quaternion establishment module M1 is used to establish the quaternion error model.

[0094] The inertial navigation establishment module M2 is used to establish the inertial navigation model and inertial navigation error model under the launch system.

[0095] The integrated navigation establishment module M3 is used to establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on a quaternion error model.

[0096] The preparation module M4 is used to start the inertial navigation system and complete preparations, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state.

[0097] The time update module M5 is used to update the time-varying values ​​in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and to perform time updates.

[0098] The measurement update module M6 is used to update satellite navigation measurements when the satellite navigation signal is valid.

[0099] The correction module M7 is used to correct the inertial navigation system based on the error estimation results of the Kalman filter.

[0100] It is worth noting that all modules involved in this embodiment are logical modules. In practical applications, a logical module can be a physical module, a part of a physical module, or an organic combination of multiple physical modules. Furthermore, to highlight the innovative aspects of this application, this embodiment does not introduce modules that are not closely related to solving the technical problems proposed in this application. However, this does not mean that other modules are absent from this embodiment.

[0101] It is not difficult to see that this embodiment is a system embodiment corresponding to the above method embodiments, and this embodiment can be implemented in conjunction with the above method embodiments. The relevant technical details and technical effects mentioned in the above method embodiments are still valid in this embodiment, and will not be repeated here to reduce repetition. Accordingly, the relevant technical details mentioned in this embodiment can also be applied to the above method embodiments.

[0102] It will be understood by those skilled in the art that the above embodiments are specific implementations of this application, and in practical applications, various changes can be made in form, detail, and description without departing from the spirit and scope of this application. For those skilled in the art, several improvements and modifications can be made without departing from the principles of this application, and these improvements and modifications are also considered to be within the scope of protection of this application.

Claims

1. A method for combining a launch system inertial navigation system and a satellite navigation system based on a quaternion error model, characterized in that, include: S1, Establish a quaternion error model; S2, Establish the inertial navigation model and inertial navigation error model under the launch system; S3, Establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on the quaternion error model; S4. Start the inertial navigation system and complete the preparatory work, including air alignment, according to the conventional workflow and working mode of the inertial navigation system, until the aircraft enters the inertial navigation state. S5 updates the time-varying values ​​in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and performs time updates accordingly. S6, when the satellite guidance signal is valid, update the satellite guidance measurement; S7, the inertial navigation system is corrected based on the error estimation results of the Kalman filter.

2. The method for combining a launch system inertial navigation and satellite navigation system based on a quaternion error model according to claim 1, characterized in that, In the current integrated navigation scenario, four coordinate systems are involved: the geocentric coordinate system, the launch coordinate system, the launch inertial coordinate system, and the carrier coordinate system. The geocentric coordinate system, i.e. The system, whose coordinate system origin. Centered on Earth axis In the equatorial plane and pointing towards the Prime Meridian axis It is the Earth's axis of rotation and points towards the North Pole. axis In the equatorial plane and with axis , axis Construct a right-handed rectangular coordinate system; Launch coordinate system, i.e., launch system The system, whose coordinate system origin. As the launch point, axis Within the horizontal plane of the launch point and pointing in the launch aiming direction. axis Perpendicular to the horizontal plane of the launch point and pointing upwards. axis and axis , axis Construct a right-handed rectangular coordinate system. The system is fixed to the Earth, and the geographical latitude of the launch point ,longitude ,high and launch azimuth Confirmed The relationship between the system and the Earth; Launch inertial coordinate system, i.e. Tie, The moment of launch and The systems coincide, but due to the Earth's rotation, the system fixed on the Earth... The orientation of the system in inertial space changes. System and All are inertial coordinate systems; The carrier coordinate system, i.e. The system, whose coordinate system origin. At the center of mass of the aircraft, axis Coinciding with the longitudinal axis of the aircraft axis Located in the longitudinal plane of symmetry of the aircraft and perpendicular to axis , axis , axis and axis , axis This forms a right-handed rectangular coordinate system.

3. The ST-EKF-based launch system inertial navigation satellite navigation integrated navigation method according to claim 2, characterized in that, Let's surround axis, axis, Axis rotation The rotation matrices are respectively , , ; （3-1）； （3-2）； （3-3）； based on , , ,get Tie The direction cosine matrix of the system is , Tie The direction cosine matrix of the system is , and They are represented as follows: （3-4）； （3-5）； in, , , These represent the roll angle, pitch angle, and yaw angle, respectively.

4. The method for combining a launch system inertial navigation and satellite navigation based on a quaternion error model according to claim 3, characterized in that, The construction of the attitude error equation for the quaternion error model includes: Representing attitude information using quaternions, the attitude quaternion error equation can be derived as follows: （4-1）； in, It is a real quaternion. To calculate quaternions, It is an additive error quaternion; Tie attitude quaternions during system rotation The differential equation is: （4-2）； The differential equation for attitude quaternion update during inertial navigation system (INS) calculation is: ；（4-3）； in, For the real System relative to The angular velocity of the system, , For the Earth's rotational angular velocity in The projection on the system, , , These are gyroscope measurements. This refers to the measurement error of the gyroscope, also known as gyroscope drift. ; Subtracting equation (4-3) from equation (4-2) yields the attitude quaternion error equation: （4-3）； Neglecting second-order minor quantities in equation (4-3), and since The system is fixedly connected to the Earth, so It is a fixed value, therefore Then the linear differential equation is obtained as follows: （4-4）； in, This represents the random constant drift of the equivalent gyroscope.

5. The method for combining a launch system inertial navigation and satellite navigation based on a quaternion error model according to claim 4, characterized in that, The construction of the velocity error equation for the quaternion error model includes: The velocity differential equation in the system is: （5-1）； in, for The differential form of the velocity in the system. For comparison, For gravity, Angular velocity vector The antisymmetric matrix is ​​obtained by gyroscope measurement; In fact, the velocity differential equation used by inertial navigation for navigation calculations is: （5-2）； in, The value is the accelerometer measurement. , , , Calculate the values ​​for the attitude matrix. , Calculate the error for the attitude matrix; Subtracting equation (5-2) from equation (5-1), and ignoring second-order minor quantities, the final velocity error equation is: （5-3）； Because the alignment time is short, the influence of position error can be ignored, and the following is taken: ; set up for The matrix after removing the first row is: （5-4）； set up for The matrix after removing the first row is: （5-5）； Obviously there is The corresponding measured values ​​are Subtracting them, we get: （5-6）； because ,therefore: （5-7）； in, This represents the nonlinear part of the equation.

6. The method for combining a launch system inertial navigation and satellite navigation based on a quaternion error model according to claim 5, characterized in that, The construction of the position error equation and the inertial device error equation of the quaternion error model includes: The position differential equation in the system is: （6-1）； Based on equation (6-1), the position error equation can be obtained as follows: （6-2）； Inertial device error modeling includes gyroscope drift and accelerometer drift, and gyroscope drift is included. The model is created using random constants plus white noise, i.e. , This represents the random constant drift of the equivalent gyroscope. To measure the white noise of the gyroscope's angular velocity, and taking the gyroscope's constant zero bias as the system state, the gyroscope zero bias error equation is: （6-3）； Accelerometer drift The model is created using random constants plus white noise, i.e. , To achieve zero bias of the equivalent accelerometer random constant, For accelerometer force measurement using white noise, and taking the accelerometer constant zero bias as the system state, the accelerometer zero bias error equation is: （6-4）。 7. The method for combining a launch system inertial navigation and satellite navigation based on a quaternion error model according to claim 6, characterized in that, Based on the attitude error equation, velocity error equation, position error equation, and inertial device error equation, the quaternion loosely coupled integrated navigation state equation is derived as follows: （7-1）； remember ,in: （7-2）； （7-3）； In equation (7-1), the state equation is divided into a linear part and a nonlinear part to facilitate model processing and make the model more intuitive. It is a nonlinear term in the state equation, specifically: （7-4）； System noise vector It consists of accelerometer zero bias and gyroscope zero bias, and is as follows: （7-5）； System noise driving matrix for: （7-6）； Here Take only the last three columns; for: （7-7）； for: （7-8）； The quaternion velocity error term is: （7-9）； The velocity-position measurement vector is: （7-10）； set up The Kalman filter measurement equation for the loosely coupled integrated navigation algorithm of inertial navigation and satellite navigation is: （7-11）； The expression is: （7-12）。 8. The method for combining a launch system inertial navigation and satellite navigation system based on a quaternion error model according to claim 7, characterized in that, The discretized forms of the state equation and the measurement equation are as follows: （8-1）； （8-2）； in, yes The system state at any given moment. and This is a discretized representation of the state equation and the noise driving matrix. The system excitation noise sequence, To measure the noise sequence; For the nonlinear state equations, the extended Kalman filter is applied to linearize the nonlinear terms through Taylor expansion. Then, the Kalman filter framework is used to process the linearized function, resulting in the following state transition matrix: （8-3）； （8-4）； and satisfy: （8-5）； in, Let be the covariance matrix of the system noise. The covariance matrix for measuring noise; The state is predicted in one step as follows: （8-6）； The state is estimated as follows: （8-7）； in, It is called the filter gain matrix, which is the weight of the observation information during state update; （8-8）； in, The mean squared error matrix for one-step prediction has diagonal elements that are the variances of the state estimates, representing the uncertainty of the estimate. （8-9）； in, This represents the prior error, i.e., the calculation error. , To estimate the mean square error matrix, and Represented as: （8-10）； （8-11）。 9. The method for combining a launch system inertial navigation and satellite navigation based on a quaternion error model according to claim 8, characterized in that, The quaternion error calculated using the navigation algorithm is: The speed is The location is ; The attitude was corrected to: （9-1）； in, The attitude quaternion obtained from the inertial navigation system solution. The corrected attitude quaternion; Speed ​​correction: （9-2）； The position is corrected to: （9-3）。 10. A launch system inertial navigation satellite navigation combined system based on a quaternion error model, characterized in that, include: The quaternion establishment module is used to establish quaternion error models. The inertial navigation system establishment module is used to establish the inertial navigation model and inertial navigation error model in the launch system. The integrated navigation establishment module is used to establish an inertial navigation and satellite navigation integrated navigation model and an integrated navigation error model based on a quaternion error model. The preparation module is used to start the inertial navigation system and complete preparations, including air alignment, according to the conventional workflow and operating mode of the inertial navigation system, until the aircraft enters the inertial navigation state. The time update module is used to update the time-varying values ​​in the measurement error model, inertial navigation error model, and integrated navigation error model in real time based on the navigation parameters of the inertial navigation system, and to perform time updates. The measurement update module is used to update satellite navigation measurements when the satellite navigation signal is valid. The correction module is used to correct the inertial navigation system based on the error estimation results of the Kalman filter.

Images