A method for coarse alignment of Doppler radar-assisted strapdown inertial navigation moving base under large azimuth misalignment

Abstract

The present invention belongs to the field of navigation technology, and specifically relates to a Doppler radar assisted strapdown inertial navigation motion base rough alignment method under large azimuth misalignment angle. Step 1: define gyro constant drift and accelerometer constant error, establish the coordinate transformation matrix from ideal navigation coordinate system n to actual navigation coordinate system n′ and the mathematical platform attitude error equation and velocity error equation of strapdown inertial navigation under large azimuth misalignment angle; Step 2: establish the state equation of strapdown inertial navigation motion base rough alignment under large azimuth misalignment angle, and obtain the state matrix; Step 3: construct the measurement of Doppler velocity radar assisted strapdown inertial navigation rough alignment, and then combine it with the coordinate transformation matrix of step 1 to obtain the measurement equation and measurement matrix of strapdown inertial navigation motion base rough alignment under large azimuth misalignment angle; Step 4: use t k Measurement of time Z k The Kalman filter is used to realize the rough alignment filtering solution of the strapdown inertial navigation system moving base under large azimuth misalignment angle, thereby realizing the rough alignment of the strapdown inertial navigation system.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, and in particular relates to a Doppler radar assisted strapdown inertial navigation motion base coarse alignment method under large azimuth misalignment angle. Background Art

[0002] Initial alignment is the first task that the strapdown inertial navigation system needs to complete before performing navigation solution. Alignment accuracy and alignment time are two important indicators for the initial alignment of the strapdown inertial navigation system. Alignment accuracy directly affects the navigation accuracy of the strapdown inertial navigation system, while alignment time determines the emergency mobility of the strapdown inertial navigation system. Especially for the ever-changing battlefield situation in modern warfare, if the strapdown inertial navigation system in the weapon system can quickly complete high-precision autonomous coarse alignment under the vehicle-mounted motion base, it will be of great significance to improve the weapon system's rapid response and emergency mobility capabilities and battlefield survivability.

[0003] The autonomous coarse alignment of the strapdown inertial navigation system on a vehicle-mounted moving base is a difficult problem that needs to be solved urgently in the field of inertial navigation technology in recent years. The difficulty mainly lies in the fact that the strapdown inertial navigation system on a moving base faces a large azimuth misalignment angle problem during autonomous coarse alignment, and the linearization assumption established in the traditional small misalignment angle model has become invalid.

[0004] However, there are two main technologies for rough alignment of strapdown inertial navigation motion base under large azimuth misalignment angle: one is to construct nonlinear error equations in the form of nonlinear filtering, and the nonlinear filters used mainly include extended Kalman filter (EKF), unscented Kalman filter (UKF) and particle filter (PF), and the other is based on optimization alignment (OBA) method. Among them, EKF needs to calculate the Jacobian matrix, and for high-dimensional complex systems, it has the disadvantages of complex algorithms and large amount of calculation; the performance of UKF depends on the description of noise. In actual application, it is difficult to describe the accurate noise distribution, which may result in performance degradation or even filter divergence; for high-dimensional systems, the calculation amount of PF is huge, and it is currently difficult to apply it in embedded navigation computers. The traditional OBA method does not take into account the deviation of IMU, which has a significant impact on the accuracy of vector observation and is not suitable for low-cost strapdown inertial navigation systems.

[0005] To this end, the present invention innovatively proposes to use the sine and cosine trigonometric functions of the large azimuth misalignment angle to replace the large azimuth misalignment angle itself, establishes a linearization model of the large azimuth misalignment angle, and uses the vehicle-mounted Doppler speed measurement radar with strong autonomy and good anti-interference to assist the strapdown inertial navigation system to achieve rough alignment of the moving base under the large azimuth misalignment angle. The method has very strong mobility and autonomy, good anti-interference and concealment, low cost, and easy engineering implementation, which greatly improves the rapid response capability, emergency mobility capability, and battlefield survivability of the weapon system under the battlefield environment. Summary of the invention

[0006] In view of the above-mentioned problems, the present invention utilizes the sine and cosine trigonometric functions of the large azimuth misalignment angle to replace the large azimuth misalignment angle itself, establishes a linearization model of the large azimuth misalignment angle, and includes the sine and cosine trigonometric functions of the large azimuth misalignment angle in the system state of the coarse alignment filter. The output structure measurement of the Doppler velocity measurement radar and the strapdown inertial navigation system is utilized, and the Kalman filter is used to perform the coarse alignment filter calculation. The overall calculation amount is small, the algorithm stability is high, the filter convergence speed is fast, and no external special maneuvering assistance is required.

[0007] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0008] A Doppler radar assisted strapdown inertial navigation motion base coarse alignment method under large azimuth misalignment angle, comprising:

[0009] Step 1: Define the gyro constant drift and accelerometer constant error, establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′, and on this basis establish the mathematical platform attitude error equation and velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle;

[0010] Step 2: Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, select the system state of the strapdown inertial navigation motion base coarse alignment, establish the state equation of the strapdown inertial navigation motion base coarse alignment under large azimuth misalignment angle, and obtain the state matrix;

[0011] Step 3: construct the measurement of Doppler velocity radar assisted strapdown inertial navigation coarse alignment, and combine the measurement with the coordinate transformation matrix of step 1 to obtain the measurement equation and measurement matrix of strapdown inertial navigation motion base coarse alignment under large azimuth misalignment angle;

[0012] Step 4: Based on the state equation and measurement equation of the rough alignment of the strapdown inertial navigation motion base established in steps 2 and 3, use t k Measurement of time Z k The Kalman filter can be used to realize the rough alignment filtering solution of the strapdown inertial navigation system moving base under large azimuth misalignment angle, and then obtain the optimal estimation result of the strapdown inertial navigation misalignment angle and other states. At this time, the mathematical platform attitude error of the strapdown inertial navigation is corrected according to the estimation result to achieve the rough alignment of the strapdown inertial navigation system.

[0013] Preferably, the step 1 comprises:

[0014] Step 1.1: Define the gyro random constant drift and accelerometer random constant error, and establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′;

[0015] Step 1.2: Combine the coordinate transformation matrix of step 1.1 to establish the mathematical platform attitude error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle;

[0016] Step 1.3: Combine the coordinate transformation matrix in step 1.1 to establish the velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle.

[0017] Preferably, the step 1.1 comprises:

[0018] Step 1.1.1: The core components of strapdown inertial navigation are gyroscopes and accelerometers. First, study the drift errors of gyroscopes and accelerometers, define gyroscope random constant drift and accelerometer random constant error, and record the gyroscope random constant drift vector in the body coordinate system as ε b , whose three components are ε bx , ε by and ε bz , the accelerometer random constant error vector is Its three components are

[0019] Step 1.1.2: Use three rotations to represent the error between the ideal navigation coordinate system n and the actual navigation coordinate system n′. The three rotation attitude error angles are φ E ,φ N and φ U , where φ E and φ N For small angles, φ U For large angles, establish the coordinate transformation matrix of the three-rotation attitude error angle;

[0020] Step 1.1.3: Establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′.

[0021] Preferably, the step 1.2 comprises:

[0022] Step 1.2.1: The rotation angular rate of the actual navigation coordinate system n′ relative to the inertial coordinate system i is Equivalent to the rotation angular rate of the ideal navigation coordinate system n relative to the inertial coordinate system i Subtract the gyro random constant drift vector ε in the navigation coordinate system n ,get formula;

[0023]

[0024] Among them, ε n =[ε E ε N ε U ] T, where ε E ,ε N ,ε U They represent the eastward, northward and celestial components of the gyro random constant drift in the navigation coordinate system respectively;

[0025] Step 1.2.2: Convert the rate of change of misalignment angle It is expressed as the rotation angular rate of the actual navigation coordinate system n′ relative to the ideal navigation coordinate system n

[0026]

[0027] Step 1.2.3: Replace the break down:

[0028]

[0029] Step 1.2.4: Replace the Substitute the formula into step 1.2.3 In the decomposition formula, It is expressed as:

[0030]

[0031] Where I represents a 3×3 identity matrix;

[0032] Step 1.2.5: Substitute the coordinate transformation matrix from step 1.1.3 into the coordinate transformation matrix from step 1.2.4. In the formula, the actual mathematical platform attitude error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle is obtained by expansion and arrangement:

[0033]

[0034]

[0035] in, The rotation angular rate The three components of east, north and sky are: is φ E The derivative of is φ N The derivative of The rotation angular rate The east and north components of

[0036] Preferably, the step 1.3 comprises:

[0037] Step 1.3.1: According to the specific force equation, the ideal speed value equation formula is:

[0038]

[0039] in, The derivative of the velocity vector in the n-frame, The coordinate transformation matrix from b to n, f b : accelerometer output, The projection of the angular velocity of the e system relative to the i system on the n system is: The angular velocity of the n system relative to the e system is projected on the n system, V n : velocity vector in the n system, g n : The gravitational acceleration is projected in the n system, and all parameters are ideal values;

[0040] The actual speed value is calculated as follows:

[0041]

[0042] The parameters in this formula are defined the same as those in the ideal speed value equation, and the parameters in this formula are actual values;

[0043] in, and The error between and f b The error between and V n The error between n ;

[0044] Step 1.3.2: Replace the Substitute the actual speed value into the calculation formula of step 1.3.1 and then subtract the ideal speed value to obtain:

[0045]

[0046] in, is the coordinate transformation matrix from the ideal navigation system to the actual navigation system, f n is the specific force output under the n system;

[0047] Step 1.3.3: Substitute the coordinate transformation matrix of step 1.1.3 into the coordinate transformation matrix of step 1.3.2. In the formula, after sorting, the velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle is obtained:

[0048]

[0049]

[0050]

[0051] The parameters with subscripts E, N, and U in the formula are their projections in the east, north, and celestial directions.

[0052] Preferably, the step 2 is specifically:

[0053] Step 2.1: Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, the system state vector X of the strapdown inertial navigation motion base coarse alignment is selected as:

[0054]

[0055] ε bx ,ε by ,ε bz Represents ε b Projection on the x, y, z axes, express Projection on the x, y, and z axes;

[0056] Step 2.2: Based on step 2.1, establish the state equation for rough alignment of the strapdown inertial navigation motion base under large azimuth misalignment angle:

[0057]

[0058] Among them, F is the state matrix; G is the noise driving matrix; W is the system noise.

[0059] Preferably, the step 3 is specifically:

[0060] Step 3.1: Assume that the projection form of the carrier motion speed output by the Doppler speed radar in the carrier coordinate system is V b ; The projection form of the carrier motion speed output by the strapdown inertial navigation system in the actual navigation coordinate system is The carrier posture matrix is Then the measurement Z of Doppler velocity radar assisted strapdown inertial navigation alignment is constructed as:

[0061]

[0062] Step 3.2: Replace the Substituting into the Z formula in step 3.1, we get the Z deformation formula:

[0063]

[0064] Step 3.3: Substitute the coordinate transformation matrix of step 1.1.3 into the Z deformation formula of step 3.2 to obtain the rough alignment measurement equation:

[0065] Z=HX+V

[0066] Among them, the measurement matrix H = [H1 H2 O 3×6 ],

[0067]

[0068] Among them, H1 and H2 are for simplified representation and have no special meaning. The parameter V in H1 is E , V N , V U V n Projection in the E, N, and U directions.

[0069] Compared with the prior art, the present invention has the following beneficial effects:

[0070] 1. The present invention adopts Doppler velocity radar to assist strapdown inertial navigation to perform rough alignment of the moving base under large azimuth misalignment angle. Compared with the commonly used GPS / Beidou assisted alignment, the present invention does not need to receive external signals, has very strong autonomy and mobility, good anti-interference and concealment, low cost, easy engineering implementation, can be applied to the complex and harsh environment of the modern battlefield, and can greatly improve the rapid response capability, emergency mobility capability and battlefield survivability of the weapon system under the battlefield environment;

[0071] 2. The present invention innovatively proposes to use the sine and cosine trigonometric functions of the large azimuth misalignment angle to replace the large azimuth misalignment angle itself, and establishes a linear model of the large azimuth misalignment angle. The linear Kalman filter can be used to realize the rough alignment of the moving base of the strapdown inertial navigation system under the large azimuth misalignment angle, which effectively avoids the problems of complex algorithm and large amount of calculation brought by nonlinear filtering. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0073] In the attached picture:

[0074] Figure 1 is a flow chart of the method of the present invention;

[0075] Figure 2 It is a principle diagram of the coarse alignment filtering algorithm of the strapdown inertial navigation system under large azimuth misalignment angle of the present invention;

[0076] Figure 3 The motion trajectory diagram of the carrier (vehicle) in the simulation verification experiment of the embodiment of the present invention;

[0077] Figure 4 This is a diagram of the misalignment angle of the strapdown inertial navigation system in the simulation verification experiment of the embodiment of the present invention after performing coarse alignment for 400 seconds under a large azimuth misalignment angle. DETAILED DESCRIPTION

[0078] The preferred embodiments of the present invention are described below in conjunction with the accompanying drawings. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present invention, and are not used to limit the present invention.

[0079] Example:

[0080] Refer to the attached Figure 1-4 As shown, a Doppler radar assisted strapdown inertial navigation motion base coarse alignment method under large azimuth misalignment angle includes:

[0081] Step 1: The core components of strapdown inertial navigation are gyroscopes and accelerometers, which are usually collectively referred to as inertial devices. Inertial device errors are the most basic errors of strapdown inertial navigation, which have a direct impact on the strapdown inertial navigation solution error and are the source of all errors of strapdown inertial navigation. Inertial device errors specifically include installation errors, scale factor errors, drift errors, etc. Among them, the deterministic parts of installation errors, scale factor errors and drift errors can be effectively compensated through experimental calibration. The present invention only studies the random errors of gyroscopes and accelerometers, defines the random constant drift of gyroscopes and the random constant error of accelerometers, establishes the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′, and on this basis establishes the mathematical platform attitude error equation and velocity error equation of strapdown inertial navigation under large azimuth misalignment angle conditions. Including:

[0082] Step 1.1: Define the gyro random constant drift and accelerometer random constant error, and establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′. Specifically include:

[0083] Step 1.1.1: The core components of strapdown inertial navigation are gyroscopes and accelerometers. First, study the drift errors of gyroscopes and accelerometers, define gyroscope random constant drift and accelerometer random constant error, and record the gyroscope random constant drift vector in the body coordinate system as ε b , whose three components are ε bx , ε by and ε bz , the accelerometer random constant error vector is Its three components are

[0084] Step 1.1.2: Assume that the carrier coordinate system is the right-front-up coordinate system, that is, the three axes of the coordinate system are along the carrier's horizontal axis to the right, the vertical axis to the front, and the heading axis to the top; the navigation coordinate system is the northeast sky geographic coordinate system, that is, the three axes of the coordinate system point to the geographic east, geographic north, and the sky along the vertical line. Due to the influence of various error sources, the error between the ideal navigation coordinate system n and the actual navigation coordinate system n' is represented by the three-rotation attitude error, and the three-rotation attitude error angles are φ E ,φN and φ U , where φ E and φ N For small angles, φ U For a large angle, Φ=[φ E φ N φ U ] T , establish the coordinate transformation matrix of the three-rotation attitude error angle:

[0085]

[0086]

[0087]

[0088] They are the coordinate transformation matrices corresponding to the three rotations; is the coordinate transformation matrix of the ideal navigation coordinate system n relative to the actual navigation coordinate system n′.

[0089] Step 1.1.3: Based on step 1.1.2, establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′:

[0090]

[0091] Step 1.2: Combine the coordinate transformation matrix of step 1.1 to establish the mathematical platform attitude error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle;

[0092] Step 1.2.1: The rotation angular rate of the actual navigation coordinate system n′ relative to the inertial coordinate system i is Equivalent to the rotation angular rate of the ideal navigation coordinate system n relative to the inertial coordinate system i Subtract the gyro random constant drift vector ε in the navigation coordinate system n ,get formula;

[0093]

[0094] Among them, ε n =[ε E ε N ε U ] T , where ε E ,ε N ,ε U They represent the east, north and celestial components of the gyro drift respectively;

[0095] Step 1.2.2: Convert the rate of change of misalignment angle It is expressed as the rotation angular rate of the actual navigation coordinate system n′ relative to the ideal navigation coordinate system n

[0096]

[0097] Step 1.2.3: Replace the break down:

[0098]

[0099] Step 1.2.4: Substitute the equation (5) of step 1.2.1 into Substitute the formula into step 1.2.3 (7) In the decomposition formula, It is expressed as:

[0100]

[0101] Where I represents a 3×3 identity matrix;

[0102] Step 1.2.5: Substitute the coordinate transformation matrix of step 1.1.3 equation (4) into step 1.2.4 equation (8) In the formula, we get:

[0103]

[0104] The actual mathematical platform attitude error equation of the strapdown inertial navigation system under the condition of large azimuth misalignment angle is obtained by expansion and arrangement:

[0105]

[0106]

[0107] in, The rotation angular rate The three components of east, north and sky are: is φ E The derivative of is φ N The derivative of The rotation angular rate The east and north components of

[0108] Since the Kalman filter cycle is short, the carrier's attitude angle changes very little in a short time. Therefore, the present invention assumes that φ U remains unchanged. Based on the above assumptions, sinφ U and cosφ U In each filtering cycle, it will be a constant value, and we get:

[0109]

[0110] Step 1.3: Combine the coordinate transformation matrix in step 1.1 to establish the velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle.

[0111] Step 1.3.1: According to the specific force equation, the ideal speed value equation formula is:

[0112]

[0113] in, The derivative of the velocity vector in the n-frame, The coordinate transformation matrix from b to n, f b : accelerometer output, The projection of the angular velocity of the e system relative to the i system on the n system is: The angular velocity of the n system relative to the e system is projected on the n system, V n : velocity vector in the n system, g n : The gravitational acceleration is projected in the n system, and all parameters are ideal values;

[0114] The actual speed value is calculated as follows:

[0115]

[0116] The parameters in this formula are defined the same as those in the ideal speed value equation, and the parameters in this formula are actual values;

[0117]

[0118]

[0119]

[0120] and The error between and f b The error between and V n The error between n , the remaining and g n The error is very small and can be ignored.

[0121] Step 1.3.2: Substitute equations (15), (16) and (17) from step 1.3.1 Substituting the actual speed value into the calculation formula of step 1.3.1 (13) and then subtracting the ideal speed value, we get:

[0122]

[0123] in, is the coordinate transformation matrix from the ideal navigation system to the actual navigation system, f n is the specific force output under the n system;

[0124] Step 1.3.3: Substitute the coordinate transformation matrix of step 1.1.3 equation (4) into the coordinate transformation matrix of step 1.3.2 equation (18) In the formula, after expansion, we get:

[0125]

[0126] After rearranging equation (19), we can obtain the velocity error equation of strapdown inertial navigation under the condition of large azimuth misalignment angle:

[0127]

[0128]

[0129]

[0130] The parameters with subscripts E, N, and U in the formula are their projections in the east, north, and celestial directions.

[0131] There are nonlinear terms in the differential equation of , and the following assumptions are made: the product of the horizontal misalignment angle and the sine trigonometric function of the azimuth misalignment angle is a small quantity, and the product of the horizontal misalignment angle and the cosine trigonometric function of the azimuth misalignment angle is itself. Therefore, δV U The differential equation can be approximated as:

[0132]

[0133] Step 2: Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, select the system state of the strapdown inertial navigation motion base coarse alignment, establish the state equation of the strapdown inertial navigation motion base coarse alignment under large azimuth misalignment angle, and obtain the state matrix; specifically:

[0134] Step 2.1: Step 1 has derived the mathematical platform attitude error equation and velocity error equation of the strapdown inertial navigation under large azimuth misalignment angle, and used this as the basis for obtaining the state matrix of the rough alignment of the motion base.

[0135] Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, the system state vector X of the strapdown inertial navigation motion base coarse alignment is selected as:

[0136]

[0137] ε bx ,ε by ,ε bz Represents ε b Projection on the x, y, z axes, express Projection on the x, y, and z axes;

[0138] Step 2.2: Based on step 2.1, establish the state equation for rough alignment of the strapdown inertial navigation motion base under large azimuth misalignment angle:

[0139]

[0140] Where F is the state matrix; G is the noise driving matrix; W is the system noise, W = [w gx ,w gy ,0,0,w ax ,w ay ,w az ] T , here w gx ,w gy are the white noise of the gyroscope along the x and y axes of the carrier, w ax ,w ay ,w az are the white noise of the accelerometer along the x, y, and z axes of the carrier, that is, E[W(t)] = 0 and E[W(t)W T (τ)] = qδ(t-τ), q is the variance intensity matrix of W. The state matrix F is expressed as:

[0141]

[0142] Among them, record:

[0143]

[0144]

[0145] Among them, ω ie is the angular velocity of the earth's rotation, L is the latitude, and h is the altitude.

[0146] Step 3: Doppler speed radar is usually installed along the axial direction of the carrier. Since the speed output by Doppler speed radar is the projection form of the carrier's motion speed in the carrier coordinate system, and the speed output by strapdown inertial navigation system is the projection form of the carrier's motion speed in the navigation coordinate system, in order to use the speed outputs of the above two systems to construct auxiliary alignment measurements, it is necessary to use the carrier attitude matrix output by the strapdown inertial navigation system to convert the speed output of Doppler speed radar from the carrier coordinate system to the navigation coordinate system.

[0147] Construct the measurement of Doppler velocity radar assisted strapdown inertial navigation rough alignment, and combine the measurement with the coordinate transformation matrix of step 1 to obtain the measurement equation and measurement matrix of strapdown inertial navigation motion base rough alignment under large azimuth misalignment angle; specifically:

[0148] Step 3.1: Assume that the projection form of the carrier motion speed output by the Doppler speed radar in the carrier coordinate system is V b ; The projection form of the carrier motion speed output by the strapdown inertial navigation system in the actual navigation coordinate system is The carrier posture matrix is Then the measurement Z of Doppler velocity radar assisted strapdown inertial navigation alignment is constructed as:

[0149]

[0150] Step 3.2: Substitute step 1.3.1 (17) Substituting into the Z formula of step 3.1 (28), we get the Z deformation formula of (28):

[0151]

[0152] Step 3.3: Substitute the coordinate transformation matrix of step 1.1.3 (4) into the Z deformation formula of step 3.2 (29), and simplify it in the same way as (22) to obtain:

[0153]

[0154] Further arrangement leads to the rough alignment measurement equation:

[0155] Z=HX+V (31)

[0156] Measurement matrix H = [H1 H2 O 3×6 ],in,

[0157]

[0158] Among them, H1 and H2 are for simplified representation and have no special meaning. The parameter V in H1 is E , V N , V U V n Projection in the E, N, and U directions.

[0159] Step 4: Based on the state equation and measurement equation of the rough alignment of the strapdown inertial navigation motion base established in steps 2 and 3, use t k Measurement of time Z k Kalman filtering can be used to achieve rough alignment filtering of the strapdown inertial navigation system moving base under large azimuth misalignment angles, and then obtain the optimal estimation results of the inertial navigation misalignment angle and other states. At this time, the mathematical platform attitude error of the strapdown inertial navigation is corrected according to the estimation result to achieve the rough alignment of the strapdown inertial navigation system.

[0160] Simulation Results and Analysis

[0161] (1) Assume that the random constant drift of the gyroscope and the random constant error of the accelerometer in the strapdown inertial navigation system are 0.02° / h and 10 -4 g, whose white noise random walks are The horizontal attitude angle error of the strapdown inertial navigation is 2°, and the azimuth angle error is 60°; the simulation time of auxiliary coarse alignment is 400s, and the carrier (vehicle) is always in motion. The motion trajectory is as follows: Figure 3 shown.

[0162] (2) The misalignment angle of the strapdown inertial navigation system after 400s of coarse alignment under large azimuth misalignment is as follows: Figure 4 As shown in the figure, it is not difficult to find that the three attitude error angles of the mathematical platform of the strapdown inertial navigation system have achieved effective convergence through rough alignment, and the estimated errors are controlled within 40', 5', and 5' respectively after rough alignment. Therefore, the simulation results show that the carrier velocity information measured by the Doppler velocity radar plays a key role in the rough alignment of the strapdown inertial navigation motion base, and the effect is significant.

[0163] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A Doppler radar assisted strapdown inertial navigation motion base coarse alignment method under large azimuth misalignment angle, characterized in that: include: Step 1: Define the gyro constant drift and accelerometer constant error, establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′, and on this basis establish the mathematical platform attitude error equation and velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle; including: Step 1.1: Define the gyro random constant drift and accelerometer random constant error, and establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′; Step 1.2: Combine the coordinate transformation matrix of step 1.1 to establish the mathematical platform attitude error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle; Step 1.2 includes: Step 1.2.1: The rotation angular rate of the actual navigation coordinate system n′ relative to the inertial coordinate system i is Equivalent to the rotation angular rate of the ideal navigation coordinate system n relative to the inertial coordinate system i Subtract the gyro random constant drift vector ε in the navigation coordinate system n ,get formula; Among them, ε n =[ε E ε N ε U ] T , where ε E ,ε N ,ε U They represent the eastward, northward and celestial components of the gyro random constant drift in the navigation coordinate system respectively; Step 1.2.2: Convert the rate of change of misalignment angle It is expressed as the rotation angular rate of the actual navigation coordinate system n′ relative to the ideal navigation coordinate system n Step 1.2.3: Replace the break down: Step 1.2.4: Replace the Substitute the formula into step 1.2.3 In the decomposition formula, It is expressed as: Where I represents a 3×3 identity matrix; Step 1.2.5: Substitute the coordinate transformation matrix from step 1.1.3 into the coordinate transformation matrix from step 1.2.

4. In the formula, the actual mathematical platform attitude error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle is obtained by expansion and arrangement: in, The rotation angular rate The three components of east, north and sky are: is φ E The derivative of is φ N The derivative of The rotation angular rate The east and north components of Step 1.3: Combine the coordinate transformation matrix of step 1.1 to establish the velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle; Step 1.3 includes: Step 1.3.1: According to the specific force equation, the ideal speed value equation formula is: in, The derivative of the velocity vector in the n-frame, The coordinate transformation matrix from b to n, f b : accelerometer output, The projection of the angular velocity of the e system relative to the i system on the n system is: The angular velocity of the n system relative to the e system is projected on the n system, V n : velocity vector in the n system, g n : The gravitational acceleration is projected in the n system, and all parameters are ideal values; The actual speed value is calculated as follows: The parameters in this formula are defined the same as those in the ideal speed value equation, and the parameters in this formula are actual values; in, and The error between and f b The error between and V n The error between n ; Step 1.3.2: Replace the Substitute the actual speed value into the calculation formula of step 1.3.1 and then subtract the ideal speed value to obtain: in, is the coordinate transformation matrix from the ideal navigation system to the actual navigation system, f n is the specific force output under the n system; Step 1.3.3: Substitute the coordinate transformation matrix of step 1.1.3 into the coordinate transformation matrix of step 1.3.

2. In the formula, after sorting, the velocity error equation of the strapdown inertial navigation under the condition of large azimuth misalignment angle is obtained: The parameters with subscripts E, N, and U in the formula are their projections in the east, north, and sky directions; Step 2: Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, select the system state of the strapdown inertial navigation motion base coarse alignment, establish the state equation of the strapdown inertial navigation motion base coarse alignment under large azimuth misalignment angle, and obtain the state matrix; specifically: Step 2.1: Based on the attitude error equation and velocity error equation of the strapdown inertial navigation mathematical platform established in step 1, the system state vector X of the strapdown inertial navigation motion base coarse alignment is selected as: ε bx ,ε by ,ε bz Represents ε b Projection on the x, y, z axes, express Projection on the x, y, and z axes; where ε b is the gyro random constant drift vector in the body coordinate system, is the random constant error vector of the accelerometer; the error between the ideal navigation coordinate system n and the actual navigation coordinate system n′ is expressed by three rotations, and the three rotation attitude error angles are φ E ,φ N and φ U , where φ E and φ N For small angles, φ U For large angles; Step 2.2: Based on step 2.1, establish the state equation for rough alignment of the strapdown inertial navigation motion base under large azimuth misalignment angle: Where F is the state matrix; G is the noise driving matrix; W is the system noise; Step 3: construct the measurement of Doppler velocity radar assisted strapdown inertial navigation rough alignment, and combine the measurement with the coordinate transformation matrix of step 1 to obtain the measurement equation and measurement matrix of strapdown inertial navigation motion base rough alignment under large azimuth misalignment angle; Step 4: Based on the state equation and measurement equation of the rough alignment of the strapdown inertial navigation motion base established in steps 2 and 3, use t k Measurement of time Z k The Kalman filter can be used to realize the rough alignment filtering solution of the strapdown inertial navigation system moving base under large azimuth misalignment angle, and then obtain the optimal estimation result of the strapdown inertial navigation misalignment angle and other states. At this time, the mathematical platform attitude error of the strapdown inertial navigation is corrected according to the estimation result to achieve the rough alignment of the strapdown inertial navigation system.

2. The method for coarse alignment of a Doppler radar-assisted strapdown inertial navigation motion base under a large azimuth misalignment angle according to claim 1, characterized in that: The step 1.1 comprises: Step 1.1.1: The core components of strapdown inertial navigation are gyroscopes and accelerometers. First, study the drift errors of gyroscopes and accelerometers, define gyroscope random constant drift and accelerometer random constant error, and record the gyroscope random constant drift vector in the body coordinate system as ε b , whose three components are ε bx , ε by and ε bz , the accelerometer random constant error vector is Its three components are Step 1.1.2: Use three rotations to represent the error between the ideal navigation coordinate system n and the actual navigation coordinate system n′. The three rotation attitude error angles are φ E ,φ N and φ U , where φ E and φ N For small angles, φ U For large angles, establish the coordinate transformation matrix of the three-rotation attitude error angle; Step 1.1.3: Establish the coordinate transformation matrix from the ideal navigation coordinate system n to the actual navigation coordinate system n′.

3. The method for coarse alignment of a Doppler radar-assisted strapdown inertial navigation motion base under a large azimuth misalignment angle according to claim 2, characterized in that: The step 3 is specifically as follows: Step 3.1: Assume that the projection form of the carrier motion speed output by the Doppler speed radar in the carrier coordinate system is V b ; The projection form of the carrier motion velocity output by the strapdown inertial navigation system in the actual navigation coordinate system is: The carrier posture matrix is Then the measurement Z of Doppler velocity radar assisted strapdown inertial navigation alignment is constructed as: Step 3.2: Replace the Substituting into the Z formula in step 3.1, we get the Z deformation formula: Step 3.3: Substitute the coordinate transformation matrix of step 1.1.3 into the Z deformation formula of step 3.2 to obtain the rough alignment measurement equation: Z=HX+V Among them, the measurement matrix H = [H1 H2 O 3×6 ], Among them, H1 and H2 are for simplified representation and have no special meaning. The parameter V in H1 is E , V N , V U V n Projection in the E, N, and U directions.

Images