Navigation method of radar reference system inertial navigation system tightly coupled with radar

Abstract

The application relates to a radar reference system inertial navigation / radar tightly coupled integrated navigation method, device and equipment assisted by satellite navigation, which comprises the following steps: establishing a Kalman filtering measurement equation based on an inertial navigation system error equation and a radar measurement error model; in an inertial navigation / satellite navigation / radar integrated navigation state or an inertial navigation / radar integrated navigation state, if radar data is valid, radar measurement updating is carried out on a 21-dimensional state vector according to the radar data at a trigger time; in the inertial navigation / satellite navigation / radar integrated navigation state, if satellite navigation data is valid, satellite navigation measurement updating is carried out on the 21-dimensional state vector on the basis of the radar measurement updating; and the attitude, speed and position of a strapdown inertial navigation system are feedback corrected. The application directly establishes a navigation reference system on a radar coordinate system, realizes online estimation of inertial / radar integrated navigation system errors and online calibration of radar errors under the assistance of satellite navigation, and improves the integrated navigation precision.

Description

Technical Field

[0001] The application relates to the field of aircraft navigation and data processing technology, specifically to a satellite-guided radar reference system inertial navigation / radar tightly coupled integrated navigation method, device and equipment. Background Technology

[0002] Strapdown Inertial Navigation System (SINS) / Global Navigation Satellite System (GNSS) combined navigation systems offer complementary advantages, are cost-effective, and provide high accuracy; however, they are susceptible to interference in increasingly complex electromagnetic environments. Inertial / radar combined navigation serves as a backup solution. The radar provides positioning and velocity information for aircraft within its coverage area. Positioning information is provided in the form of azimuth, elevation, and range, while velocity information is calculated using the Doppler shift of the echo signal. Radar navigation systems can provide effective velocity and positioning information, correcting the inertial navigation system and improving navigation accuracy. However, its error characteristics change with distance, and its accuracy is generally worse than satellite navigation when the aircraft is far from the radar station. This is partly because the echo signal strength weakens with distance, increasing ranging error, and partly because the radar's positioning and orientation system has inherent errors, which also cause position errors. Therefore, even if the radar itself has high measurement accuracy, insufficient accuracy of its own position and attitude references can lead to significant errors in the navigation information it provides. Inertial navigation system (INS) reference errors and mechanical installation errors can both cause misalignment angles and positional deviations between the radar coordinate system in the radar computer and the actual radar coordinate system. These errors introduce the measurement information provided by the radar into the navigation system, thereby reducing the accuracy of integrated navigation. Summary of the Invention

[0003] The main purpose of this application is to provide a radar reference system inertial navigation / radar tightly coupled integrated navigation method, device and equipment with satellite navigation assistance. It aims to solve the technical problems of existing aircraft navigation systems being susceptible to interference in complex electromagnetic environments, and the large cumulative error of navigation parameters and the decline in positioning and velocity measurement accuracy caused by the lack of online compensation for radar coordinate system reference inaccuracy, installation position deviation and measurement zero point / scale error in traditional inertial navigation / radar integrated navigation.

[0004] To achieve the above objectives, this application provides a satellite-guided radar reference system inertial navigation / radar tightly coupled integrated navigation method, including: establishing an integrated navigation architecture with the radar coordinate system as the navigation reference system; A radar measurement error model incorporating radar reference misalignment angle and fixed measurement error is constructed within the integrated navigation architecture. Using a first-order small-perturbation linearization method, the error equations for the strapdown inertial navigation system in the radar coordinate system are constructed. Based on these error equations, a Kalman filter is built, and its state variables and state transition matrix are extended to 21 dimensions to obtain a 21-dimensional Kalman filter. Finally, the radar measurement matrix and satellite navigation measurement matrix are obtained based on the 21-dimensional Kalman filter and the radar measurement error model. The 21-dimensional Kalman filter is updated in time to obtain the time-updated 21-dimensional Kalman filter. In the inertial navigation / satellite navigation / radar integrated navigation or inertial / radar integrated navigation state, if the radar data is valid, the radar measurement update is triggered based on the radar measurement matrix to perform the radar measurement update of the time-updated 21-dimensional Kalman filter. In the inertial / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, the satellite navigation measurement update is triggered based on the satellite navigation measurement matrix to perform the satellite navigation measurement update of the time-updated 21-dimensional Kalman filter. The attitude, velocity, and position of the strapdown inertial navigation system are corrected by feedback based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates.

[0005] Optionally, a radar measurement error model, including the radar reference misalignment angle and fixed measurement error, is constructed under the integrated navigation architecture, including: The reference error matrix is ​​used to describe the non-coincidence of the three axes between the actual radar coordinate system and the mathematically established radar coordinate system. The reference error is linearized using a first-order Taylor expansion. Based on the radar reference misalignment angle, the radar reference error matrix is ​​obtained, and the theoretical velocity and position in the actual radar coordinate system are obtained based on the reference error matrix. Based on the transformation relationship between the radar rectangular coordinate system and the polar coordinate system, the above theoretical velocity and position are converted into theoretical values ​​of azimuth angle, elevation angle and radial velocity. Based on the theoretical values ​​of azimuth, elevation, and radial velocity, a radar measurement error model is established by superimposing fixed measurement errors such as azimuth zero-position error, elevation zero-position error, and radial distance scale coefficient error, and combining them with random noise terms.

[0006] Optionally, the error equations of the strapdown inertial navigation system include velocity error equations, position error equations, and attitude error equations; Using a first-order small-perturbation linearization method, the error equations for the strapdown inertial navigation system in the radar coordinate system are constructed, including: Based on the first-order small perturbation linearization method, the attitude differential equation is taken as a small perturbation derivative to obtain the attitude error equation containing the inertial navigation misalignment angle. Taking the small perturbation derivative of the velocity differential equation and obtaining the velocity error equation based on the spherical central gravitational field model; Based on the position differential equation in the radar system, the position error equation is obtained.

[0007] Optionally, the state variables of the state equation are extended to 21 dimensions, resulting in a 21-dimensional state vector, including: The state variables of the state equation are defined as a 21-dimensional column vector containing inertial navigation attitude error, inertial navigation velocity error, inertial navigation position error, gyroscope bias, accelerometer bias, radar fixed measurement error, and radar reference misalignment angle.

[0008] Optionally, the measurement equations of the Kalman filter are determined based on the 21-dimensional state vector and the radar measurement error model, including: Radar observations are constructed based on the differences between the azimuth, elevation, radial distance, and radial velocity output by the radar and the azimuth, elevation, radial distance, and radial velocity obtained by converting the output of the inertial element. The radar measurement matrix is ​​obtained by calculating the partial derivatives of the radar observations with respect to the 21-dimensional state vector. When the satellite navigation signal is valid, the velocity and position output by GNSS are converted to the radar coordinate system to obtain satellite navigation observations; Determine the satellite navigation measurement matrix based on satellite navigation observations; The measurement equations for the Kalman filter are determined based on the radar measurement matrix and the satellite navigation measurement matrix.

[0009] Optionally, the attitude, velocity, and position of the strapdown inertial navigation system are corrected based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates, including: The attitude angle error obtained from radar measurement updates and satellite navigation measurement updates is converted into quaternion form and multiplied with the attitude quaternion calculated by inertial navigation to obtain the corrected attitude quaternion. The corrected velocity is obtained by subtracting the velocity errors obtained from radar measurement updates and satellite navigation measurement updates from the velocity calculated by inertial navigation. The position error obtained from radar measurement updates and satellite navigation measurement updates is subtracted from the position calculated by inertial navigation to obtain the corrected position.

[0010] Optionally, the expression for radar measurement update is:

[0011] in, It is the measurement matrix updated by radar measurement. It is a radar measurement vector. It is the filter gain matrix updated by radar measurements. It is a one-step prediction error variance matrix. It is the estimated error variance matrix after radar measurement update. It is the measurement noise variance matrix updated by radar measurements. It is the updated state vector after radar measurement.

[0012] Optionally, the expression for updating satellite navigation measurements is:

[0013] in, It is the measurement matrix updated by satellite navigation measurement. It is a satellite guidance measurement vector. It is the filter gain matrix updated by satellite navigation measurement. It is the estimated error variance matrix obtained after the radar measurement update under the three combined states. It is the estimated error variance matrix after the satellite navigation measurement update. It is the measurement noise variance matrix updated by satellite navigation measurements. It is the final state vector obtained by updating the satellite navigation under the three combined states.

[0014] To achieve the above objectives, a second aspect of this application also provides a satellite-aided radar reference frame inertial navigation / radar tightly coupled integrated navigation device, comprising: The coordinate system establishment module is used to establish a combined navigation architecture with the radar coordinate system as the navigation reference system; The measurement matrix determination module is used to construct a radar measurement error model containing the radar reference misalignment angle and fixed measurement errors under the integrated navigation architecture. It utilizes a first-order small-perturbation linearization method to construct the error equations of the strapdown inertial navigation system in the radar coordinate system. Based on these error equations, a Kalman filter is constructed, and its state variables and state transition matrix are extended to 21 dimensions to obtain a 21-dimensional Kalman filter. Finally, based on the 21-dimensional Kalman filter and the radar measurement error model, the radar measurement matrix and satellite navigation measurement matrix are obtained. The filter update module is used to update the 21-dimensional Kalman filter in time to obtain the time-updated 21-dimensional Kalman filter. In the inertial navigation / satellite navigation / radar integrated navigation or inertial / radar integrated navigation state, if the radar data is valid, the time-updated 21-dimensional Kalman filter is triggered based on the radar measurement matrix to perform radar measurement update. In the inertial / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, the satellite navigation measurement is triggered based on the satellite navigation measurement matrix to perform satellite navigation measurement update. The feedback correction module is used to perform feedback correction on the attitude, velocity and position of the strapdown inertial navigation system based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates.

[0015] To achieve the above objectives, a third aspect of this application also provides an electronic device, including at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program executable by the at least one processor, and when the computer program is executed by the at least one processor, causes the at least one processor to perform the satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described above.

[0016] This application discloses a satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method, apparatus, and device. The method includes: establishing an integrated navigation architecture with the radar coordinate system as the navigation reference frame; constructing a radar measurement error model including the radar reference misalignment angle and fixed measurement error under the integrated navigation architecture; constructing the error equation of the strapdown inertial navigation system in the radar coordinate system using a first-order small perturbation linearization method; constructing a Kalman filter based on the error equation of the strapdown inertial navigation system, and extending the state variables and state transition matrix of the Kalman filter to 21 dimensions to obtain a 21-dimensional Kalman filter; based on the 2... A 1D Kalman filter and a radar measurement error model are used to obtain the radar measurement matrix and satellite navigation measurement matrix. A 21D Kalman filter is then updated over time to obtain a time-updated 21D Kalman filter. In either inertial navigation / satellite navigation / radar integrated navigation or inertial / radar integrated navigation, if the radar data is valid, the time-updated 21D Kalman filter is triggered based on the radar measurement matrix to update radar measurements. In the same inertial / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, the time-updated 21D Kalman filter is triggered based on the satellite navigation measurement matrix to update satellite navigation measurements. This application designs an online estimation method for radar reference error and fixed measurement error, directly establishing the navigation coordinate system under the radar coordinate system. This enables online error estimation of the inertial / radar integrated navigation system under satellite navigation assistance, which can be used for online radar error calibration, improving integrated navigation accuracy, and also for error analysis in navigation post-processing. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of a radar rectangular coordinate system provided in an embodiment of this application; Figure 2 This is a schematic diagram of the radar polar coordinate system provided in an embodiment of this application; Figure 3 This is a flowchart illustrating the implementation of the integrated navigation system provided in this application. Figure 4 This is a flowchart illustrating the method provided in an embodiment of this application.

[0018] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0019] It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit this application.

[0020] Explanation of key terms: Target data: refers to radar polar coordinate measurement data collected by the radar receiver on board the aircraft, as well as satellite navigation measurement data from the GNSS satellite navigation system.

[0021] Edge device / server / terminal: This application corresponds to the airborne navigation processor of the aircraft or the ground navigation terminal.

[0022] Business object / control object: This application corresponds to the aircraft attitude control loop, trajectory planning module and navigation parameter output interface.

[0023] Traditional aircraft navigation systems often employ loosely or tightly coupled SINS / GNSS architectures, relying on satellite navigation signals to provide absolute position and velocity references. In complex electromagnetic interference or denial-of-service environments, these signals are susceptible to suppression or deception, leading to navigation system failure. Traditional inertial navigation / radar integrated navigation typically projects radar measurements directly onto the geographic or vehicle coordinate system, failing to incorporate radar installation reference errors, mechanical assembly zero-position errors, and ranging scale errors into state estimation. The misalignment angle and position deviation between the radar coordinate system and the actual physical coordinate system amplify with distance, resulting in systematic biases in the measurement residuals. This prevents the filter from converging to the true error, ultimately causing decreased navigation accuracy and long-term drift. This application addresses these technical shortcomings by reconstructing the navigation reference system to the radar coordinate system and extending the radar's inherent error parameters to the Kalman filter state vector, combined with satellite navigation assistance for online calibration. To meet the high-precision navigation requirements of aircraft, this paper establishes the radar coordinate system as the navigation coordinate system. In the case of satellite navigation assistance in the early stage of the trajectory, to meet the requirements of high-precision navigation with tight coupling of inertial and radar, this invention proposes a satellite-assisted radar reference system inertial navigation / radar tight coupling combined navigation method.

[0024] refer to Figure 3 and Figure 4 The first embodiment of this application provides a satellite-aided radar reference system inertial navigation / radar tightly coupled integrated navigation method to solve the technical problems of uncompensated radar coordinate system reference error and decreased navigation accuracy in complex environments mentioned in the background art. This method can be executed by a processor, which can be set in the airborne navigation terminal of the aircraft. The execution process of this method is as follows: Step S101: Establish a combined navigation architecture with the radar coordinate system as the navigation reference system.

[0025] In one embodiment of this application, the navigation reference system is directly established on the radar coordinate system. By utilizing the transformation relationships between the geocentric coordinate system, the carrier coordinate system, the geocentric inertial coordinate system, and the radar measurement coordinate system, a rotation matrix from the radar rectangular coordinate system to the carrier coordinate system and a transformation matrix from the radar rectangular coordinate system to the geocentric coordinate system are constructed, providing a unified mathematical basis for the subsequent derivation of the error equation.

[0026] Specifically, the geocentric-fixed coordinate system is fixedly connected to the Earth, with its origin at the Earth's center. The axis lies in the equatorial plane and points towards the prime meridian. The axis is the Earth's axis of rotation and points towards the North Pole. The axis lies in the equatorial plane and is parallel to it. axis, The axes form a right-handed rectangular coordinate system.

[0027] Origin of the carrier coordinate system As the center of mass of the carrier, The axis points directly in front of the carrier along its longitudinal axis. The axis lies in the plane of the carrier's principal axis of symmetry and points upwards from the projectile. shaft and axis, The axes form a right-handed rectangular coordinate system and point to the right of the carrier.

[0028] A coordinate system that remains stationary or in uniform rectilinear motion in space is called an inertial coordinate system. The origin of the geocentric inertial coordinate system is the Earth's center of mass. x The axis lies in the equatorial plane and points towards the vernal equinox. z The axis points along the Earth's rotation axis towards the common poles. y The axis satisfies the right-hand rule.

[0029] Radar measurement coordinate system, abbreviated as r There are two types of radar coordinate systems: the ground-based radar rectangular coordinate system and the ground-based radar polar coordinate system. The radar rectangular coordinate system takes the radar station as its origin. The axis points in the local horizontal plane. The axis is perpendicular to the horizontal plane and points upwards. shaft and axis, The axes form a right-handed rectangular coordinate system. The geographical latitude of the radar station. ,longitude ,high and the azimuth of the land The relationship between the radar rectangular coordinate system and the Earth was determined, such as... Figure 1 As shown.

[0030] The basic principle of radar measurement and positioning is to determine the target's position by combining echo ranging with the target's azimuth and elevation information. Therefore, its primary measurement is the target's azimuth. A Target elevation angle E radial distance R and radial velocity V These parameters are in the radar polar coordinate system. The radar polar coordinate system is as follows: Figure 2 As shown: The rotation matrix from the launch coordinate system (r-frame) to the carrier coordinate system (b-frame) is: The attitude angle of the aircraft in the radar reference frame is determined by the pitch angle. Yaw angle and roll angle The three Euler angles describe the pitch angles, first by rotating about the z-axis. Then rotate around the y-axis at the y-axis angle. Finally, rotate around the x-axis by the roll angle. By following the 3-2-1 rotation sequence, the rotation matrix can be obtained. As shown in the following formula.

[0031] (1) The rotation matrix from the carrier coordinate system (b-frame) to the radar rectangular coordinate system (r-frame) is: (2) The transformation matrix between the radar rectangular coordinate system and the Earth-centered Earth-fixed coordinate system is: The data was obtained through three rotations and involved the initial longitude of the ground-based radar. Geographical latitude and heading The attitude matrix rotated from the geocentric coordinate system to the radar rectangular coordinate system is shown in equation (3).

[0032] (3) Step S102: Construct a radar measurement error model containing the radar reference misalignment angle and fixed measurement error under the integrated navigation architecture; construct the strapdown inertial navigation system error equation in the radar coordinate system using the first-order small perturbation linearization method; construct the state equation of the Kalman filter based on the strapdown inertial navigation system error equation, extend the state variables of the state equation to 21 dimensions to obtain a 21-dimensional state vector; determine the measurement equation of the Kalman filter based on the 21-dimensional state vector and the radar measurement error model, and determine the 21-dimensional Kalman filter based on the measurement equation and the state equation.

[0033] Furthermore, the process of constructing a radar measurement error model including radar reference misalignment angle and fixed measurement error under the integrated navigation architecture may be as follows: Describe the three-axis non-coincidence relationship between the actual radar coordinate system and the mathematically established radar coordinate system using the reference error matrix; perform linearization processing on the reference error using the first-order Taylor expansion, obtain the radar reference error matrix based on the radar reference misalignment angle, and obtain the theoretical velocity and position in the actual radar coordinate system based on the reference error matrix; based on the conversion relationship between the radar rectangular coordinate system and the polar coordinate system, convert the above theoretical velocity and position into the theoretical values of azimuth angle, elevation angle, and radial velocity; superimpose fixed measurement errors such as azimuth zero error, elevation zero error, and radial distance scale factor error on the above theoretical values of azimuth angle, elevation angle, and radial velocity, and combine with the random noise term to establish a radar measurement error model.

[0034] Furthermore, the strapdown inertial navigation system error equations include velocity error equations, position error equations, and attitude error equations; using the first-order small perturbation linearization method, the process of constructing the strapdown inertial navigation system error equations in the radar coordinate system may be as follows: Based on the first-order small perturbation linearization method, take the small perturbation differential of the attitude differential equation to obtain the attitude error equation including the inertial navigation misalignment angle; take the small perturbation differential of the velocity differential equation and obtain the velocity error equation according to the spherical central gravitational field model; based on the position differential equation in the radar system, obtain the position error equation.

[0035] Furthermore, expand the state variables of the state equation to 21 dimensions, and the 21-dimensional state vector includes: Define the state variables of the state equation as a 21-dimensional column vector including inertial navigation attitude error, inertial navigation velocity error, inertial navigation position error, gyroscope zero bias, accelerometer zero bias, radar fixed measurement error, and radar reference misalignment angle.

[0036] Furthermore, determining the measurement equation of the Kalman filter based on the 21-dimensional state vector and the radar measurement error model includes: Construct a radar measurement quantity based on the differences between the azimuth angle, elevation angle, radial distance, and radial velocity output by the radar and the azimuth angle, elevation angle, radial distance, and radial velocity obtained by converting the output of the inertial components; calculate the partial derivatives of the radar measurement quantity with respect to the 21-dimensional state vector to obtain the radar measurement matrix; when the satellite navigation signal is valid, convert the velocity and position output by GNSS to the radar coordinate system to obtain the satellite navigation measurement quantity; determine the satellite navigation measurement matrix according to the satellite navigation measurement quantity; determine the measurement equation of the Kalman filter based on the radar measurement matrix and the satellite navigation measurement matrix.

[0037] It should be noted that in this application, satellite navigation is abbreviated as GNSS, and it will not be elaborated further hereinafter.

[0038] Specifically, in the radar rectangular coordinate system, the measurement quantity of the radar is the position of the target and speed The above two parameters can be represented as three-dimensional vectors, that is: (4) However, the actual measured coordinates of a radar station do not always perfectly coincide with the mathematically established radar coordinate system. This misalignment of the three axes can be explained by a matrix. It means that among them This refers to a mathematically established radar system. To represent the actual radar system, the mathematical representation of the radar system for any vector is as follows: In actual radar systems, The two satisfy the following relationship.

[0039] (5) Although there are slight non-coincidences among the three axes, the misalignment angle can be considered as a small angle, which can be obtained using a first-order Taylor expansion. It is expressed as follows: (6) in, This represents the radar's reference misalignment angle error.

[0040] Substituting equation (6) into equations (4) and (5), we can obtain the theoretical position in the actual radar coordinate system. and speed As shown in equations (7) and (8): (7) (8) Among them are and .

[0041] Based on the relevant definitions of radar measurement coordinate systems, the transformation relationship between radar polar coordinates and radar rectangular coordinates can be derived. Specifically, the transformation relationship between the position vector in the radar polar coordinates and the position vector in the rectangular coordinates is as follows: (9) Conversely, the transformation relationship between the position vector in the Cartesian coordinate system and the position vector in the radar polar coordinate system is as follows: (10) In addition, the radial velocity in the radar polar coordinate system Satisfy the following formula: (11) Referring to the calculation methods of equations (10) and (11), the results obtained from equations (7) to (8) are... and Substituting the values, we can obtain the azimuth angle measured under the actual coordinates of the radar station. Elevation angle radial distance and radial velocity The theoretical value. The theoretical value of distance is: (12) Expanding and combining the results using the binomial theorem, we obtain the following expression: (13) Considering that the formula has only first-order precision, we can ignore second-order minor quantities in the above formula: (14) Similarly, the theoretical values ​​of elevation angles in a real radar system are: (15) The theoretical value of the azimuth angle in the actual radar coordinate system is: (16) The theoretical value of radial velocity in the actual radar coordinate system is: (17) Further consider the actual radar measurement errors, namely the zero-position errors of azimuth and elevation angles and the scale errors of radial distance. Measurement errors include mechanical errors, angle sensor readout errors, random noise, and ranging scale coefficient errors. Mechanical errors refer to the fixed angle measurement errors caused by assembly gaps of mechanical moving parts, etc. Angle sensors generally have a fixed resolution, and the readout results are integer scales, resulting in a certain degree of accuracy loss. The algorithm for online estimation and compensation of the above errors is shown in the following formula (18).

[0042] (18) in, It is the azimuth zero position error. It is the zero-position error of elevation and elevation angles. It's a distance scale error, and , and It is the random noise corresponding to the measurements of the three quantities: azimuth, elevation, and radial distance.

[0043] Substituting equations (14) to (16) into equation (18), we can obtain the relationship between the actual radar measurement value and the theoretical value as follows: (19) in, These are the measured values ​​in the actual radar measurement coordinate system.

[0044] In this embodiment, the mechanical arrangement of the strapdown inertial navigation algorithm in the radar measurement coordinate system is as follows: (20) Based on the differential equation in equation (20), the error equation of the radar system strapdown inertial navigation system can be derived using the first-order small perturbation linearization method.

[0045] The attitude differential equation in the radar measurement system is: (twenty one) in, and Angular velocity vectors and The antisymmetric matrix is ​​represented by the two angular velocities, which respectively characterize the rotation of the carrier system and the radar system relative to the geocentric inertial frame.

[0046] Considering radar measurement errors, the calculated rate of change of the transformation matrix is: (twenty two) The calculated transformation matrix can be written as: (twenty three) make: (twenty four) achievable (25) in, for An antisymmetric matrix, that is, satisfying the condition: (26) And in the above formula It is the error angle between the radar measurement coordinate system and the inertial navigation calculation result, i.e., the inertial navigation misalignment angle.

[0047] Differentiating both sides of equations (24) and (25) yields: (27) Furthermore, taking the small perturbation differential of the attitude differential equation (21) yields: (28) Substituting formula (24) into formula (28) yields: (29) Comparing formulas (27) and (29), we can obtain: (30) Written in vector form: (31) Expanding the right side of the above equation, we get: (32) Substituting formula (32) into formula (31) yields: (33) Because the radar system is fixed to the Earth, therefore It is a fixed value, that is, it satisfies And assume Random constant drift of the equivalent gyroscope White noise measurement of gyroscope angular velocity Composition, that is .

[0048] The final attitude error equation in the launch frame is: (34) The velocity differential equation in the radar measurement system is: (35) Linearizing the above equation, we have: (36) in, ,and yes An antisymmetric matrix that satisfies: (37) Furthermore, simplifying the gravitational field model to a spherical, centered gravitational field, we obtain: (38) Substituting equation (38) into equation (36), and assuming the accelerometer measurement error... Zero bias from the random constant of the equivalent accelerometer White noise compared to accelerometer force measurement Composition, that is Then we have: (39) The position error equation under radar measurement system is: (40) Therefore, the position error equation under the radar measurement system can be obtained: (41) Specifically, in this application, the navigation uses the Kalman filter method, and the Kalman filter is established as follows: This application defines the state variables of the Discrete Kalman Filter as follows: (k=1,2,...), state transition matrix Observation and measurement matrix The integrated navigation system model is as follows: (42) Based on the navigation error equations derived from equations (34), (39), and (41), the 15-dimensional conventional state equations for the radar system can be obtained, which can be written in matrix form as follows: (43) remember (44) To improve the accuracy of integrated navigation, this paper extends the state variables and state transition matrix to 21 dimensions. The state variables include the inertial navigation misalignment angle. Inertial navigation velocity error Inertial navigation position error Inertial gyroscope zero bias Inertial navigation system zero bias Radar measurement error Radar reference error Let the state variable be denoted as Its expression is as follows: (45) The corresponding state transition matrix is ​​then constructed as follows: (46) in, The result of the calculation of equation (44) at time k.

[0049] The measurement equation is the second equation in equation (42), and the radar observation is selected as: (47) in, The azimuth, elevation, radial range, and radial velocity output by the radar. The output of the inertial element includes the azimuth angle, elevation angle, radial distance, and radial velocity.

[0050] The radar measurement matrix is ​​established as follows: (48) To calculate the measurement matrix, the following partial derivatives need to be obtained: , , and (49) The partial derivatives above can be expanded into matrix form as follows: (50) (51) (52) (53) in, The relevant items are expanded as follows: right The partial derivative is: (54) right The partial derivative is: (55) right The partial derivative is: (56) right The partial derivative is: (57) right The partial derivative is: (58) right The partial derivative is: (59) right The partial derivative is: (60) The relevant items are expanded as follows: right The partial derivative is: (61) right The partial derivative is: (62) right The partial derivative is: (63) right A , E , R The partial derivative is: (64) The relevant items are expanded as follows: right The partial derivative is: (65) right The partial derivative is: (66) right The partial derivative is: (67) right A , E , R The partial derivative is in: (68) Find the radial velocity partial derivatives , right The partial derivative is: (69) similarity right and The partial derivative is: (70) (71) right Taking the partial derivative, we get: (72) right and The partial derivative is: (73) When satellite navigation is used, combined navigation can also be performed using the velocity and position information output by GNSS. Since the velocity and position output by GNSS are in the ECEF system, they need to be converted to the navigation coordinate system first. The relevant formulas are shown below: (74) in, and It is navigation information output by GNSS. It is the location of the radar station under the ECEF system. This is the transformation matrix. Therefore, the calculation formula for the observations in the satellite-guided integrated navigation measurement equations can be obtained as follows: (75) in, It is satellite navigation observation. and It is the speed and position output by the satellite navigation system. and It is the velocity and position calculated by the inertial element.

[0051] The measurement matrix corresponding to the measurement equation is as follows: (76) When both satellite navigation and radar are available, a combined navigation mode of inertial navigation / satellite navigation / radar is used. The radar observations are used to perform online estimation based on the radar measurement matrix using a 21-dimensional Kalman filter to obtain the first measurement update. Then, the satellite navigation performs a second measurement update based on the first measurement update. When the satellite navigation signal fails, navigation is performed using a combination of inertial navigation and radar. Only the radar is used to update the measurement using a 21-dimensional Kalman filter based on the radar measurement matrix. It should be noted that radar updates are always performed, and if satellite navigation is available, it is used to update again.

[0052] Step S103: Update the 21-dimensional Kalman filter in time to obtain the time-updated 21-dimensional Kalman filter. In the inertial navigation / satellite navigation / radar integrated navigation or inertial / radar integrated navigation state, if the radar data is valid, trigger the time-updated 21-dimensional Kalman filter to perform radar measurement update based on the radar measurement matrix. In the inertial / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, trigger the time-updated 21-dimensional Kalman filter to perform satellite navigation measurement update based on the satellite navigation measurement matrix.

[0053] Upon initial entry into integrated navigation, assuming both satellite navigation and radar are available, a three-mode combination of inertial navigation / satellite navigation / radar is employed for integrated navigation, with online estimation of the last six dimensions of the state variables. When satellite navigation fails, the system switches to inertial navigation / radar integrated navigation mode. In the filter, the last six dimensions of the state equation are automatically compensated for the observations, achieving high-precision inertial navigation / radar integrated navigation.

[0054] The filter is updated periodically, and the calculation formula is as follows: (77) In inertial navigation / satellite navigation / radar integrated navigation or inertial / radar integrated navigation mode, when radar data effectively triggers radar measurement updates, the filter measurement update formula is as follows: (78) in, It is the measurement matrix updated by radar measurement. It is a radar measurement vector. It is the filter gain matrix updated by radar measurements. It is a one-step prediction error variance matrix. It is the estimated error variance matrix after radar measurement update. It is the measurement noise variance matrix updated by radar measurements. It is the updated state vector after radar measurement.

[0055] In inertial / satellite navigation / radar integrated navigation mode, valid satellite navigation data triggers measurement updates. The filter measurement update formula is as follows: (79) in, It is the measurement matrix updated by satellite navigation measurement. It is a satellite guidance measurement vector. It is the filter gain matrix updated by satellite navigation measurement. It is the estimated error variance matrix obtained after the radar measurement update under the three combined states. It is the estimated error variance matrix after the satellite navigation measurement update. It is the measurement noise variance matrix updated by satellite navigation measurements. It is the final state vector obtained by updating the satellite navigation under the three combined states.

[0056] Step S104: Feedback correction is performed on the attitude, velocity, and position of the strapdown inertial navigation system based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates.

[0057] Furthermore, the process of feedback correction of the attitude, velocity, and position of the strapdown inertial navigation system based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation updates can include the following: converting the attitude angle error obtained from radar measurement updates and satellite navigation updates into quaternion form, and multiplying it with the attitude quaternion calculated by the inertial navigation system to obtain the corrected attitude quaternion; subtracting the velocity error obtained from radar measurement updates and satellite navigation updates from the velocity calculated by the inertial navigation system to obtain the corrected velocity; and subtracting the position error obtained from radar measurement updates and satellite navigation updates from the position calculated by the inertial navigation system to obtain the corrected position.

[0058] The expression for posture correction is as follows: (80) In formula (80), The attitude quaternion in the radar frame is calculated using an inertial navigation algorithm. The corrected attitude quaternion. The attitude angle error obtained by filtering The corresponding quaternion.

[0059] The expression for velocity correction is as follows: (81) In formula (81), The velocity in the launch system is calculated by the navigation algorithm. The corrected speed, This represents the speed error obtained through filtering.

[0060] The expression for position correction is as follows: (81) In formula (82), The velocity in the launch system is calculated by the navigation algorithm. The corrected speed, This represents the speed error obtained through filtering.

[0061] To address the aforementioned technical problems, the second embodiment of this application provides a satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation device, the device comprising: The system comprises the following modules: a coordinate system establishment module for establishing a combined navigation architecture with the radar coordinate system as the navigation reference system; a measurement matrix determination module for constructing a radar measurement error model including the radar reference misalignment angle and fixed measurement errors within the combined navigation architecture; a first-order small-perturbation linearization method for constructing the strapdown inertial navigation system error equations in the radar coordinate system; a Kalman filter state equation for constructing the Kalman filter state equations based on the strapdown inertial navigation system error equations, extending the state variables of the state equations to 21 dimensions to obtain a 21-dimensional state vector; a measurement equation for the Kalman filter for determining the measurement matrix based on the 21-dimensional state vector and the radar measurement error model; and a filter update module for constructing a radar measurement error model including the radar reference misalignment angle and fixed measurement errors within the combined navigation architecture. A radar measurement error model is constructed. A first-order small-perturbation linearization method is used to construct a 21-dimensional Kalman filter for the strapdown inertial navigation system in the radar coordinate system. Time updates are performed to obtain the updated 21-dimensional state vector. In either the inertial navigation / satellite navigation / radar integrated navigation state or the inertial navigation / radar integrated navigation state, if the radar data is valid, the 21-dimensional state vector is updated based on the radar data after triggering the time update. In the inertial navigation / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, the 21-dimensional state vector is updated based on the radar measurement update. A feedback correction module is used to perform feedback correction on the attitude, velocity, and position of the strapdown inertial navigation system based on the 21-dimensional state vector obtained from the radar measurement update and the satellite navigation measurement update.

[0062] To address the aforementioned technical problems, the third embodiment of this application provides an electronic device, characterized in that the process may include a processor and a memory, the memory storing a computer program, and the processor executing the computer program to implement the satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in the method embodiment.

[0063] It is understood that this embodiment is the program product claim corresponding to the method claim, and therefore this embodiment has all the technical effects of the method claim, which will not be repeated here.

[0064] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method, characterized in that, include: Establish a combined navigation architecture with the radar coordinate system as the navigation reference system; A radar measurement error model, including the radar reference misalignment angle and fixed measurement error, is constructed under the integrated navigation architecture. Using a first-order small-perturbation linearization method, the error equation of the strapdown inertial navigation system in the radar coordinate system is constructed. Based on the error equation of the strapdown inertial navigation system, the state equation of the Kalman filter is constructed, and the state variables of the state equation are extended to 21 dimensions to obtain a 21-dimensional state vector. Based on the 21-dimensional state vector and the radar measurement error model, the measurement equation of the Kalman filter is determined, and based on the measurement equation and the state equation, the 21-dimensional Kalman filter is determined. The 21-dimensional Kalman filter is updated over time to obtain a time-updated 21-dimensional state vector. In the inertial navigation / satellite navigation / radar integrated navigation state or the inertial navigation / radar integrated navigation state, if the radar data is valid, the 21-dimensional state vector is updated by radar measurement based on the radar data after triggering the time update. In the inertial navigation / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, the 21-dimensional state vector is updated by satellite navigation measurement based on the radar measurement update. The attitude, velocity, and position of the strapdown inertial navigation system are corrected by feedback based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates.

2. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The construction of a radar measurement error model, which includes the radar reference misalignment angle and fixed measurement error, under the integrated navigation architecture includes: The reference error matrix is ​​used to describe the non-coincidence of the three axes between the actual radar coordinate system and the mathematically established radar coordinate system. The reference error is linearized using a first-order Taylor expansion. Based on the radar reference misalignment angle, the radar reference error matrix is ​​obtained, and the theoretical velocity and position in the actual radar coordinate system are obtained based on the reference error matrix. Based on the transformation relationship between the radar rectangular coordinate system and the polar coordinate system, the above theoretical velocity and position are converted into theoretical values ​​of azimuth angle, elevation angle and radial velocity. Based on the theoretical values ​​of azimuth, elevation, and radial velocity, a radar measurement error model is established by superimposing fixed measurement errors such as azimuth zero-position error, elevation zero-position error, and radial distance scale coefficient error, and combining them with random noise terms.

3. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The error equations of the strapdown inertial navigation system include velocity error equations, position error equations, and attitude error equations; The method of using first-order small perturbation linearization to construct the error equations of the strapdown inertial navigation system in the radar coordinate system includes: Based on the first-order small perturbation linearization method, the attitude differential equation is taken as a small perturbation derivative to obtain the attitude error equation containing the inertial navigation misalignment angle. Taking the small perturbation derivative of the velocity differential equation and obtaining the velocity error equation based on the spherical central gravitational field model; Based on the position differential equation in the radar system, the position error equation is obtained.

4. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The process of extending the state variables of the state equation to 21 dimensions to obtain a 21-dimensional state vector includes: The state variables of the state equation are defined as a 21-dimensional column vector containing inertial navigation attitude error, inertial navigation velocity error, inertial navigation position error, gyroscope bias, accelerometer bias, radar fixed measurement error, and radar reference misalignment angle.

5. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The measurement equations for determining the Kalman filter based on the 21-dimensional state vector and radar measurement error model include: Radar observations are constructed based on the differences between the azimuth, elevation, radial distance, and radial velocity output by the radar and the azimuth, elevation, radial distance, and radial velocity obtained by converting the output of the inertial element. The radar measurement matrix is ​​obtained by calculating the partial derivatives of the radar observations with respect to the 21-dimensional state vector. When the satellite navigation signal is valid, the velocity and position output by GNSS are converted to the radar coordinate system to obtain satellite navigation observations; Determine the satellite navigation measurement matrix based on satellite navigation observations; The measurement equations for the Kalman filter are determined based on the radar measurement matrix and the satellite navigation measurement matrix.

6. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The step of feeding back and correcting the attitude, velocity, and position of the strapdown inertial navigation system using the 21-dimensional state vector obtained from radar and satellite navigation updates includes: The attitude angle error obtained from radar measurement updates and satellite navigation measurement updates is converted into quaternion form and multiplied with the attitude quaternion calculated by inertial navigation to obtain the corrected attitude quaternion. The corrected velocity is obtained by subtracting the velocity errors obtained from radar measurement updates and satellite navigation measurement updates from the velocity calculated by inertial navigation. The position error obtained from radar measurement updates and satellite navigation measurement updates is subtracted from the position calculated by inertial navigation to obtain the corrected position.

7. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The expression for updating the radar measurements is: in, It is the measurement matrix updated by radar measurement. It is a radar measurement vector. It is the filter gain matrix updated by radar measurements. It is a one-step prediction error variance matrix. It is the estimated error variance matrix after radar measurement update. It is the measurement noise variance matrix updated by radar measurements. It is the updated state vector after radar measurement.

8. The satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in claim 1, characterized in that, The expression for updating the satellite navigation measurement is: in, It is the measurement matrix updated by satellite navigation measurement. It is a satellite guidance measurement vector. It is the filter gain matrix updated by satellite navigation measurement. It is the estimated error variance matrix obtained after the radar measurement update under the three combined states. It is the estimated error variance matrix after the satellite navigation measurement update. It is the measurement noise variance matrix updated by satellite navigation measurements. It is the final state vector obtained by updating the satellite navigation under the three combined states.

9. A satellite-aided radar reference frame inertial navigation / radar tightly coupled integrated navigation device, characterized in that, include: The coordinate system establishment module is used to establish a combined navigation architecture with the radar coordinate system as the navigation reference system; The measurement matrix determination module is used to construct a radar measurement error model including the radar reference misalignment angle and fixed measurement error under the integrated navigation architecture; to construct the error equation of the strapdown inertial navigation system in the radar coordinate system using the first-order small perturbation linearization method; to construct the state equation of the Kalman filter based on the error equation of the strapdown inertial navigation system, and to extend the state variables of the state equation to 21 dimensions to obtain a 21-dimensional state vector; to determine the measurement equation of the Kalman filter based on the 21-dimensional state vector and the radar measurement error model, and to determine the 21-dimensional Kalman filter based on the measurement equation and the state equation. The filtering update module is used to construct a radar measurement error model including the radar reference misalignment angle and fixed measurement error under the integrated navigation architecture; using a first-order small perturbation linearization method, it constructs a strapdown inertial navigation system in the radar coordinate system and updates the 21-dimensional Kalman filter in time to obtain the time-updated 21-dimensional state vector; in the inertial navigation / satellite navigation / radar integrated navigation state or the inertial navigation / radar integrated navigation state, if the radar data is valid, it updates the 21-dimensional state vector with radar measurements after triggering the time update based on the radar data; in the inertial navigation / satellite navigation / radar integrated navigation state, if the satellite navigation data is valid, it updates the 21-dimensional state vector with satellite navigation measurements based on the radar measurement update. The feedback correction module is used to perform feedback correction on the attitude, velocity and position of the strapdown inertial navigation system based on the 21-dimensional state vector obtained from radar measurement updates and satellite navigation measurement updates.

10. An electronic device, characterized in that, It includes a processor and a memory, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the satellite-guided radar reference frame inertial navigation / radar tightly coupled integrated navigation method as described in any one of claims 1 to 7.

Images