Rocket initial azimuth alignment correction method

Abstract

The application provides a rocket initial azimuth alignment correction method, comprising the following steps: S1, collecting satellite navigation data, inertial navigation data and standard trajectory speed at at least one preset time after the rocket takes off; S2, obtaining effective navigation data through preprocessing; S3, obtaining a first percentage according to each effective navigation data and a preset number, and judging whether the first percentage is greater than a first standard percentage; if yes, obtaining first fitting data; if no, setting the inertial navigation error to zero; S4, obtaining the inertial navigation error according to the first fitting data, the preset time and the collection time; S5, obtaining corrected attitude quaternion according to the inertial navigation error, the standard trajectory speed, initial state data and real-time attitude quaternion, and replacing the real-time attitude quaternion with the corrected attitude quaternion. The beneficial effect is that the real-time attitude quaternion is intelligently corrected to eliminate the azimuth error, and the rocket can smoothly enter the orbit.

Description

Technical Field

[0001] This invention relates to the technical field of rocket orientation correction, and more specifically, to a method for correcting the initial orientation of a rocket. Background Technology

[0002] Solid rockets are aircraft that rely on the reaction force generated by the ejection of working medium by an engine to propel themselves forward. They are characterized by high maneuverability and short preparation time, and can meet the needs of rapid maneuverability, rapid deployment and rapid access to space. They can be widely used in military, civilian and commercial small and medium-sized satellite launch missions.

[0003] The initial attitude obtained by initial orientation alignment after rocket launch generally contains errors. If a low-precision inertial navigation system is used for self-alignment, the error will be even greater. If orientation correction is not performed as soon as possible after rocket launch, a large amount of navigation error will accumulate after a long period of flight navigation calculations, causing the rocket to fail to enter orbit smoothly or resulting in low orbital insertion accuracy. Moreover, due to the low cost requirements of commercial rockets, low-precision inertial navigation systems are currently widely used for initial orientation alignment, resulting in a large initial orientation alignment error and a large error between the attitude calculated by the onboard computer and the actual attitude of the rocket. Summary of the Invention

[0004] The problem this invention aims to solve is to provide a method for correcting the initial azimuth alignment of a rocket, which intelligently corrects the real-time attitude quaternion based on satellite navigation data and inertial navigation data, and eliminates azimuth errors through multiple corrections to ensure the rocket successfully enters orbit.

[0005] To address the above problems, this invention provides a method for correcting the initial azimuth alignment of a rocket, comprising:

[0006] Step S1: At least one preset moment after the rocket takes off, control the onboard computer on the rocket to continuously collect a preset number of satellite navigation data and an inertial navigation group corresponding to each satellite navigation data, collect the standard ballistic velocity of the rocket at the preset moment, and record a collection moment corresponding to each satellite navigation data.

[0007] Step S2: Preprocess the satellite navigation data to obtain multiple valid navigation data;

[0008] Step S3: Based on the quantity of each valid navigation data and the preset quantity, a first quantity percentage is obtained, and it is determined whether the first quantity percentage is greater than a preset first standard percentage.

[0009] If so, then perform least squares fitting on each of the effective data to obtain a corresponding first fitted data, and then proceed to step S4;

[0010] If not, set the inertial navigation error to zero and proceed to step S5;

[0011] Step S4: Obtain the corresponding inertial navigation error based on the first fitted data, the preset time, and the acquisition time corresponding to each of the valid navigation data.

[0012] Step S5: Control the onboard computer to acquire a real-time attitude quaternion of the rocket, obtain a corrected attitude quaternion based on the inertial navigation error, the standard ballistic velocity, the preset initial state data and the real-time attitude quaternion, and control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial azimuth alignment correction.

[0013] In this scheme, considering that low-precision inertial navigation systems are commonly used for initial rocket alignment, resulting in large initial alignment errors and significant discrepancies between the attitude calculated by the onboard computer and the actual rocket attitude, this scheme continuously collects a preset amount of satellite navigation data and inertial navigation data from the onboard computer. After filtering and selecting the satellite navigation data, the inertial navigation error is calculated based on the valid data and the first fitted data. Finally, the corrected attitude quaternion is obtained using the inertial navigation error, the standard ballistic velocity, the initial state data, and the real-time attitude quaternion. The corrected attitude quaternion has high accuracy, and replacing the real-time attitude quaternion with it completes the initial alignment correction, allowing the rocket to successfully enter orbit.

[0014] Furthermore, at multiple preset moments after rocket launch, an initial orientation alignment correction can be performed based on this scheme to make the real-time attitude quaternion increasingly accurate. After multiple corrections, the orientation error can be eliminated, ensuring that the rocket successfully enters orbit.

[0015] Preferably, in step S1, the onboard computer is controlled to start at the preset time and continuously collect 30 satellite navigation data points, and collect one satellite navigation data point and the corresponding inertial navigation data point every 0.1 seconds.

[0016] Preferably, each piece of satellite navigation data collected corresponds to a positioning marker, and step S2 includes:

[0017] Step S21: For each piece of satellite navigation data, determine whether the positioning flag corresponding to the satellite navigation data indicates no positioning.

[0018] If so, delete the satellite navigation data corresponding to the positioning marker, and then proceed to step S22;

[0019] If not, the satellite navigation data corresponding to the positioning mark is retained as the first navigation data, and then the process proceeds to step S22;

[0020] Step S22: Process the first navigation data and the inertial navigation data corresponding to each first navigation data to obtain a plurality of valid navigation data.

[0021] In this scheme, a positioning marker is associated with the satellite navigation data during collection. The validity of the satellite navigation data can be determined based on the positioning marker. The satellite navigation data is then filtered in the first step to obtain the first navigation data, thereby improving the accuracy of subsequent initial orientation alignment correction.

[0022] Preferably, each of the first navigation data sets includes first X-direction navigation data, first Y-direction navigation data, and first Z-direction navigation data, and each of the inertial navigation data sets includes second X-direction navigation data, second Y-direction navigation data, and second Z-direction navigation data. Then, step S22 includes:

[0023] Step S221: For each first navigation data and the inertial navigation data corresponding to the first navigation data, calculate a first deviation value by subtracting the first X-direction navigation data and the second X-direction navigation data, calculate a second deviation value by subtracting the first Y-direction navigation data and the second Y-direction navigation data, and calculate a third deviation value by subtracting the first Z-direction navigation data and the second Z-direction navigation data.

[0024] Step S222: Calculate the absolute values ​​of the first deviation value, the second deviation value, and the third deviation value to obtain a first absolute value, a second absolute value, and a third absolute value, respectively.

[0025] Step S223: Determine whether there is at least one absolute value among the first absolute value, the second absolute value, and the third absolute value that is greater than a first preset value.

[0026] If so, delete the first navigation data and then proceed to step S224;

[0027] If not, the first navigation data is retained as the second navigation data, and then the process proceeds to step S224;

[0028] Step S224: Perform least squares fitting on each of the second navigation data to obtain a corresponding second fitting data, and process the second fitting data and each of the second navigation data to obtain multiple effective navigation data.

[0029] In this scheme, the first navigation data is filtered in the second step using the first absolute value, the second absolute value, and the third absolute value to obtain the second navigation data, so as to improve the accuracy of subsequent initial orientation alignment correction.

[0030] Preferably, step S224 includes:

[0031] Step S2241: Based on the quantity of each of the second navigation data and the preset quantity, a second quantity percentage is obtained, and it is determined whether the second quantity percentage is greater than a preset second standard percentage.

[0032] If so, then perform least squares fitting on each of the second navigation data to obtain a first fitting value, a second fitting value and a third fitting value as the second fitting data, and then proceed to step S2242;

[0033] If not, set the inertial navigation error to zero and proceed to step S3;

[0034] Step S2242: For each second navigation data, a fourth deviation value is obtained by subtracting the first X-direction navigation data and the first fitted value; a fifth deviation value is obtained by subtracting the first Y-direction navigation data and the second fitted value; and a sixth deviation value is obtained by subtracting the first Z-direction navigation data and the third fitted value.

[0035] Step S2243: Calculate the absolute values ​​of the fourth deviation value, the fifth deviation value, and the sixth deviation value to obtain the corresponding fourth absolute value, fifth absolute value, and sixth absolute value;

[0036] Step S2244: Determine whether at least one of the fourth, fifth, and sixth absolute values ​​is greater than the second preset value.

[0037] If so, delete the second navigation data and then proceed to step S3;

[0038] If not, the second navigation data is retained as the valid navigation data, and then the process proceeds to step S3.

[0039] In this scheme, the second navigation data is filtered in a third step using the fourth absolute value, the fifth absolute value, and the sixth absolute value to obtain valid navigation data, thereby improving the accuracy of subsequent initial orientation alignment correction.

[0040] Preferably, the first preset value is 15 and the second preset value is 0.15.

[0041] Preferably, the first standard percentage is 25% and the second standard percentage is 50%.

[0042] Preferably, if the first fitting data includes fourth and fifth fitting values ​​correlated in the X direction, sixth and seventh fitting values ​​correlated in the Y direction, and eighth and ninth fitting values ​​correlated in the Z direction, then step S4 includes:

[0043] Step S41: Calculate the average time corresponding to each of the valid navigation data collection times.

[0044] Step S42: A first error value is obtained based on the fourth fitted value, the fifth fitted value, the acquisition time, and the average time; a second error value is obtained based on the sixth fitted value, the seventh fitted value, the acquisition time, and the average time; and a third error value is obtained based on the eighth fitted value, the ninth fitted value, the acquisition time, and the average time.

[0045] Step S43: Calculate the absolute values ​​of the first error value, the second error value, and the third error value to obtain the corresponding seventh absolute value, eighth absolute value, and ninth absolute value.

[0046] Step S44: Determine whether at least one of the seventh, eighth, and ninth absolute values ​​is greater than a third preset value.

[0047] If so, the first error value, the second error value, and the third error value are set to zero and used as the inertial navigation error, and then the process proceeds to step S5;

[0048] If not, the first error value, the second error value, and the third error value are taken as the inertial navigation error, and then the process proceeds to step S5.

[0049] Preferably, the preset initial state data includes an initial pitch angle, an initial yaw angle, and a theoretical ballistic velocity, and the standard ballistic velocity includes the standard ballistic velocity in the X direction and the standard ballistic velocity in the Z direction. Then, step S5 includes:

[0050] Step S51: Control the onboard computer to acquire the real-time attitude quaternion of the rocket, and obtain an initial roll angle deviation based on the third error value, the standard ballistic velocity, the standard ballistic velocity in the X direction and the standard ballistic velocity in the Z direction.

[0051] Step S52: Obtain a comparative attitude quaternion based on the initial roll angle deviation, the initial pitch angle, and the initial yaw angle; and obtain the corrected attitude quaternion based on the comparative attitude quaternion and the real-time attitude quaternion.

[0052] Step S53: Control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial orientation alignment correction. Attached Figure Description

[0053] Figure 1 This is a flowchart of the steps of the present invention;

[0054] Figure 2 This is a flowchart illustrating step S2 of the present invention.

[0055] Figure 3 This is a flowchart illustrating step S22 of the present invention.

[0056] Figure 4 This is a flowchart illustrating step S224 of the present invention.

[0057] Figure 5 This is a flowchart illustrating step S4 of the present invention.

[0058] Figure 6 This is a flowchart of step S5 of the present invention. Detailed Implementation

[0059] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0060] In a preferred embodiment of the present invention, based on the above-mentioned problems existing in the prior art, a method for correcting the initial azimuth alignment of a rocket is provided, such as... Figure 1 As shown, it includes:

[0061] Step S1: At least one preset moment after the rocket takes off, control the onboard computer on the rocket to continuously collect a preset number of satellite navigation data and a set of inertial navigation data corresponding to each satellite navigation data, collect the standard ballistic velocity of the rocket at the preset moment, and record a collection moment corresponding to each satellite navigation data.

[0062] Step S2: Preprocess the satellite navigation data to obtain multiple valid navigation data;

[0063] Step S3: Based on the number of each valid navigation data point and a preset number, a first quantity percentage is obtained, and it is determined whether the first quantity percentage is greater than a preset first standard percentage.

[0064] If so, perform least squares fitting on each valid data to obtain the corresponding first fitted data, and then proceed to step S4.

[0065] If not, set the inertial navigation error to zero and proceed to step S5;

[0066] Step S4: Obtain the corresponding inertial navigation error based on the first fitted data, the preset time, and the acquisition time corresponding to each valid navigation data.

[0067] Step S5: Control the onboard computer to acquire a real-time attitude quaternion of the rocket, obtain a corrected attitude quaternion based on the inertial navigation error, standard ballistic velocity, preset initial state data and real-time attitude quaternion, and control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial azimuth alignment correction.

[0068] Specifically, in this embodiment, considering that low-precision inertial navigation systems are commonly used for self-alignment during the initial azimuth alignment of rockets, resulting in a large initial azimuth alignment error and a large error between the attitude calculated by the onboard computer and the actual attitude of the rocket, this solution continuously collects a preset amount of satellite navigation data and inertial navigation data by the onboard computer. After filtering and screening each satellite navigation data, the inertial navigation error is calculated based on each valid data and the first fitted data. Finally, the corrected attitude quaternion is obtained by using the inertial navigation error, standard ballistic velocity, initial state data, and real-time attitude quaternion. At this point, the obtained corrected attitude quaternion has a high degree of accuracy. Replacing the real-time attitude quaternion with it completes the initial azimuth alignment correction, allowing the rocket to successfully enter orbit.

[0069] Preferably, at multiple preset moments after rocket launch, an initial orientation alignment correction can be performed based on this scheme to make the real-time attitude quaternion increasingly accurate. After multiple corrections, the orientation error can be eliminated, ensuring that the rocket successfully enters orbit.

[0070] Preferably, for example, in specific operations, the first initial bearing alignment correction can be performed 20 seconds after the rocket liftoff, the second initial bearing alignment correction can be performed 25 seconds after the rocket liftoff, and the third initial bearing alignment correction can be performed 30 seconds after the rocket liftoff. The bearing error can be basically eliminated through three initial bearing alignment corrections.

[0071] Preferably, the number of times the initial orientation alignment correction is performed after the rocket takes off is not limited, nor is the interval between two adjacent initial orientation alignment corrections limited.

[0072] Preferably, the initial orientation alignment correction process needs to begin at a predetermined time after rocket liftoff to prevent the rocket from accumulating a large orientation error during the flight phase before correction, which would require more propellant for correction and cause loss of rocket energy.

[0073] In a preferred embodiment of the present invention, in step S1, the onboard computer is controlled to start at a preset time and continuously collect 30 satellite navigation data points, and collect one satellite navigation data point and the corresponding inertial navigation data every 0.1 seconds.

[0074] Specifically, in this embodiment, the onboard computer samples satellite navigation data at a sampling frequency of 10Hz for a sampling duration of 3s, and samples a total of 30 sets of satellite navigation data. Sufficient satellite navigation data is used to provide data support for the initial azimuth alignment correction.

[0075] Preferably, the number of satellite navigation data is not limited to 30; the accuracy of initial azimuth alignment correction can be improved by increasing the number of satellite navigation data.

[0076] Preferably, the data acquisition interval for satellite navigation is not limited to 0.1 seconds. As the amount of satellite navigation data increases, the acquisition interval can be appropriately reduced to ensure that the sampling duration remains basically unchanged.

[0077] In a preferred embodiment of the present invention, each collected satellite navigation data corresponds to a positioning marker, and step S2 is as follows: Figure 2 As shown, it includes:

[0078] Step S21: For each satellite navigation data point, determine whether the positioning flag corresponding to the satellite navigation data indicates no positioning.

[0079] If so, delete the satellite navigation data corresponding to the positioning marker, and then proceed to step S22;

[0080] If not, retain the satellite navigation data corresponding to the positioning mark as the first navigation data, and then proceed to step S22;

[0081] Step S22: Process each first navigation data and the corresponding inertial navigation data to obtain multiple valid navigation data.

[0082] Specifically, in this embodiment, a positioning marker is associated when collecting satellite navigation data. Based on the positioning marker, it can be determined whether the satellite navigation data is valid. The first step of filtering the satellite navigation data is performed to obtain the first navigation data to improve the accuracy of subsequent initial orientation alignment correction.

[0083] Preferably, after obtaining the first navigation data through the first step of filtering, a first navigation data can be recorded every 10ms to facilitate subsequent calculations.

[0084] In a preferred embodiment of the present invention, each first navigation data includes first X-direction navigation data, first Y-direction navigation data, and first Z-direction navigation data, and each inertial navigation data includes second X-direction navigation data, second Y-direction navigation data, and second Z-direction navigation data. Then step S22 is as follows: Figure 3 As shown, it includes:

[0085] Step S221: For each first navigation data and the inertial navigation data corresponding to the first navigation data, calculate the difference between the first X-direction navigation data and the second X-direction navigation data to obtain a corresponding first deviation value, calculate the difference between the first Y-direction navigation data and the second Y-direction navigation data to obtain a corresponding second deviation value, and calculate the difference between the first Z-direction navigation data and the second Z-direction navigation data to obtain a corresponding third deviation value.

[0086] Step S222: Calculate the absolute values ​​of the first deviation value, the second deviation value, and the third deviation value to obtain the corresponding first absolute value, second absolute value, and third absolute value.

[0087] Step S223: Determine whether at least one of the first absolute value, the second absolute value, and the third absolute value is greater than the first preset value.

[0088] If so, delete the first navigation data and then proceed to step S224;

[0089] If not, the first navigation data is retained as the second navigation data, and then the process proceeds to step S224;

[0090] Step S224: Perform least squares fitting on each second navigation data to obtain a corresponding second fitting data, and process the second fitting data and each second navigation data to obtain multiple effective navigation data.

[0091] Specifically, in this embodiment, the first navigation data is filtered in the second step using the first absolute value, the second absolute value, and the third absolute value to obtain the second navigation data, so as to improve the accuracy of subsequent initial orientation alignment correction.

[0092] Preferably, the formulas for calculating the first deviation value, the second deviation value, and the third deviation value are as follows:

[0093] ΔVx(t)=Vx BD (t)-Vx INS (t)

[0094] ΔVy(t)=Vy BD (t)-Vy INS (t)

[0095] ΔVz(t)=Vz BD(t)-Vz INS (t)

[0096] in,

[0097] t represents the data acquisition time;

[0098] Vx BD (t) represents the navigation data in the first X direction;

[0099] Vy BD (t) represents the navigation data in the first Y direction;

[0100] Vz BD (t) represents the navigation data in the first Z direction;

[0101] Vx INS (t) represents the navigation data in the second X direction;

[0102] Vy INS (t) represents the navigation data in the second Y direction;

[0103] Vz INS (t) represents the navigation data in the second Z direction;

[0104] ΔVx(t) represents the first deviation value;

[0105] ΔVy(t) represents the second deviation value;

[0106] ΔVz(t) represents the third deviation value.

[0107] In a preferred embodiment of the present invention, step S224 is as follows: Figure 4 As shown, it includes:

[0108] Step S2241: Based on the quantity of each second navigation data and a preset quantity, a second quantity percentage is obtained, and it is determined whether the second quantity percentage is greater than a preset second standard percentage.

[0109] If so, then perform least squares fitting on each of the second navigation data to obtain a first fitting value, a second fitting value and a third fitting value as the second fitting data, and then proceed to step S2242;

[0110] If not, set the inertial navigation error to zero and proceed to step S3;

[0111] Step S2242: For each second navigation data, a fourth deviation value is obtained by subtracting the first X-direction navigation data and the first fitted value; a fifth deviation value is obtained by subtracting the first Y-direction navigation data and the second fitted value; and a sixth deviation value is obtained by subtracting the first Z-direction navigation data and the third fitted value.

[0112] Step S2243: Calculate the absolute values ​​of the fourth, fifth, and sixth deviation values ​​to obtain the corresponding fourth absolute value, fifth absolute value, and sixth absolute value;

[0113] Step S2244: Determine whether at least one of the fourth, fifth, and sixth absolute values ​​is greater than the second preset value.

[0114] If so, delete the second navigation data and then proceed to step S3;

[0115] If not, the second navigation data is retained as valid navigation data, and then the process proceeds to step S3.

[0116] Specifically, in this embodiment, the second navigation data is filtered in the third step using the fourth, fifth, and sixth absolute values ​​to obtain valid navigation data, thereby improving the accuracy of subsequent initial orientation alignment correction.

[0117] In a preferred embodiment of the present invention, the first preset value is 15 and the second preset value is 0.15.

[0118] Specifically, in this embodiment, the first preset value is not limited to 15 and can be adjusted according to the actual situation. Similarly, the second preset value is not limited to 0.15.

[0119] In a preferred embodiment of the present invention, the first standard percentage is 25% and the second standard percentage is 50%.

[0120] Specifically, in this embodiment, the first standard percentage is not limited to 25% and can be adjusted according to the actual situation. Similarly, the second standard percentage is not limited to 50%.

[0121] In a preferred embodiment of the present invention, the first fitting data includes fourth and fifth fitting values ​​correlated in the X direction, sixth and seventh fitting values ​​correlated in the Y direction, and eighth and ninth fitting values ​​correlated in the Z direction. Then step S4 is as follows: Figure 5 As shown, it includes:

[0122] Step S41: Calculate the average time corresponding to each valid navigation data collection time.

[0123] Step S42: A first error value is obtained based on the fourth fitted value, the fifth fitted value, the acquisition time, and the average time; a second error value is obtained based on the sixth fitted value, the seventh fitted value, the acquisition time, and the average time; and a third error value is obtained based on the eighth fitted value, the ninth fitted value, the acquisition time, and the average time.

[0124] Step S43: Calculate the absolute values ​​of the first error value, the second error value, and the third error value to obtain the corresponding seventh absolute value, eighth absolute value, and ninth absolute value;

[0125] Step S44: Determine whether at least one of the seventh, eighth, and ninth absolute values ​​is greater than the third preset value.

[0126] If so, the first error value, the second error value, and the third error value are set to zero and used as inertial navigation error, and then the process proceeds to step S5;

[0127] If not, the first error value, the second error value, and the third error value are taken as inertial navigation errors, and then proceed to step S5.

[0128] Specifically, in this embodiment, the calculation formulas for the first error value, the second error value, and the third error value are as follows:

[0129]

[0130]

[0131]

[0132] in,

[0133] This represents the average time of each valid navigation data point;

[0134] t represents the preset time;

[0135] a vx0 This represents the fourth fitted value;

[0136] a vy0 This represents the sixth fitted value;

[0137] a vz0 This represents the eighth fitted value;

[0138] a vx1 This represents the fifth fitted value;

[0139] a vy1 This represents the seventh fitted value;

[0140] a vz1 This represents the ninth fitted value;

[0141] ΔV xn Indicates the first error value;

[0142] ΔV yn Indicates the second error value;

[0143] ΔV zn This represents the third error value.

[0144] Specifically, in this embodiment, the least squares fitting algorithm is as follows: For a given set of n data points (x... i ,y i )(i=1,2,…,n), use Perform a linear least squares fit;

[0145]

[0146]

[0147] in,

[0148]

[0149]

[0150]

[0151]

[0152] In a preferred embodiment of the present invention, the preset initial state data includes an initial pitch angle, an initial yaw angle, and a theoretical ballistic velocity, and the standard ballistic velocity includes the standard ballistic velocity in the X direction and the standard ballistic velocity in the Z direction. Then step S5 is as follows: Figure 6 As shown, it includes:

[0153] Step S51: Control the onboard computer to obtain the rocket's real-time attitude quaternion, and obtain an initial roll angle deviation based on the third error value, theoretical ballistic velocity, standard ballistic velocity in the X direction, and standard ballistic velocity in the Z direction.

[0154] Step S52: Obtain a comparative attitude quaternion based on the initial roll angle deviation, initial pitch angle and initial yaw angle, and obtain the corrected attitude quaternion based on the comparative attitude quaternion and the real-time attitude quaternion.

[0155] Step S53: Control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial orientation alignment correction.

[0156] Specifically, in this embodiment, the formula for calculating the initial roll angle deviation is as follows:

[0157]

[0158]

[0159] in,

[0160] DVg represents the theoretical ballistic velocity;

[0161] Vx0 represents the standard ballistic velocity in the X direction;

[0162] Vz0 represents the standard ballistic velocity in the Z direction;

[0163] Δγ 0new This indicates the initial roll angle deviation.

[0164] Specifically, in this embodiment, the formula for calculating the comparison attitude quaternion is as follows:

[0165]

[0166] c φ0 =cos(Δφ0 / 2)

[0167] s φ0 =sin(Δφ0 / 2)

[0168] c ψ0 =cos(Δψ0 / 2)

[0169] s ψ0 =sin(Δψ0 / 2)

[0170] c γ0 =cos(Δγ) 0new / 2)

[0171]

[0172] in,

[0173] Δφ0 represents the initial pitch angle;

[0174] Δψ0 represents the initial yaw angle;

[0175] Δγ 0new This indicates the initial roll angle deviation.

[0176] Preferably, the corrected attitude quaternion is obtained through the following calculation formula:

[0177]

[0178] in,

[0179] This represents the corrected attitude quaternion;

[0180] Represents the contrastive quaternion;

[0181] Represents the real-time attitude quaternion.

[0182] While the disclosure is as stated above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of this disclosure, and all such changes and modifications will fall within the protection scope of this invention.

Claims

1. A method for correcting the initial azimuth alignment of a rocket, characterized in that, include: Step S1: At least one preset moment after the rocket takes off, control the onboard computer on the rocket to continuously collect a preset number of satellite navigation data and an inertial navigation group corresponding to each satellite navigation data, collect the standard ballistic velocity of the rocket at the preset moment, and record a collection moment corresponding to each satellite navigation data. Step S2: Preprocess the satellite navigation data to obtain multiple valid navigation data; Step S3: Based on the quantity of each valid navigation data and the preset quantity, a first quantity percentage is obtained, and it is determined whether the first quantity percentage is greater than a preset first standard percentage. If so, perform least squares fitting on each of the effective navigation data to obtain a corresponding first fitting data, and then proceed to step S4; If not, set the inertial navigation error to zero and proceed to step S5; Step S4: Obtain the corresponding inertial navigation error based on the first fitted data, the preset time, and the acquisition time corresponding to each of the valid navigation data. Step S5: Control the onboard computer to acquire a real-time attitude quaternion of the rocket, obtain a corrected attitude quaternion based on the inertial navigation error, the standard ballistic velocity, the preset initial state data and the real-time attitude quaternion, and control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial azimuth alignment correction. In step S1, the onboard computer is controlled to start at the preset time and continuously collect 30 satellite navigation data points, and collect one satellite navigation data point and the corresponding inertial navigation data point every 0.1 seconds; Each piece of satellite navigation data collected corresponds to a positioning marker. Step S2 includes: Step S21: For each piece of satellite navigation data, determine whether the positioning flag corresponding to the satellite navigation data indicates no positioning. If so, delete the satellite navigation data corresponding to the positioning marker, and then proceed to step S22; If not, the satellite navigation data corresponding to the positioning mark is retained as the first navigation data, and then the process proceeds to step S22; Step S22: Process the first navigation data and the inertial navigation data corresponding to each first navigation data to obtain a plurality of valid navigation data; Each set of the first navigation data includes first X-direction navigation data, first Y-direction navigation data, and first Z-direction navigation data, and each set of inertial navigation data includes second X-direction navigation data, second Y-direction navigation data, and second Z-direction navigation data. Therefore, step S22 includes: Step S221: For each first navigation data and the inertial navigation data corresponding to the first navigation data, calculate a first deviation value by subtracting the first X-direction navigation data and the second X-direction navigation data, calculate a second deviation value by subtracting the first Y-direction navigation data and the second Y-direction navigation data, and calculate a third deviation value by subtracting the first Z-direction navigation data and the second Z-direction navigation data. Step S222: Calculate the absolute values ​​of the first deviation value, the second deviation value, and the third deviation value to obtain a first absolute value, a second absolute value, and a third absolute value, respectively. Step S223: Determine whether there is at least one absolute value among the first absolute value, the second absolute value, and the third absolute value that is greater than a first preset value. If so, delete the first navigation data and then proceed to step S224; If not, the first navigation data is retained as the second navigation data, and then the process proceeds to step S224; Step S224: Perform least squares fitting on each of the second navigation data to obtain a corresponding second fitting data, and process the second fitting data and each of the second navigation data to obtain multiple effective navigation data.

2. The rocket initial azimuth alignment correction method according to claim 1, characterized in that, Step S224 includes: Step S2241: Based on the quantity of each of the second navigation data and the preset quantity, a second quantity percentage is obtained, and it is determined whether the second quantity percentage is greater than a preset second standard percentage. If so, then perform least squares fitting on each of the second navigation data to obtain a first fitting value, a second fitting value and a third fitting value as the second fitting data, and then proceed to step S2242; If not, set the inertial navigation error to zero and proceed to step S3; Step S2242: For each second navigation data, a fourth deviation value is obtained by subtracting the first X-direction navigation data and the first fitted value; a fifth deviation value is obtained by subtracting the first Y-direction navigation data and the second fitted value; and a sixth deviation value is obtained by subtracting the first Z-direction navigation data and the third fitted value. Step S2243: Calculate the absolute values ​​of the fourth deviation value, the fifth deviation value, and the sixth deviation value to obtain the corresponding fourth absolute value, fifth absolute value, and sixth absolute value; Step S2244: Determine whether at least one of the fourth, fifth, and sixth absolute values ​​is greater than the second preset value. If so, delete the second navigation data and then proceed to step S3; If not, the second navigation data is retained as the valid navigation data, and then the process proceeds to step S3.

3. The rocket initial azimuth alignment correction method according to claim 2, characterized in that, The first preset value is 15, and the second preset value is 0.

15.

4. The rocket initial azimuth alignment correction method according to claim 2, characterized in that, The first standard percentage is 25%, and the second standard percentage is 50%.

5. The rocket initial azimuth alignment correction method according to claim 1, characterized in that, The first fitting data includes fourth and fifth fitting values ​​correlated in the X direction, sixth and seventh fitting values ​​correlated in the Y direction, and eighth and ninth fitting values ​​correlated in the Z direction. Therefore, step S4 includes: Step S41: Calculate the average time corresponding to each of the valid navigation data collection times. Step S42: A first error value is obtained based on the fourth fitted value, the fifth fitted value, the acquisition time, and the average time; a second error value is obtained based on the sixth fitted value, the seventh fitted value, the acquisition time, and the average time; and a third error value is obtained based on the eighth fitted value, the ninth fitted value, the acquisition time, and the average time. Step S43: Calculate the absolute values ​​of the first error value, the second error value, and the third error value to obtain the corresponding seventh absolute value, eighth absolute value, and ninth absolute value. Step S44: Determine whether at least one of the seventh, eighth, and ninth absolute values ​​is greater than a third preset value. If so, the first error value, the second error value, and the third error value are set to zero and used as the inertial navigation error, and then the process proceeds to step S5; If not, the first error value, the second error value, and the third error value are taken as the inertial navigation error, and then the process proceeds to step S5.

6. The rocket initial azimuth alignment correction method according to claim 5, characterized in that, The preset initial state data includes an initial pitch angle, an initial yaw angle, and a theoretical ballistic velocity. The standard ballistic velocity includes the standard ballistic velocity in the X direction and the standard ballistic velocity in the Z direction. Therefore, step S5 includes: Step S51: Control the onboard computer to acquire the real-time attitude quaternion of the rocket, and obtain an initial roll angle deviation based on the third error value, the standard ballistic velocity, the standard ballistic velocity in the X direction and the standard ballistic velocity in the Z direction. Step S52: Obtain a comparative attitude quaternion based on the initial roll angle deviation, the initial pitch angle, and the initial yaw angle; and obtain the corrected attitude quaternion based on the comparative attitude quaternion and the real-time attitude quaternion. Step S53: Control the onboard computer to replace the real-time attitude quaternion with the corrected attitude quaternion to complete the initial orientation alignment correction.

Images